From 6fd9082544df2065ec873c8fb8c4b83816604d0d Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:14:42 +0100 Subject: [PATCH 001/100] adds initial code and implementation for xform class --- .../isaaclab/isaaclab/sim/utils/__init__.py | 1 + source/isaaclab/isaaclab/sim/utils/xform.py | 183 +++++ source/isaaclab/test/sim/test_utils_xform.py | 705 ++++++++++++++++++ 3 files changed, 889 insertions(+) create mode 100644 source/isaaclab/isaaclab/sim/utils/xform.py create mode 100644 source/isaaclab/test/sim/test_utils_xform.py diff --git a/source/isaaclab/isaaclab/sim/utils/__init__.py b/source/isaaclab/isaaclab/sim/utils/__init__.py index b6ccbae7d5b..da7da10baf9 100644 --- a/source/isaaclab/isaaclab/sim/utils/__init__.py +++ b/source/isaaclab/isaaclab/sim/utils/__init__.py @@ -10,3 +10,4 @@ from .queries import * # noqa: F401, F403 from .semantics import * # noqa: F401, F403 from .stage import * # noqa: F401, F403 +from .xform import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sim/utils/xform.py b/source/isaaclab/isaaclab/sim/utils/xform.py new file mode 100644 index 00000000000..16e1455917b --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/xform.py @@ -0,0 +1,183 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for working with USD transform (xform) operations. + +This module provides utilities for manipulating USD transform operations (xform ops) on prims. +Transform operations in USD define how geometry is positioned, oriented, and scaled in 3D space. + +The utilities in this module help standardize transform stacks, clear operations, and manipulate +transforms in a consistent way across different USD assets. +""" + +from __future__ import annotations + +__all__ = ["standardize_xform_ops"] + +import logging +from pxr import Gf, Usd, UsdGeom + +# import logger +logger = logging.getLogger(__name__) + +_INVALID_XFORM_OPS = [ + "xformOp:rotateX", + "xformOp:rotateXZY", + "xformOp:rotateY", + "xformOp:rotateYXZ", + "xformOp:rotateYZX", + "xformOp:rotateZ", + "xformOp:rotateZYX", + "xformOp:rotateZXY", + "xformOp:rotateXYZ", + "xformOp:transform", +] +"""List of invalid xform ops that should be removed.""" + + +def standardize_xform_ops( + prim: Usd.Prim, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + scale: tuple[float, float, float] | None = None, +) -> bool: + """Standardize and normalize the transform operations on a USD prim. + + This function standardizes a prim's transform stack to use the common transform operation + order: translate, orient (quaternion rotation), and scale. It performs the following: + + 1. Captures the current local pose of the prim (relative to parent) + 2. Clears all existing transform operations + 3. Removes deprecated/non-standard transform operations (e.g., rotateXYZ, transform matrix) + 4. Establishes the standard transform stack: [translate, orient, scale] + 5. Handles unit resolution for scale attributes + 6. Restores the original local pose using the new standardized operations + + This is particularly useful when importing assets from different sources that may use + various transform operation conventions, ensuring a consistent and predictable transform + stack across all prims in the scene. + + .. note:: + The standard transform operation order follows USD best practices: + ``xformOp:translate``, ``xformOp:orient``, ``xformOp:scale``. This order is + compatible with most USD tools and workflows. + + .. warning:: + This function modifies the prim's transform stack in place. While it preserves + the local pose by default, any animation or time-sampled transform data will be lost + as only the current (default) time code values are preserved. + + Args: + prim: The USD prim to standardize transform operations for. Must be a valid + prim that supports the Xformable schema. + translation: Optional translation (x, y, z) to set. If None, preserves current + local translation. Defaults to None. + orientation: Optional orientation quaternion (w, x, y, z) to set. If None, preserves + current local orientation. Defaults to None. + scale: Optional scale (x, y, z) to set. If None, preserves current scale or uses + (1.0, 1.0, 1.0) if no scale exists. Defaults to None. + + Returns: + True if the transform operations were standardized successfully, False otherwise. + + Raises: + ValueError: If the prim is not valid or does not support transform operations. + + Example: + >>> import isaaclab.sim as sim_utils + >>> + >>> # Get a prim with non-standard transform operations + >>> prim = stage.GetPrimAtPath("/World/Asset") + >>> # Standardize its transform stack while preserving pose + >>> sim_utils.standardize_xform_ops(prim) + >>> # The prim now uses: translate, orient, scale in that order + >>> + >>> # Or standardize and set new transform values + >>> sim_utils.standardize_xform_ops( + ... prim, + ... translation=(1.0, 2.0, 3.0), + ... orientation=(1.0, 0.0, 0.0, 0.0), + ... scale=(2.0, 2.0, 2.0) + ... ) + """ + # Validate prim + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPath()}' is not valid.") + + # Check if prim is an Xformable + if not prim.IsA(UsdGeom.Xformable): + logger.error(f"Prim at path '{prim.GetPath()}' is not an Xformable.") + return False + + # Create xformable interface + xformable = UsdGeom.Xformable(prim) + # Get current property names + prop_names = prim.GetPropertyNames() + + # Obtain current local transformations + tf = Gf.Transform(xformable.GetLocalTransformation()) + xform_pos = Gf.Vec3d(tf.GetTranslation()) + xform_quat = Gf.Quatd(tf.GetRotation().GetQuat()) + xform_scale = Gf.Vec3d(tf.GetScale()) + + if translation is not None: + xform_pos = Gf.Vec3d(*translation) + if orientation is not None: + xform_quat = Gf.Quatd(*orientation) + + # Handle scale resolution + if scale is not None: + # User provided scale + xform_scale = scale + elif "xformOp:scale" in prop_names: + # Handle unit resolution for scale if present + # This occurs when assets are imported with different unit scales + # Reference: Omniverse Metrics Assembler + if "xformOp:scale:unitsResolve" in prop_names: + units_resolve = prim.GetAttribute("xformOp:scale:unitsResolve").Get() + for i in range(3): + xform_scale[i] = xform_scale[i] * units_resolve[i] + # Convert to tuple + xform_scale = tuple(xform_scale) + else: + # No scale exists, use default uniform scale + xform_scale = Gf.Vec3d(1.0, 1.0, 1.0) + + # Clear the existing transform operation order + has_reset = xformable.GetResetXformStack() + for prop_name in prop_names: + if prop_name in _INVALID_XFORM_OPS: + prim.RemoveProperty(prop_name) + + # Remove unitsResolve attribute if present (already handled in scale resolution above) + if "xformOp:scale:unitsResolve" in prop_names: + prim.RemoveProperty("xformOp:scale:unitsResolve") + + # Set up or retrieve scale operation + xform_op_scale = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) + if not xform_op_scale: + xform_op_scale = xformable.AddXformOp(UsdGeom.XformOp.TypeScale, UsdGeom.XformOp.PrecisionDouble, "") + + # Set up or retrieve translate operation + xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + if not xform_op_translate: + xform_op_translate = xformable.AddXformOp(UsdGeom.XformOp.TypeTranslate, UsdGeom.XformOp.PrecisionDouble, "") + + # Set up or retrieve orient (quaternion rotation) operation + xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + if not xform_op_orient: + xform_op_orient = xformable.AddXformOp(UsdGeom.XformOp.TypeOrient, UsdGeom.XformOp.PrecisionDouble, "") + + # Set the transform operation order: translate -> orient -> scale + # This is the standard USD convention and ensures consistent behavior + xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) + + # Set the transform values using the new standardized transform operations + # Convert tuples to Gf types for USD + xform_op_translate.Set(xform_pos) + xform_op_orient.Set(xform_quat) + xform_op_scale.Set(xform_scale) + + return True diff --git a/source/isaaclab/test/sim/test_utils_xform.py b/source/isaaclab/test/sim/test_utils_xform.py new file mode 100644 index 00000000000..dc21409cb3c --- /dev/null +++ b/source/isaaclab/test/sim/test_utils_xform.py @@ -0,0 +1,705 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import math + +import pytest +from pxr import Gf, Sdf, Usd, UsdGeom + +import isaaclab.sim as sim_utils + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + + +def assert_vec3_close(v1: Gf.Vec3d | Gf.Vec3f, v2: tuple | Gf.Vec3d | Gf.Vec3f, eps: float = 1e-6): + """Assert two 3D vectors are close.""" + if isinstance(v2, tuple): + v2 = Gf.Vec3d(*v2) + for i in range(3): + assert math.isclose(v1[i], v2[i], abs_tol=eps), f"Vector mismatch at index {i}: {v1[i]} != {v2[i]}" + + +def assert_quat_close(q1: Gf.Quatf | Gf.Quatd, q2: Gf.Quatf | Gf.Quatd | tuple, eps: float = 1e-6): + """Assert two quaternions are close, accounting for double-cover (q and -q represent same rotation).""" + if isinstance(q2, tuple): + q2 = Gf.Quatd(*q2) + # Check if quaternions are close (either q1 ≈ q2 or q1 ≈ -q2) + real_match = math.isclose(q1.GetReal(), q2.GetReal(), abs_tol=eps) + imag_match = all(math.isclose(q1.GetImaginary()[i], q2.GetImaginary()[i], abs_tol=eps) for i in range(3)) + + real_match_neg = math.isclose(q1.GetReal(), -q2.GetReal(), abs_tol=eps) + imag_match_neg = all(math.isclose(q1.GetImaginary()[i], -q2.GetImaginary()[i], abs_tol=eps) for i in range(3)) + + assert (real_match and imag_match) or ( + real_match_neg and imag_match_neg + ), f"Quaternion mismatch: {q1} != {q2} (and not equal to negative either)" + + +def get_xform_ops(prim: Usd.Prim) -> list[str]: + """Get the ordered list of xform operation names for a prim.""" + xformable = UsdGeom.Xformable(prim) + return [op.GetOpName() for op in xformable.GetOrderedXformOps()] + + +""" +Tests. +""" + + +def test_standardize_xform_ops_basic(): + """Test basic functionality of standardize_xform_ops on a simple prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a simple xform prim with standard operations + prim = sim_utils.create_prim( + "/World/TestXform", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), # w, x, y, z + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + + # Verify the operation succeeded + assert result is True + assert prim.IsValid() + + # Check that the xform operations are in the correct order + xform_ops = get_xform_ops(prim) + assert xform_ops == [ + "xformOp:translate", + "xformOp:orient", + "xformOp:scale", + ], f"Expected standard xform order, got {xform_ops}" + + # Verify the transform values are preserved (approximately) + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), (1.0, 2.0, 3.0)) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), (1.0, 0.0, 0.0, 0.0)) + assert_vec3_close(prim.GetAttribute("xformOp:scale").Get(), (1.0, 1.0, 1.0)) + + +def test_standardize_xform_ops_with_rotation_xyz(): + """Test standardize_xform_ops removes deprecated rotateXYZ operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim and manually add deprecated rotation operations + prim_path = "/World/TestRotateXYZ" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + # Add deprecated rotateXYZ operation + rotate_xyz_op = xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_xyz_op.Set(Gf.Vec3d(45.0, 30.0, 60.0)) + # Add translate operation + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Verify the deprecated operation exists + assert "xformOp:rotateXYZ" in prim.GetPropertyNames() + + # Get pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved (may have small numeric differences due to rotation conversion) + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-4) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-4) + + # Verify the deprecated operation is removed + assert "xformOp:rotateXYZ" not in prim.GetPropertyNames() + # Verify standard operations exist + assert "xformOp:translate" in prim.GetPropertyNames() + assert "xformOp:orient" in prim.GetPropertyNames() + assert "xformOp:scale" in prim.GetPropertyNames() + # Check the xform operation order + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_transform_matrix(): + """Test standardize_xform_ops removes transform matrix operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with a transform matrix + prim_path = "/World/TestTransformMatrix" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add transform matrix operation + transform_op = xformable.AddTransformOp(UsdGeom.XformOp.PrecisionDouble) + # Create a simple translation matrix + matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(5.0, 10.0, 15.0)) + transform_op.Set(matrix) + + # Verify the transform operation exists + assert "xformOp:transform" in prim.GetPropertyNames() + + # Get pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify the transform operation is removed + assert "xformOp:transform" not in prim.GetPropertyNames() + # Verify standard operations exist + assert "xformOp:translate" in prim.GetPropertyNames() + assert "xformOp:orient" in prim.GetPropertyNames() + assert "xformOp:scale" in prim.GetPropertyNames() + + +def test_standardize_xform_ops_preserves_world_pose(): + """Test that standardize_xform_ops preserves the world-space pose of the prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with specific world pose + translation = (10.0, 20.0, 30.0) + # Rotation of 90 degrees around Z axis + orientation = (0.7071068, 0.0, 0.0, 0.7071068) # w, x, y, z + scale = (2.0, 3.0, 4.0) + + prim = sim_utils.create_prim( + "/World/TestPreservePose", + "Xform", + translation=translation, + orientation=orientation, + scale=scale, + stage=stage, + ) + + # Get the world pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get the world pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify the world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + +def test_standardize_xform_ops_with_units_resolve(): + """Test standardize_xform_ops handles scale:unitsResolve attribute.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim + prim_path = "/World/TestUnitsResolve" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add scale operation + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(1.0, 1.0, 1.0)) + + # Manually add a unitsResolve scale attribute (simulating imported asset with different units) + units_resolve_attr = prim.CreateAttribute("xformOp:scale:unitsResolve", Sdf.ValueTypeNames.Double3) + units_resolve_attr.Set(Gf.Vec3d(100.0, 100.0, 100.0)) # e.g., cm to m conversion + + # Verify both attributes exist + assert "xformOp:scale" in prim.GetPropertyNames() + assert "xformOp:scale:unitsResolve" in prim.GetPropertyNames() + + # Get pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify unitsResolve is removed + assert "xformOp:scale:unitsResolve" not in prim.GetPropertyNames() + + # Verify scale is updated (1.0 * 100.0 = 100.0) + scale = prim.GetAttribute("xformOp:scale").Get() + assert_vec3_close(scale, (100.0, 100.0, 100.0)) + + +def test_standardize_xform_ops_with_hierarchy(): + """Test standardize_xform_ops works correctly with prim hierarchies.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent prim + parent_prim = sim_utils.create_prim( + "/World/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Create child prim + child_prim = sim_utils.create_prim( + "/World/Parent/Child", + "Xform", + translation=(0.0, 3.0, 0.0), + orientation=(0.7071068, 0.0, 0.7071068, 0.0), # 90 deg around Y + scale=(0.5, 0.5, 0.5), + stage=stage, + ) + + # Get world poses before standardization + parent_pos_before, parent_quat_before = sim_utils.resolve_prim_pose(parent_prim) + child_pos_before, child_quat_before = sim_utils.resolve_prim_pose(child_prim) + + # Apply standardize_xform_ops to both + sim_utils.standardize_xform_ops(parent_prim) + sim_utils.standardize_xform_ops(child_prim) + + # Get world poses after standardization + parent_pos_after, parent_quat_after = sim_utils.resolve_prim_pose(parent_prim) + child_pos_after, child_quat_after = sim_utils.resolve_prim_pose(child_prim) + + # Verify world poses are preserved + assert_vec3_close(Gf.Vec3d(*parent_pos_before), parent_pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*parent_quat_before), parent_quat_after, eps=1e-5) + assert_vec3_close(Gf.Vec3d(*child_pos_before), child_pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*child_quat_before), child_quat_after, eps=1e-5) + + +def test_standardize_xform_ops_multiple_deprecated_ops(): + """Test standardize_xform_ops removes multiple deprecated operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with multiple deprecated operations + prim_path = "/World/TestMultipleDeprecated" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add various deprecated rotation operations + rotate_x_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + rotate_x_op.Set(45.0) + rotate_y_op = xformable.AddRotateYOp(UsdGeom.XformOp.PrecisionDouble) + rotate_y_op.Set(30.0) + rotate_z_op = xformable.AddRotateZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_z_op.Set(60.0) + + # Verify deprecated operations exist + assert "xformOp:rotateX" in prim.GetPropertyNames() + assert "xformOp:rotateY" in prim.GetPropertyNames() + assert "xformOp:rotateZ" in prim.GetPropertyNames() + + # Obtain current local transformations + pos, quat = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + sim_utils.standardize_xform_ops(prim) + + # Obtain current local transformations + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos), Gf.Vec3d(*pos_after), eps=1e-5) + assert_quat_close(Gf.Quatd(*quat), Gf.Quatd(*quat_after), eps=1e-5) + + # Verify all deprecated operations are removed + assert "xformOp:rotateX" not in prim.GetPropertyNames() + assert "xformOp:rotateY" not in prim.GetPropertyNames() + assert "xformOp:rotateZ" not in prim.GetPropertyNames() + # Verify standard operations exist + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_existing_standard_ops(): + """Test standardize_xform_ops when prim already has standard operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations already in place + prim = sim_utils.create_prim( + "/World/TestExistingStandard", + "Xform", + translation=(7.0, 8.0, 9.0), + orientation=(0.9238795, 0.3826834, 0.0, 0.0), # rotation around X + scale=(1.5, 2.5, 3.5), + stage=stage, + ) + + # Get initial values + initial_translate = prim.GetAttribute("xformOp:translate").Get() + initial_orient = prim.GetAttribute("xformOp:orient").Get() + initial_scale = prim.GetAttribute("xformOp:scale").Get() + + # Get world pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get world pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify operations still exist and are in correct order + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + # Verify values are approximately preserved + final_translate = prim.GetAttribute("xformOp:translate").Get() + final_orient = prim.GetAttribute("xformOp:orient").Get() + final_scale = prim.GetAttribute("xformOp:scale").Get() + + assert_vec3_close(initial_translate, final_translate, eps=1e-5) + assert_quat_close(initial_orient, final_orient, eps=1e-5) + assert_vec3_close(initial_scale, final_scale, eps=1e-5) + + +def test_standardize_xform_ops_invalid_prim(): + """Test standardize_xform_ops raises error for invalid prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim (non-existent path) + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + + # Verify the prim is invalid + assert not invalid_prim.IsValid() + + # Attempt to apply standardize_xform_ops and expect ValueError + with pytest.raises(ValueError, match="not valid"): + sim_utils.standardize_xform_ops(invalid_prim) + + +def test_standardize_xform_ops_on_geometry_prim(): + """Test standardize_xform_ops on a geometry prim (Cube, Sphere, etc.).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a cube with transform + cube_prim = sim_utils.create_prim( + "/World/TestCube", + "Cube", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + attributes={"size": 1.0}, + stage=stage, + ) + + # Get world pose before + pos_before, quat_before = sim_utils.resolve_prim_pose(cube_prim) + + # Apply standardize_xform_ops + sim_utils.standardize_xform_ops(cube_prim) + + # Get world pose after + pos_after, quat_after = sim_utils.resolve_prim_pose(cube_prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + # Verify standard operations exist + xform_ops = get_xform_ops(cube_prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_non_uniform_scale(): + """Test standardize_xform_ops with non-uniform scale.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with non-uniform scale + prim = sim_utils.create_prim( + "/World/TestNonUniformScale", + "Xform", + translation=(5.0, 10.0, 15.0), + orientation=(0.7071068, 0.0, 0.7071068, 0.0), # 90 deg around Y + scale=(1.0, 2.0, 3.0), # Non-uniform scale + stage=stage, + ) + + # Get initial scale + initial_scale = prim.GetAttribute("xformOp:scale").Get() + + # Get world pose before standardization + pos_before, quat_before = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Get world pose after standardization + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + # Verify world pose is preserved + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + # Verify scale is preserved + final_scale = prim.GetAttribute("xformOp:scale").Get() + assert_vec3_close(initial_scale, final_scale, eps=1e-5) + + +def test_standardize_xform_ops_identity_transform(): + """Test standardize_xform_ops with identity transform (no translation, rotation, or scale).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with identity transform + prim = sim_utils.create_prim( + "/World/TestIdentity", + "Xform", + translation=(0.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), # Identity quaternion + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Apply standardize_xform_ops + sim_utils.standardize_xform_ops(prim) + + # Verify standard operations exist + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + # Verify identity values + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), (0.0, 0.0, 0.0)) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), (1.0, 0.0, 0.0, 0.0)) + assert_vec3_close(prim.GetAttribute("xformOp:scale").Get(), (1.0, 1.0, 1.0)) + + +def test_standardize_xform_ops_with_explicit_values(): + """Test standardize_xform_ops with explicit translation, orientation, and scale values.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with some initial transform + prim = sim_utils.create_prim( + "/World/TestExplicitValues", + "Xform", + translation=(10.0, 10.0, 10.0), + orientation=(0.7071068, 0.7071068, 0.0, 0.0), + scale=(5.0, 5.0, 5.0), + stage=stage, + ) + + # Apply standardize_xform_ops with new explicit values + new_translation = (1.0, 2.0, 3.0) + new_orientation = (1.0, 0.0, 0.0, 0.0) + new_scale = (2.0, 2.0, 2.0) + + result = sim_utils.standardize_xform_ops( + prim, translation=new_translation, orientation=new_orientation, scale=new_scale + ) + assert result is True + + # Verify the new values are set + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), new_translation) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), new_orientation) + assert_vec3_close(prim.GetAttribute("xformOp:scale").Get(), new_scale) + + # Verify the prim is at the expected world location + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + assert_vec3_close(Gf.Vec3d(*pos_after), new_translation, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_after), new_orientation, eps=1e-5) + + # Verify standard operation order + xform_ops = get_xform_ops(prim) + assert xform_ops == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +def test_standardize_xform_ops_with_partial_values(): + """Test standardize_xform_ops with only some values specified.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim + prim = sim_utils.create_prim( + "/World/TestPartialValues", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(0.9238795, 0.3826834, 0.0, 0.0), # rotation around X + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Get initial local pose + pos_before, quat_before = sim_utils.resolve_prim_pose(prim, ref_prim=prim.GetParent()) + scale_before = prim.GetAttribute("xformOp:scale").Get() + + # Apply standardize_xform_ops with only translation specified + new_translation = (10.0, 20.0, 30.0) + result = sim_utils.standardize_xform_ops(prim, translation=new_translation) + assert result is True + + # Verify translation is updated + assert_vec3_close(prim.GetAttribute("xformOp:translate").Get(), new_translation) + + # Verify orientation and scale are preserved + quat_after = prim.GetAttribute("xformOp:orient").Get() + scale_after = prim.GetAttribute("xformOp:scale").Get() + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + assert_vec3_close(scale_before, scale_after, eps=1e-5) + + # Verify the prim's world orientation hasn't changed (only translation changed) + _, quat_after_world = sim_utils.resolve_prim_pose(prim) + assert_quat_close(Gf.Quatd(*quat_before), quat_after_world, eps=1e-5) + + +def test_standardize_xform_ops_non_xformable_prim(): + """Test standardize_xform_ops returns False for non-Xformable prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a Material prim (not Xformable) + from pxr import UsdShade + + material_prim = UsdShade.Material.Define(stage, "/World/TestMaterial").GetPrim() + + # Verify the prim is valid but not Xformable + assert material_prim.IsValid() + assert not material_prim.IsA(UsdGeom.Xformable) + + # Attempt to apply standardize_xform_ops - should return False + result = sim_utils.standardize_xform_ops(material_prim) + assert result is False + + +def test_standardize_xform_ops_preserves_reset_xform_stack(): + """Test that standardize_xform_ops preserves the resetXformStack attribute.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim + prim = sim_utils.create_prim("/World/TestResetStack", "Xform", stage=stage) + xformable = UsdGeom.Xformable(prim) + + # Set resetXformStack to True + xformable.SetResetXformStack(True) + assert xformable.GetResetXformStack() is True + + # Apply standardize_xform_ops + result = sim_utils.standardize_xform_ops(prim) + assert result is True + + # Verify resetXformStack is preserved + assert xformable.GetResetXformStack() is True + + +def test_standardize_xform_ops_with_complex_hierarchy(): + """Test standardize_xform_ops on deeply nested hierarchy.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a complex hierarchy + root = sim_utils.create_prim("/World/Root", "Xform", translation=(1.0, 0.0, 0.0), stage=stage) + child1 = sim_utils.create_prim("/World/Root/Child1", "Xform", translation=(0.0, 1.0, 0.0), stage=stage) + child2 = sim_utils.create_prim("/World/Root/Child1/Child2", "Xform", translation=(0.0, 0.0, 1.0), stage=stage) + child3 = sim_utils.create_prim("/World/Root/Child1/Child2/Child3", "Cube", translation=(1.0, 1.0, 1.0), stage=stage) + + # Get world poses before + poses_before = {} + for name, prim in [("root", root), ("child1", child1), ("child2", child2), ("child3", child3)]: + poses_before[name] = sim_utils.resolve_prim_pose(prim) + + # Apply standardize_xform_ops to all prims + assert sim_utils.standardize_xform_ops(root) is True + assert sim_utils.standardize_xform_ops(child1) is True + assert sim_utils.standardize_xform_ops(child2) is True + assert sim_utils.standardize_xform_ops(child3) is True + + # Get world poses after + poses_after = {} + for name, prim in [("root", root), ("child1", child1), ("child2", child2), ("child3", child3)]: + poses_after[name] = sim_utils.resolve_prim_pose(prim) + + # Verify all world poses are preserved + for name in poses_before: + pos_before, quat_before = poses_before[name] + pos_after, quat_after = poses_after[name] + assert_vec3_close(Gf.Vec3d(*pos_before), pos_after, eps=1e-5) + assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) + + +""" +Performance Benchmarking Tests +""" + +import time + + +def test_standardize_xform_ops_performance_batch(): + """Benchmark standardize_xform_ops performance on multiple prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create many test prims + num_prims = 1024 + prims = [] + + for i in range(num_prims): + prim = stage.DefinePrim(f"/World/PerfTestBatch/Prim_{i:03d}", "Xform") + xformable = UsdGeom.Xformable(prim) + # Add various deprecated operations + xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(i * 1.0, i * 2.0, i * 3.0)) + xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(i, i, i)) + prims.append(prim) + + # Benchmark batch operation + start_time = time.perf_counter() + for prim in prims: + result = sim_utils.standardize_xform_ops(prim) + assert result is True + end_time = time.perf_counter() + + # Print timing + elapsed_ms = (end_time - start_time) * 1000 + avg_ms = elapsed_ms / num_prims + print(f"\n Batch standardization ({num_prims} prims): {elapsed_ms:.4f} ms total, {avg_ms:.4f} ms/prim") + + # Verify operation is reasonably fast + assert avg_ms < 0.1, f"Average operation took {avg_ms:.2f}ms/prim, expected < 0.1ms/prim" From dcc4737597bf646ddd3773a58798fc0e82bc62f6 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:22:05 +0100 Subject: [PATCH 002/100] wraps code inside sdf block for optimization --- source/isaaclab/isaaclab/sim/utils/xform.py | 76 +++++++++++---------- 1 file changed, 40 insertions(+), 36 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/xform.py b/source/isaaclab/isaaclab/sim/utils/xform.py index 16e1455917b..a1f609080b3 100644 --- a/source/isaaclab/isaaclab/sim/utils/xform.py +++ b/source/isaaclab/isaaclab/sim/utils/xform.py @@ -14,10 +14,9 @@ from __future__ import annotations -__all__ = ["standardize_xform_ops"] - import logging -from pxr import Gf, Usd, UsdGeom + +from pxr import Gf, Sdf, Usd, UsdGeom # import logger logger = logging.getLogger(__name__) @@ -145,39 +144,44 @@ def standardize_xform_ops( # No scale exists, use default uniform scale xform_scale = Gf.Vec3d(1.0, 1.0, 1.0) - # Clear the existing transform operation order + # Verify if xform stack is reset has_reset = xformable.GetResetXformStack() - for prop_name in prop_names: - if prop_name in _INVALID_XFORM_OPS: - prim.RemoveProperty(prop_name) - - # Remove unitsResolve attribute if present (already handled in scale resolution above) - if "xformOp:scale:unitsResolve" in prop_names: - prim.RemoveProperty("xformOp:scale:unitsResolve") - - # Set up or retrieve scale operation - xform_op_scale = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) - if not xform_op_scale: - xform_op_scale = xformable.AddXformOp(UsdGeom.XformOp.TypeScale, UsdGeom.XformOp.PrecisionDouble, "") - - # Set up or retrieve translate operation - xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) - if not xform_op_translate: - xform_op_translate = xformable.AddXformOp(UsdGeom.XformOp.TypeTranslate, UsdGeom.XformOp.PrecisionDouble, "") - - # Set up or retrieve orient (quaternion rotation) operation - xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) - if not xform_op_orient: - xform_op_orient = xformable.AddXformOp(UsdGeom.XformOp.TypeOrient, UsdGeom.XformOp.PrecisionDouble, "") - - # Set the transform operation order: translate -> orient -> scale - # This is the standard USD convention and ensures consistent behavior - xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) - - # Set the transform values using the new standardized transform operations - # Convert tuples to Gf types for USD - xform_op_translate.Set(xform_pos) - xform_op_orient.Set(xform_quat) - xform_op_scale.Set(xform_scale) + # Batch the operations + with Sdf.ChangeBlock(): + # Clear the existing transform operation order + for prop_name in prop_names: + if prop_name in _INVALID_XFORM_OPS: + prim.RemoveProperty(prop_name) + + # Remove unitsResolve attribute if present (already handled in scale resolution above) + if "xformOp:scale:unitsResolve" in prop_names: + prim.RemoveProperty("xformOp:scale:unitsResolve") + + # Set up or retrieve scale operation + xform_op_scale = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) + if not xform_op_scale: + xform_op_scale = xformable.AddXformOp(UsdGeom.XformOp.TypeScale, UsdGeom.XformOp.PrecisionDouble, "") + + # Set up or retrieve translate operation + xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + if not xform_op_translate: + xform_op_translate = xformable.AddXformOp( + UsdGeom.XformOp.TypeTranslate, UsdGeom.XformOp.PrecisionDouble, "" + ) + + # Set up or retrieve orient (quaternion rotation) operation + xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + if not xform_op_orient: + xform_op_orient = xformable.AddXformOp(UsdGeom.XformOp.TypeOrient, UsdGeom.XformOp.PrecisionDouble, "") + + # Set the transform operation order: translate -> orient -> scale + # This is the standard USD convention and ensures consistent behavior + xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) + + # Set the transform values using the new standardized transform operations + # Convert tuples to Gf types for USD + xform_op_translate.Set(xform_pos) + xform_op_orient.Set(xform_quat) + xform_op_scale.Set(xform_scale) return True From fcf6c1a4f147056dc63eee3b3070c0f15f3b46c7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:23:11 +0100 Subject: [PATCH 003/100] runs formatter --- source/isaaclab/isaaclab/sim/utils/xform.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/xform.py b/source/isaaclab/isaaclab/sim/utils/xform.py index a1f609080b3..4b9eaf2e366 100644 --- a/source/isaaclab/isaaclab/sim/utils/xform.py +++ b/source/isaaclab/isaaclab/sim/utils/xform.py @@ -174,14 +174,14 @@ def standardize_xform_ops( if not xform_op_orient: xform_op_orient = xformable.AddXformOp(UsdGeom.XformOp.TypeOrient, UsdGeom.XformOp.PrecisionDouble, "") - # Set the transform operation order: translate -> orient -> scale - # This is the standard USD convention and ensures consistent behavior - xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) - # Set the transform values using the new standardized transform operations # Convert tuples to Gf types for USD xform_op_translate.Set(xform_pos) xform_op_orient.Set(xform_quat) xform_op_scale.Set(xform_scale) + # Set the transform operation order: translate -> orient -> scale + # This is the standard USD convention and ensures consistent behavior + xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) + return True From f54ae43da2389cf288d457fb09a0986cc2e5fd3b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:30:43 +0100 Subject: [PATCH 004/100] updates doc strings --- source/isaaclab/isaaclab/sim/utils/xform.py | 89 +++++++++++++-------- 1 file changed, 56 insertions(+), 33 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/xform.py b/source/isaaclab/isaaclab/sim/utils/xform.py index 4b9eaf2e366..fea5cfe5425 100644 --- a/source/isaaclab/isaaclab/sim/utils/xform.py +++ b/source/isaaclab/isaaclab/sim/utils/xform.py @@ -42,64 +42,87 @@ def standardize_xform_ops( orientation: tuple[float, float, float, float] | None = None, scale: tuple[float, float, float] | None = None, ) -> bool: - """Standardize and normalize the transform operations on a USD prim. + """Standardize the transform operation stack on a USD prim to a canonical form. - This function standardizes a prim's transform stack to use the common transform operation - order: translate, orient (quaternion rotation), and scale. It performs the following: + This function converts a prim's transform stack to use the standard USD transform operation + order: [translate, orient, scale]. The function performs the following operations: - 1. Captures the current local pose of the prim (relative to parent) - 2. Clears all existing transform operations - 3. Removes deprecated/non-standard transform operations (e.g., rotateXYZ, transform matrix) - 4. Establishes the standard transform stack: [translate, orient, scale] - 5. Handles unit resolution for scale attributes - 6. Restores the original local pose using the new standardized operations + 1. Validates that the prim is Xformable + 2. Captures the current local transform (translation, rotation, scale) + 3. Resolves and bakes unit scale conversions (xformOp:scale:unitsResolve) + 4. Creates or reuses standard transform operations (translate, orient, scale) + 5. Sets the transform operation order to [translate, orient, scale] + 6. Applies the preserved or user-specified transform values - This is particularly useful when importing assets from different sources that may use - various transform operation conventions, ensuring a consistent and predictable transform - stack across all prims in the scene. + The entire modification is performed within an ``Sdf.ChangeBlock`` for optimal performance + when processing multiple prims. .. note:: - The standard transform operation order follows USD best practices: + **Standard Transform Order:** The function enforces the USD best practice order: ``xformOp:translate``, ``xformOp:orient``, ``xformOp:scale``. This order is - compatible with most USD tools and workflows. + compatible with most USD tools and workflows, and uses quaternions for rotation + (avoiding gimbal lock issues). + + .. note:: + **Pose Preservation:** By default, the function preserves the prim's local transform + (relative to its parent). The world-space position of the prim remains unchanged + unless explicit ``translation``, ``orientation``, or ``scale`` values are provided. .. warning:: - This function modifies the prim's transform stack in place. While it preserves - the local pose by default, any animation or time-sampled transform data will be lost - as only the current (default) time code values are preserved. + **Animation Data Loss:** This function only preserves transform values at the default + time code (``Usd.TimeCode.Default()``). Any animation or time-sampled transform data + will be lost. Use this function during asset import or preparation, not on animated prims. + + .. warning:: + **Unit Scale Resolution:** If the prim has a ``xformOp:scale:unitsResolve`` attribute + (common in imported assets with unit mismatches), it will be baked into the scale + and removed. For example, a scale of (1, 1, 1) with unitsResolve of (100, 100, 100) + becomes a final scale of (100, 100, 100). Args: - prim: The USD prim to standardize transform operations for. Must be a valid - prim that supports the Xformable schema. - translation: Optional translation (x, y, z) to set. If None, preserves current + prim: The USD prim to standardize. Must be a valid prim that supports the + UsdGeom.Xformable schema (e.g., Xform, Mesh, Cube, etc.). Material and + Shader prims are not Xformable and will return False. + translation: Optional translation vector (x, y, z) in local space. If provided, + overrides the prim's current translation. If None, preserves the current local translation. Defaults to None. - orientation: Optional orientation quaternion (w, x, y, z) to set. If None, preserves - current local orientation. Defaults to None. - scale: Optional scale (x, y, z) to set. If None, preserves current scale or uses - (1.0, 1.0, 1.0) if no scale exists. Defaults to None. + orientation: Optional orientation quaternion (w, x, y, z) in local space. If provided, + overrides the prim's current orientation. If None, preserves the current + local orientation. Defaults to None. + scale: Optional scale vector (x, y, z). If provided, overrides the prim's current scale. + If None, preserves the current scale (after unit resolution) or uses (1, 1, 1) + if no scale exists. Defaults to None. Returns: - True if the transform operations were standardized successfully, False otherwise. + bool: True if the transform operations were successfully standardized. False if the + prim is not Xformable (e.g., Material, Shader prims). The function will log an + error message when returning False. Raises: - ValueError: If the prim is not valid or does not support transform operations. + ValueError: If the prim is not valid (i.e., does not exist or is an invalid prim). Example: >>> import isaaclab.sim as sim_utils >>> - >>> # Get a prim with non-standard transform operations - >>> prim = stage.GetPrimAtPath("/World/Asset") - >>> # Standardize its transform stack while preserving pose - >>> sim_utils.standardize_xform_ops(prim) - >>> # The prim now uses: translate, orient, scale in that order + >>> # Standardize a prim with non-standard transform operations + >>> prim = stage.GetPrimAtPath("/World/ImportedAsset") + >>> result = sim_utils.standardize_xform_ops(prim) + >>> if result: + ... print("Transform stack standardized successfully") + >>> # The prim now uses: [translate, orient, scale] in that order >>> - >>> # Or standardize and set new transform values + >>> # Standardize and set new transform values >>> sim_utils.standardize_xform_ops( ... prim, ... translation=(1.0, 2.0, 3.0), - ... orientation=(1.0, 0.0, 0.0, 0.0), + ... orientation=(1.0, 0.0, 0.0, 0.0), # identity rotation (w, x, y, z) ... scale=(2.0, 2.0, 2.0) ... ) + >>> + >>> # Batch processing for performance + >>> prims_to_standardize = [stage.GetPrimAtPath(p) for p in prim_paths] + >>> for prim in prims_to_standardize: + ... sim_utils.standardize_xform_ops(prim) # Each call uses Sdf.ChangeBlock """ # Validate prim if not prim.IsValid(): From 2f9afadaf901f4db58de46291a85cc61765068ea Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:53:46 +0100 Subject: [PATCH 005/100] fixes type-hinting --- source/isaaclab/isaaclab/sim/utils/xform.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/xform.py b/source/isaaclab/isaaclab/sim/utils/xform.py index fea5cfe5425..d9d3b716530 100644 --- a/source/isaaclab/isaaclab/sim/utils/xform.py +++ b/source/isaaclab/isaaclab/sim/utils/xform.py @@ -38,9 +38,9 @@ def standardize_xform_ops( prim: Usd.Prim, - translation: tuple[float, float, float] | None = None, - orientation: tuple[float, float, float, float] | None = None, - scale: tuple[float, float, float] | None = None, + translation: tuple[float, ...] | None = None, + orientation: tuple[float, ...] | None = None, + scale: tuple[float, ...] | None = None, ) -> bool: """Standardize the transform operation stack on a USD prim to a canonical form. From c1413d4125fdecc0369cd6395196814c153ee037 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:54:34 +0100 Subject: [PATCH 006/100] removes isaac sim xform class from create_prim --- source/isaaclab/isaaclab/sim/utils/prims.py | 136 ++++++++++++++------ 1 file changed, 100 insertions(+), 36 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 2155ddc2172..52207dad3b5 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -21,13 +21,14 @@ from isaacsim.core.cloner import Cloner from isaacsim.core.version import get_version from omni.usd.commands import DeletePrimsCommand, MovePrimCommand -from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade +from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade from isaaclab.utils.string import to_camel_case from .queries import find_matching_prim_paths from .semantics import add_labels from .stage import attach_stage_to_usd_context, get_current_stage, get_current_stage_id +from .xform import standardize_xform_ops if TYPE_CHECKING: from isaaclab.sim.spawners.spawner_cfg import SpawnerCfg @@ -57,40 +58,82 @@ def create_prim( """Creates a prim in the provided USD stage. The method applies the specified transforms, the semantic label and sets the specified attributes. + The transform can be specified either in world space (using ``position``) or local space (using + ``translation``). + + The function determines the coordinate system of the transform based on the provided arguments. + + * If ``position`` is provided, it is assumed the orientation is provided in the world frame as well. + * If ``translation`` is provided, it is assumed the orientation is provided in the local frame as well. + + The scale is always applied in the local frame. + + .. note:: + Transform operations are standardized to the USD convention: translate, orient (quaternion), + and scale, in that order. See :func:`standardize_xform_ops` for more details. Args: - prim_path: The path of the new prim. - prim_type: Prim type name - position: prim position (applied last) - translation: prim translation (applied last) - orientation: prim rotation as quaternion - scale: scaling factor in x, y, z. - usd_path: Path to the USD that this prim will reference. - semantic_label: Semantic label. - semantic_type: set to "class" unless otherwise specified. - attributes: Key-value pairs of prim attributes to set. - stage: The stage to create the prim in. Defaults to None, in which case the current stage is used. + prim_path: + The path of the new prim. + prim_type: + Prim type name. Defaults to "Xform", in which case a simple Xform prim is created. + position: + Prim position in world space as (x, y, z). If the prim has a parent, this is + automatically converted to local space relative to the parent. Cannot be used with + ``translation``. Defaults to None, in which case no position is applied. + translation: + Prim translation in local space as (x, y, z). This is applied directly without + any coordinate transformation. Cannot be used with ``position``. Defaults to None, + in which case no translation is applied. + orientation: + Prim rotation as a quaternion (w, x, y, z). When used with ``position``, the + orientation is also converted from world space to local space. When used with ``translation``, + it is applied directly as local orientation. Defaults to None. + scale: + Scaling factor in x, y, z. Applied in local space. Defaults to None, + in which case a uniform scale of 1.0 is applied. + usd_path: + Path to the USD file that this prim will reference. Defaults to None. + semantic_label: + Semantic label to apply to the prim. Defaults to None, in which case no label is added. + semantic_type: + Semantic type for the label. Defaults to "class". + attributes: + Key-value pairs of prim attributes to set. Defaults to None, in which case no attributes are set. + stage: + The stage to create the prim in. Defaults to None, in which case the current stage is used. Returns: The created USD prim. Raises: - ValueError: If there is already a prim at the provided prim_path. + ValueError: If there is already a prim at the provided prim path. + ValueError: If both position and translation are provided. Example: >>> import isaaclab.sim as sim_utils >>> - >>> # create a cube (/World/Cube) of size 2 centered at (1.0, 0.5, 0.0) + >>> # Create a cube at world position (1.0, 0.5, 0.0) >>> sim_utils.create_prim( - ... prim_path="/World/Cube", + ... prim_path="/World/Parent/Cube", ... prim_type="Cube", ... position=(1.0, 0.5, 0.0), ... attributes={"size": 2.0} ... ) - Usd.Prim() + Usd.Prim() + >>> + >>> # Create a sphere with local translation relative to its parent + >>> sim_utils.create_prim( + ... prim_path="/World/Parent/Sphere", + ... prim_type="Sphere", + ... translation=(0.5, 0.0, 0.0), + ... scale=(2.0, 2.0, 2.0) + ... ) + Usd.Prim() """ - # Note: Imported here to prevent cyclic dependency in the module. - from isaacsim.core.prims import XFormPrim + # Ensure that user doesn't provide both position and translation + if position is not None and translation is not None: + raise ValueError("Cannot provide both position and translation. Please provide only one.") # obtain stage handle stage = get_current_stage() if stage is None else stage @@ -114,26 +157,47 @@ def create_prim( if semantic_label is not None: add_labels(prim, labels=[semantic_label], instance_name=semantic_type) - # apply the transformations - from isaacsim.core.api.simulation_context.simulation_context import SimulationContext + # convert position and orientation to translation and orientation + # world --> local + if position is not None: + # this means that user provided pose in the world frame + # obtain parent transform + parent_prim = prim.GetParent() + if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: + # Get parent's world transform + parent_xformable = UsdGeom.Xformable(parent_prim) + parent_world_tf = parent_xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + + # Create world transform for the desired position and orientation + desired_world_tf = Gf.Matrix4d() + desired_world_tf.SetTranslateOnly(Gf.Vec3d(*position)) + + if orientation is not None: + # Set rotation from quaternion (w, x, y, z) + quat = Gf.Quatd(*orientation) + desired_world_tf.SetRotateOnly(quat) + + # Convert world transform to local: local = inv(parent_world) * world + parent_world_tf_inv = parent_world_tf.GetInverse() + local_tf = desired_world_tf * parent_world_tf_inv + + # Extract local translation and orientation + local_transform = Gf.Transform(local_tf) + translation = tuple(local_transform.GetTranslation()) + if orientation is not None: + quat_result = local_transform.GetRotation().GetQuat() + orientation = (quat_result.GetReal(), *quat_result.GetImaginary()) + else: + # No parent or parent is root, position is already in local space + translation = position - if SimulationContext.instance() is None: - # FIXME: remove this, we should never even use backend utils especially not numpy ones - import isaacsim.core.utils.numpy as backend_utils + # Convert sequences to properly-typed tuples for standardize_xform_ops + translation_tuple = None if translation is None else tuple(translation) + orientation_tuple = None if orientation is None else tuple(orientation) + scale_tuple = None if scale is None else tuple(scale) - device = "cpu" - else: - backend_utils = SimulationContext.instance().backend_utils - device = SimulationContext.instance().device - if position is not None: - position = backend_utils.expand_dims(backend_utils.convert(position, device), 0) - if translation is not None: - translation = backend_utils.expand_dims(backend_utils.convert(translation, device), 0) - if orientation is not None: - orientation = backend_utils.expand_dims(backend_utils.convert(orientation, device), 0) - if scale is not None: - scale = backend_utils.expand_dims(backend_utils.convert(scale, device), 0) - XFormPrim(prim_path, positions=position, translations=translation, orientations=orientation, scales=scale) + # standardize the xform ops + standardize_xform_ops(prim, translation_tuple, orientation_tuple, scale_tuple) return prim From 4652ac628f3b668c82a56cc09284c2b86be88723 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 12:02:27 +0100 Subject: [PATCH 007/100] moves functions for pose to transforms module --- .../isaaclab/isaaclab/sim/utils/__init__.py | 2 +- source/isaaclab/isaaclab/sim/utils/prims.py | 91 ---------- .../sim/utils/{xform.py => transforms.py} | 86 +++++++++ source/isaaclab/test/sim/test_utils_prims.py | 170 ------------------ ...tils_xform.py => test_utils_transforms.py} | 165 +++++++++++++++++ 5 files changed, 252 insertions(+), 262 deletions(-) rename source/isaaclab/isaaclab/sim/utils/{xform.py => transforms.py} (71%) rename source/isaaclab/test/sim/{test_utils_xform.py => test_utils_transforms.py} (78%) diff --git a/source/isaaclab/isaaclab/sim/utils/__init__.py b/source/isaaclab/isaaclab/sim/utils/__init__.py index da7da10baf9..7fc1c6d1a77 100644 --- a/source/isaaclab/isaaclab/sim/utils/__init__.py +++ b/source/isaaclab/isaaclab/sim/utils/__init__.py @@ -10,4 +10,4 @@ from .queries import * # noqa: F401, F403 from .semantics import * # noqa: F401, F403 from .stage import * # noqa: F401, F403 -from .xform import * # noqa: F401, F403 +from .transforms import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 52207dad3b5..5e017481cd1 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -304,97 +304,6 @@ def make_uninstanceable(prim_path: str | Sdf.Path, stage: Usd.Stage | None = Non all_prims += child_prim.GetFilteredChildren(Usd.TraverseInstanceProxies()) -def resolve_prim_pose( - prim: Usd.Prim, ref_prim: Usd.Prim | None = None -) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: - """Resolve the pose of a prim with respect to another prim. - - Note: - This function ignores scale and skew by orthonormalizing the transformation - matrix at the final step. However, if any ancestor prim in the hierarchy - has non-uniform scale, that scale will still affect the resulting position - and orientation of the prim (because it's baked into the transform before - scale removal). - - In other words: scale **is not removed hierarchically**. If you need - completely scale-free poses, you must walk the transform chain and strip - scale at each level. Please open an issue if you need this functionality. - - Args: - prim: The USD prim to resolve the pose for. - ref_prim: The USD prim to compute the pose with respect to. - Defaults to None, in which case the world frame is used. - - Returns: - A tuple containing the position (as a 3D vector) and the quaternion orientation - in the (w, x, y, z) format. - - Raises: - ValueError: If the prim or ref prim is not valid. - """ - # check if prim is valid - if not prim.IsValid(): - raise ValueError(f"Prim at path '{prim.GetPath().pathString}' is not valid.") - # get prim xform - xform = UsdGeom.Xformable(prim) - prim_tf = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - # sanitize quaternion - # this is needed, otherwise the quaternion might be non-normalized - prim_tf = prim_tf.GetOrthonormalized() - - if ref_prim is not None: - # check if ref prim is valid - if not ref_prim.IsValid(): - raise ValueError(f"Ref prim at path '{ref_prim.GetPath().pathString}' is not valid.") - # get ref prim xform - ref_xform = UsdGeom.Xformable(ref_prim) - ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - # make sure ref tf is orthonormal - ref_tf = ref_tf.GetOrthonormalized() - # compute relative transform to get prim in ref frame - prim_tf = prim_tf * ref_tf.GetInverse() - - # extract position and orientation - prim_pos = [*prim_tf.ExtractTranslation()] - prim_quat = [prim_tf.ExtractRotationQuat().real, *prim_tf.ExtractRotationQuat().imaginary] - return tuple(prim_pos), tuple(prim_quat) - - -def resolve_prim_scale(prim: Usd.Prim) -> tuple[float, float, float]: - """Resolve the scale of a prim in the world frame. - - At an attribute level, a USD prim's scale is a scaling transformation applied to the prim with - respect to its parent prim. This function resolves the scale of the prim in the world frame, - by computing the local to world transform of the prim. This is equivalent to traversing up - the prim hierarchy and accounting for the rotations and scales of the prims. - - For instance, if a prim has a scale of (1, 2, 3) and it is a child of a prim with a scale of (4, 5, 6), - then the scale of the prim in the world frame is (4, 10, 18). - - Args: - prim: The USD prim to resolve the scale for. - - Returns: - The scale of the prim in the x, y, and z directions in the world frame. - - Raises: - ValueError: If the prim is not valid. - """ - # check if prim is valid - if not prim.IsValid(): - raise ValueError(f"Prim at path '{prim.GetPath().pathString}' is not valid.") - # compute local to world transform - xform = UsdGeom.Xformable(prim) - world_transform = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - # extract scale - return tuple([*(v.GetLength() for v in world_transform.ExtractRotationMatrix())]) - - -""" -Attribute - Setters. -""" - - def set_prim_visibility(prim: Usd.Prim, visible: bool) -> None: """Sets the visibility of the prim in the opened stage. diff --git a/source/isaaclab/isaaclab/sim/utils/xform.py b/source/isaaclab/isaaclab/sim/utils/transforms.py similarity index 71% rename from source/isaaclab/isaaclab/sim/utils/xform.py rename to source/isaaclab/isaaclab/sim/utils/transforms.py index d9d3b716530..171e06f70c5 100644 --- a/source/isaaclab/isaaclab/sim/utils/xform.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -208,3 +208,89 @@ def standardize_xform_ops( xformable.SetXformOpOrder([xform_op_translate, xform_op_orient, xform_op_scale], has_reset) return True + + +def resolve_prim_pose( + prim: Usd.Prim, ref_prim: Usd.Prim | None = None +) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + """Resolve the pose of a prim with respect to another prim. + + Note: + This function ignores scale and skew by orthonormalizing the transformation + matrix at the final step. However, if any ancestor prim in the hierarchy + has non-uniform scale, that scale will still affect the resulting position + and orientation of the prim (because it's baked into the transform before + scale removal). + + In other words: scale **is not removed hierarchically**. If you need + completely scale-free poses, you must walk the transform chain and strip + scale at each level. Please open an issue if you need this functionality. + + Args: + prim: The USD prim to resolve the pose for. + ref_prim: The USD prim to compute the pose with respect to. + Defaults to None, in which case the world frame is used. + + Returns: + A tuple containing the position (as a 3D vector) and the quaternion orientation + in the (w, x, y, z) format. + + Raises: + ValueError: If the prim or ref prim is not valid. + """ + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + # get prim xform + xform = UsdGeom.Xformable(prim) + prim_tf = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf = prim_tf.GetOrthonormalized() + + if ref_prim is not None: + # check if ref prim is valid + if not ref_prim.IsValid(): + raise ValueError(f"Ref prim at path '{ref_prim.GetPath().pathString}' is not valid.") + # get ref prim xform + ref_xform = UsdGeom.Xformable(ref_prim) + ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # make sure ref tf is orthonormal + ref_tf = ref_tf.GetOrthonormalized() + # compute relative transform to get prim in ref frame + prim_tf = prim_tf * ref_tf.GetInverse() + + # extract position and orientation + prim_pos = [*prim_tf.ExtractTranslation()] + prim_quat = [prim_tf.ExtractRotationQuat().real, *prim_tf.ExtractRotationQuat().imaginary] + return tuple(prim_pos), tuple(prim_quat) + + +def resolve_prim_scale(prim: Usd.Prim) -> tuple[float, float, float]: + """Resolve the scale of a prim in the world frame. + + At an attribute level, a USD prim's scale is a scaling transformation applied to the prim with + respect to its parent prim. This function resolves the scale of the prim in the world frame, + by computing the local to world transform of the prim. This is equivalent to traversing up + the prim hierarchy and accounting for the rotations and scales of the prims. + + For instance, if a prim has a scale of (1, 2, 3) and it is a child of a prim with a scale of (4, 5, 6), + then the scale of the prim in the world frame is (4, 10, 18). + + Args: + prim: The USD prim to resolve the scale for. + + Returns: + The scale of the prim in the x, y, and z directions in the world frame. + + Raises: + ValueError: If the prim is not valid. + """ + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + # compute local to world transform + xform = UsdGeom.Xformable(prim) + world_transform = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # extract scale + return tuple([*(v.GetLength() for v in world_transform.ExtractRotationMatrix())]) diff --git a/source/isaaclab/test/sim/test_utils_prims.py b/source/isaaclab/test/sim/test_utils_prims.py index 6a79645b595..54c04afe7e9 100644 --- a/source/isaaclab/test/sim/test_utils_prims.py +++ b/source/isaaclab/test/sim/test_utils_prims.py @@ -14,14 +14,11 @@ """Rest everything follows.""" import math -import numpy as np -import torch import pytest from pxr import Gf, Sdf, Usd, UsdGeom import isaaclab.sim as sim_utils -import isaaclab.utils.math as math_utils from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR @@ -188,173 +185,6 @@ def test_move_prim(): assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), Gf.Quatd(0.0, 0.0, 0.0, 1.0)) -""" -USD Prim properties and attributes. -""" - - -def test_resolve_prim_pose(): - """Test resolve_prim_pose() function.""" - # number of objects - num_objects = 20 - # sample random scales for x, y, z - rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) - rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) - # sample random positions - rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) - # sample random rotations - rand_quats = np.random.randn(num_objects, 3, 4) - rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) - - # create objects - for i in range(num_objects): - # simple cubes - cube_prim = sim_utils.create_prim( - f"/World/Cubes/instance_{i:02d}", - "Cube", - translation=rand_positions[i, 0], - orientation=rand_quats[i, 0], - scale=rand_scales[i, 0], - attributes={"size": rand_widths[i]}, - ) - # xform hierarchy - xform_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}", - "Xform", - translation=rand_positions[i, 1], - orientation=rand_quats[i, 1], - scale=rand_scales[i, 1], - ) - geometry_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/geometry", - "Sphere", - translation=rand_positions[i, 2], - orientation=rand_quats[i, 2], - scale=rand_scales[i, 2], - attributes={"radius": rand_widths[i]}, - ) - dummy_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/dummy", - "Sphere", - ) - - # cube prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(cube_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) - # xform prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(xform_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) - # dummy prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(dummy_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) - - # geometry prim w.r.t. xform prim - pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) - # TODO: Enabling scale causes the test to fail because the current implementation of - # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known - # limitation. Until this is fixed, the test is disabled here to ensure the test passes. - # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) - - # dummy prim w.r.t. xform prim - pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) - pos, quat = np.array(pos), np.array(quat) - np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) - np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) - # xform prim w.r.t. cube prim - pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) - pos, quat = np.array(pos), np.array(quat) - # -- compute ground truth values - gt_pos, gt_quat = math_utils.subtract_frame_transforms( - torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), - torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), - torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), - torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), - ) - gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() - quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, gt_pos, atol=1e-3) - np.testing.assert_allclose(quat, gt_quat, atol=1e-3) - - -def test_resolve_prim_scale(): - """Test resolve_prim_scale() function. - - To simplify the test, we assume that the effective scale at a prim - is the product of the scales of the prims in the hierarchy: - - scale = scale_of_xform * scale_of_geometry_prim - - This is only true when rotations are identity or the transforms are - orthogonal and uniformly scaled. Otherwise, scale is not composable - like that in local component-wise fashion. - """ - # number of objects - num_objects = 20 - # sample random scales for x, y, z - rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) - rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) - # sample random positions - rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) - - # create objects - for i in range(num_objects): - # simple cubes - cube_prim = sim_utils.create_prim( - f"/World/Cubes/instance_{i:02d}", - "Cube", - translation=rand_positions[i, 0], - scale=rand_scales[i, 0], - attributes={"size": rand_widths[i]}, - ) - # xform hierarchy - xform_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}", - "Xform", - translation=rand_positions[i, 1], - scale=rand_scales[i, 1], - ) - geometry_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/geometry", - "Sphere", - translation=rand_positions[i, 2], - scale=rand_scales[i, 2], - attributes={"radius": rand_widths[i]}, - ) - dummy_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/dummy", - "Sphere", - ) - - # cube prim - scale = sim_utils.resolve_prim_scale(cube_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 0], atol=1e-5) - # xform prim - scale = sim_utils.resolve_prim_scale(xform_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) - # geometry prim - scale = sim_utils.resolve_prim_scale(geometry_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 1] * rand_scales[i, 2], atol=1e-5) - # dummy prim - scale = sim_utils.resolve_prim_scale(dummy_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) - - """ USD references and variants. """ diff --git a/source/isaaclab/test/sim/test_utils_xform.py b/source/isaaclab/test/sim/test_utils_transforms.py similarity index 78% rename from source/isaaclab/test/sim/test_utils_xform.py rename to source/isaaclab/test/sim/test_utils_transforms.py index dc21409cb3c..6fa92ce0599 100644 --- a/source/isaaclab/test/sim/test_utils_xform.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -13,11 +13,14 @@ """Rest everything follows.""" import math +import numpy as np +import torch import pytest from pxr import Gf, Sdf, Usd, UsdGeom import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils @pytest.fixture(autouse=True) @@ -703,3 +706,165 @@ def test_standardize_xform_ops_performance_batch(): # Verify operation is reasonably fast assert avg_ms < 0.1, f"Average operation took {avg_ms:.2f}ms/prim, expected < 0.1ms/prim" + + +def test_resolve_prim_pose(): + """Test resolve_prim_pose() function.""" + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + # sample random rotations + rand_quats = np.random.randn(num_objects, 3, 4) + rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + orientation=rand_quats[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + orientation=rand_quats[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + orientation=rand_quats[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(cube_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) + # xform prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + # dummy prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(dummy_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + + # geometry prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) + # TODO: Enabling scale causes the test to fail because the current implementation of + # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known + # limitation. Until this is fixed, the test is disabled here to ensure the test passes. + # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) + + # dummy prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) + np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) + # xform prim w.r.t. cube prim + pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) + pos, quat = np.array(pos), np.array(quat) + # -- compute ground truth values + gt_pos, gt_quat = math_utils.subtract_frame_transforms( + torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), + torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), + ) + gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() + quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, gt_pos, atol=1e-3) + np.testing.assert_allclose(quat, gt_quat, atol=1e-3) + + +def test_resolve_prim_scale(): + """Test resolve_prim_scale() function. + + To simplify the test, we assume that the effective scale at a prim + is the product of the scales of the prims in the hierarchy: + + scale = scale_of_xform * scale_of_geometry_prim + + This is only true when rotations are identity or the transforms are + orthogonal and uniformly scaled. Otherwise, scale is not composable + like that in local component-wise fashion. + """ + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim + scale = sim_utils.resolve_prim_scale(cube_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 0], atol=1e-5) + # xform prim + scale = sim_utils.resolve_prim_scale(xform_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + # geometry prim + scale = sim_utils.resolve_prim_scale(geometry_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1] * rand_scales[i, 2], atol=1e-5) + # dummy prim + scale = sim_utils.resolve_prim_scale(dummy_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) From a10e419558dd419362a35ed15d6492ef55432dd1 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 12:02:33 +0100 Subject: [PATCH 008/100] adds api docs --- docs/source/api/lab/isaaclab.sim.utils.rst | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/docs/source/api/lab/isaaclab.sim.utils.rst b/docs/source/api/lab/isaaclab.sim.utils.rst index 9d59df77bcc..f27e574efb9 100644 --- a/docs/source/api/lab/isaaclab.sim.utils.rst +++ b/docs/source/api/lab/isaaclab.sim.utils.rst @@ -10,6 +10,7 @@ stage queries prims + transforms semantics legacy @@ -34,6 +35,13 @@ Prims :members: :show-inheritance: +Transforms +---------- + +.. automodule:: isaaclab.sim.utils.transforms + :members: + :show-inheritance: + Semantics --------- From a1a714d89b313e39c1c8a9e48978415d2cf30368 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 12:57:07 +0100 Subject: [PATCH 009/100] makes a function to abstract out convert world pose call --- source/isaaclab/isaaclab/sim/utils/prims.py | 36 +-- .../isaaclab/isaaclab/sim/utils/transforms.py | 93 +++++++ .../test/sim/test_utils_transforms.py | 230 ++++++++++++++++++ 3 files changed, 329 insertions(+), 30 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 5e017481cd1..94e0ef62093 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -28,7 +28,7 @@ from .queries import find_matching_prim_paths from .semantics import add_labels from .stage import attach_stage_to_usd_context, get_current_stage, get_current_stage_id -from .xform import standardize_xform_ops +from .transforms import convert_world_pose_to_local, standardize_xform_ops if TYPE_CHECKING: from isaaclab.sim.spawners.spawner_cfg import SpawnerCfg @@ -160,36 +160,12 @@ def create_prim( # convert position and orientation to translation and orientation # world --> local if position is not None: + # convert position to tuple + position = tuple(position) + # convert orientation to tuple + orientation = tuple(orientation) if orientation is not None else None # this means that user provided pose in the world frame - # obtain parent transform - parent_prim = prim.GetParent() - if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: - # Get parent's world transform - parent_xformable = UsdGeom.Xformable(parent_prim) - parent_world_tf = parent_xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - - # Create world transform for the desired position and orientation - desired_world_tf = Gf.Matrix4d() - desired_world_tf.SetTranslateOnly(Gf.Vec3d(*position)) - - if orientation is not None: - # Set rotation from quaternion (w, x, y, z) - quat = Gf.Quatd(*orientation) - desired_world_tf.SetRotateOnly(quat) - - # Convert world transform to local: local = inv(parent_world) * world - parent_world_tf_inv = parent_world_tf.GetInverse() - local_tf = desired_world_tf * parent_world_tf_inv - - # Extract local translation and orientation - local_transform = Gf.Transform(local_tf) - translation = tuple(local_transform.GetTranslation()) - if orientation is not None: - quat_result = local_transform.GetRotation().GetQuat() - orientation = (quat_result.GetReal(), *quat_result.GetImaginary()) - else: - # No parent or parent is root, position is already in local space - translation = position + translation, orientation = convert_world_pose_to_local(position, orientation, ref_prim=prim.GetParent()) # Convert sequences to properly-typed tuples for standardize_xform_ops translation_tuple = None if translation is None else tuple(translation) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 171e06f70c5..1df8c9c58ee 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -294,3 +294,96 @@ def resolve_prim_scale(prim: Usd.Prim) -> tuple[float, float, float]: world_transform = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) # extract scale return tuple([*(v.GetLength() for v in world_transform.ExtractRotationMatrix())]) + + +def convert_world_pose_to_local( + position: tuple[float, ...], + orientation: tuple[float, ...] | None, + ref_prim: Usd.Prim, +) -> tuple[tuple[float, float, float], tuple[float, float, float, float] | None]: + """Convert a world-space pose to local-space pose relative to a reference prim. + + This function takes a position and orientation in world space and converts them to local space + relative to the given reference prim. This is useful when creating or positioning prims where you + know the desired world position but need to set local transform attributes relative to another prim. + + The conversion uses the standard USD transformation math: + ``local_transform = world_transform * inverse(ref_world_transform)`` + + .. note:: + If the reference prim is invalid or is the root path, the position and orientation are returned + unchanged, as they are already effectively in local/world space. + + Args: + position: The world-space position as (x, y, z). + orientation: The world-space orientation as quaternion (w, x, y, z). If None, only position is converted + and None is returned for orientation. + ref_prim: The reference USD prim to compute the local transform relative to. If this is invalid or + is the root path, the world pose is returned unchanged. + + Returns: + A tuple of (local_translation, local_orientation) where: + + - local_translation is a tuple of (x, y, z) in local space relative to ref_prim + - local_orientation is a tuple of (w, x, y, z) in local space relative to ref_prim, or None if no orientation was provided + + Raises: + ValueError: If the reference prim is not a valid USD prim. + ValueError: If the reference prim is not a valid USD Xformable. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Usd, UsdGeom + >>> + >>> # Get reference prim + >>> stage = sim_utils.get_current_stage() + >>> ref_prim = stage.GetPrimAtPath("/World/Reference") + >>> + >>> # Convert world pose to local (relative to ref_prim) + >>> world_pos = (10.0, 5.0, 0.0) + >>> world_quat = (1.0, 0.0, 0.0, 0.0) # identity rotation + >>> local_pos, local_quat = sim_utils.convert_world_pose_to_local( + ... world_pos, world_quat, ref_prim + ... ) + >>> print(f"Local position: {local_pos}") + >>> print(f"Local orientation: {local_quat}") + """ + # Check if prim is valid + if not ref_prim.IsValid(): + raise ValueError(f"Reference prim at path '{ref_prim.GetPath().pathString}' is not valid.") + + # If reference prim is the root, return world pose as-is + if ref_prim.GetPath() == Sdf.Path.absoluteRootPath: + return position, orientation # type: ignore + + # Check if reference prim is a valid xformable + ref_xformable = UsdGeom.Xformable(ref_prim) + if not ref_xformable: + raise ValueError(f"Reference prim at path '{ref_prim.GetPath().pathString}' is not a valid xformable.") + + # Get reference prim's world transform + ref_world_tf = ref_xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + + # Create world transform for the desired position and orientation + desired_world_tf = Gf.Matrix4d() + desired_world_tf.SetTranslateOnly(Gf.Vec3d(*position)) + + if orientation is not None: + # Set rotation from quaternion (w, x, y, z) + quat = Gf.Quatd(*orientation) + desired_world_tf.SetRotateOnly(quat) + + # Convert world transform to local: local = world * inv(ref_world) + ref_world_tf_inv = ref_world_tf.GetInverse() + local_tf = desired_world_tf * ref_world_tf_inv + + # Extract local translation and orientation + local_transform = Gf.Transform(local_tf) + local_translation = tuple(local_transform.GetTranslation()) + + local_orientation = None + if orientation is not None: + quat_result = local_transform.GetRotation().GetQuat() + local_orientation = (quat_result.GetReal(), *quat_result.GetImaginary()) + + return local_translation, local_orientation diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 6fa92ce0599..7a1a8cf5037 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -668,6 +668,236 @@ def test_standardize_xform_ops_with_complex_hierarchy(): assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) +def test_convert_world_pose_to_local_basic(): + """Test basic world-to-local pose conversion.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent and child prims + parent_prim = sim_utils.create_prim( + "/World/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), # identity rotation + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # World pose we want to achieve for a child + world_position = (10.0, 3.0, 0.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + # Assert orientation is not None + assert local_orientation is not None + + # The expected local translation is world_position - parent_position = (10-5, 3-0, 0-0) = (5, 3, 0) + assert_vec3_close(Gf.Vec3d(*local_translation), (5.0, 3.0, 0.0), eps=1e-5) + assert_quat_close(Gf.Quatd(*local_orientation), (1.0, 0.0, 0.0, 0.0), eps=1e-5) + + +def test_convert_world_pose_to_local_with_rotation(): + """Test world-to-local conversion with parent rotation.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent with 90-degree rotation around Z axis + parent_prim = sim_utils.create_prim( + "/World/RotatedParent", + "Xform", + translation=(0.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # World pose: position at (1, 0, 0) with identity rotation + world_position = (1.0, 0.0, 0.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + + # Create a child with the local transform and verify world pose + child_prim = sim_utils.create_prim( + "/World/RotatedParent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Get world pose of child + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) + + # Verify it matches the desired world pose + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-5) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-5) + + +def test_convert_world_pose_to_local_with_scale(): + """Test world-to-local conversion with parent scale.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent with non-uniform scale + parent_prim = sim_utils.create_prim( + "/World/ScaledParent", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # World pose we want + world_position = (5.0, 6.0, 7.0) + world_orientation = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + + # Create child and verify + child_prim = sim_utils.create_prim( + "/World/ScaledParent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Get world pose + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) + + # Verify (may have some tolerance due to scale effects on rotation) + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) + + +def test_convert_world_pose_to_local_invalid_parent(): + """Test world-to-local conversion with invalid parent returns world pose unchanged.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + assert not invalid_prim.IsValid() + + world_position = (10.0, 20.0, 30.0) + world_orientation = (0.7071068, 0.0, 0.7071068, 0.0) + + # Convert with invalid reference prim + with pytest.raises(ValueError): + sim_utils.convert_world_pose_to_local(world_position, world_orientation, invalid_prim) + + +def test_convert_world_pose_to_local_root_parent(): + """Test world-to-local conversion with root as parent returns world pose unchanged.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get the pseudo-root prim + root_prim = stage.GetPrimAtPath("/") + + world_position = (15.0, 25.0, 35.0) + world_orientation = (0.9238795, 0.3826834, 0.0, 0.0) + + # Convert with root parent + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, root_prim + ) + # Assert orientation is not None + assert local_orientation is not None + + # Should return unchanged + assert_vec3_close(Gf.Vec3d(*local_translation), world_position, eps=1e-10) + assert_quat_close(Gf.Quatd(*local_orientation), world_orientation, eps=1e-10) + + +def test_convert_world_pose_to_local_none_orientation(): + """Test world-to-local conversion with None orientation.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent + parent_prim = sim_utils.create_prim( + "/World/ParentNoOrient", + "Xform", + translation=(3.0, 4.0, 5.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + stage=stage, + ) + + world_position = (10.0, 10.0, 10.0) + + # Convert with None orientation + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, None, parent_prim + ) + + # Orientation should be None + assert local_orientation is None + # Translation should still be converted + assert local_translation is not None + + +def test_convert_world_pose_to_local_complex_hierarchy(): + """Test world-to-local conversion in a complex hierarchy.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a complex hierarchy + grandparent = sim_utils.create_prim( + "/World/Grandparent", + "Xform", + translation=(10.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + parent = sim_utils.create_prim( + "/World/Grandparent/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), # local to grandparent + orientation=(0.7071068, 0.7071068, 0.0, 0.0), # 90 deg around X + scale=(0.5, 0.5, 0.5), + stage=stage, + ) + + # World pose we want to achieve + world_position = (20.0, 15.0, 10.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) + + # Convert to local space relative to parent + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent + ) + + # Create child with the computed local transform + child = sim_utils.create_prim( + "/World/Grandparent/Parent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Verify world pose + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) + + # Should match the desired world pose (with some tolerance for complex transforms) + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) + + """ Performance Benchmarking Tests """ From d347aef0380ba226d2e1e9f8363822b61e1a0250 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 13:05:30 +0100 Subject: [PATCH 010/100] runs formatter --- source/isaaclab/isaaclab/sim/utils/prims.py | 2 +- source/isaaclab/test/sim/test_utils_transforms.py | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 94e0ef62093..b0114b1b215 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -21,7 +21,7 @@ from isaacsim.core.cloner import Cloner from isaacsim.core.version import get_version from omni.usd.commands import DeletePrimsCommand, MovePrimCommand -from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade from isaaclab.utils.string import to_camel_case diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 7a1a8cf5037..2890acc6123 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -838,9 +838,7 @@ def test_convert_world_pose_to_local_none_orientation(): world_position = (10.0, 10.0, 10.0) # Convert with None orientation - local_translation, local_orientation = sim_utils.convert_world_pose_to_local( - world_position, None, parent_prim - ) + local_translation, local_orientation = sim_utils.convert_world_pose_to_local(world_position, None, parent_prim) # Orientation should be None assert local_orientation is None From 9929597417a99dad904c3a2d035908e5dee4358d Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 21:02:15 +0100 Subject: [PATCH 011/100] adds test for different backend data types to create prim --- source/isaaclab/isaaclab/sim/utils/prims.py | 61 +++++++-- source/isaaclab/test/sim/test_utils_prims.py | 133 +++++++++++++++++++ 2 files changed, 180 insertions(+), 14 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index b0114b1b215..9696f6af061 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -45,10 +45,10 @@ def create_prim( prim_path: str, prim_type: str = "Xform", - position: Sequence[float] | None = None, - translation: Sequence[float] | None = None, - orientation: Sequence[float] | None = None, - scale: Sequence[float] | None = None, + position: Any | None = None, + translation: Any | None = None, + orientation: Any | None = None, + scale: Any | None = None, usd_path: str | None = None, semantic_label: str | None = None, semantic_type: str = "class", @@ -68,6 +68,9 @@ def create_prim( The scale is always applied in the local frame. + The function handles various sequence types (list, tuple, numpy array, torch tensor) + and converts them to properly-typed tuples for operations on the prim. + .. note:: Transform operations are standardized to the USD convention: translate, orient (quaternion), and scale, in that order. See :func:`standardize_xform_ops` for more details. @@ -157,23 +160,20 @@ def create_prim( if semantic_label is not None: add_labels(prim, labels=[semantic_label], instance_name=semantic_type) + # convert input arguments to tuples + position = _to_tuple(position) if position is not None else None + translation = _to_tuple(translation) if translation is not None else None + orientation = _to_tuple(orientation) if orientation is not None else None + scale = _to_tuple(scale) if scale is not None else None + # convert position and orientation to translation and orientation # world --> local if position is not None: - # convert position to tuple - position = tuple(position) - # convert orientation to tuple - orientation = tuple(orientation) if orientation is not None else None # this means that user provided pose in the world frame translation, orientation = convert_world_pose_to_local(position, orientation, ref_prim=prim.GetParent()) - # Convert sequences to properly-typed tuples for standardize_xform_ops - translation_tuple = None if translation is None else tuple(translation) - orientation_tuple = None if orientation is None else tuple(orientation) - scale_tuple = None if scale is None else tuple(scale) - # standardize the xform ops - standardize_xform_ops(prim, translation_tuple, orientation_tuple, scale_tuple) + standardize_xform_ops(prim, translation, orientation, scale) return prim @@ -960,3 +960,36 @@ class TableVariants: f"Setting variant selection '{variant_selection}' for variant set '{variant_set_name}' on" f" prim '{prim_path}'." ) + + +""" +Internal Helpers. +""" + + +def _to_tuple(value: Any) -> tuple[float, ...]: + """Convert various sequence types (list, tuple, numpy array, torch tensor) to tuple. + + This function handles conversion from different array-like types to Python tuples. + It validates dimensionality and automatically moves CUDA tensors to CPU if necessary. + + Args: + value: A sequence-like object containing floats. It can be a list, tuple, + numpy array, or a torch tensor. + + Returns: + A tuple of floats. + + Raises: + ValueError: If the input value is not one dimensional. + """ + # check if it is a torch tensor or numpy array (both have tolist()) + if hasattr(value, "tolist"): + # ensure that it is one dimensional + if hasattr(value, "ndim") and value.ndim != 1: + raise ValueError(f"Input value is not one dimensional: {value.shape}") + + return tuple(value.tolist()) # type: ignore + else: + # otherwise assume it is already a sequence (list, tuple, etc.) + return tuple(value) diff --git a/source/isaaclab/test/sim/test_utils_prims.py b/source/isaaclab/test/sim/test_utils_prims.py index 54c04afe7e9..d8ac9c474ac 100644 --- a/source/isaaclab/test/sim/test_utils_prims.py +++ b/source/isaaclab/test/sim/test_utils_prims.py @@ -14,6 +14,8 @@ """Rest everything follows.""" import math +import numpy as np +import torch import pytest from pxr import Gf, Sdf, Usd, UsdGeom @@ -115,6 +117,137 @@ def test_create_prim(): assert op_names == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] +@pytest.mark.parametrize( + "input_type", + ["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], + ids=["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], +) +def test_create_prim_with_different_input_types(input_type: str): + """Test create_prim() with different input types (list, tuple, numpy array, torch tensor).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Define test values + translation_vals = [1.0, 2.0, 3.0] + orientation_vals = [1.0, 0.0, 0.0, 0.0] # w, x, y, z + scale_vals = [2.0, 3.0, 4.0] + + # Convert to the specified input type + if input_type == "list": + translation = translation_vals + orientation = orientation_vals + scale = scale_vals + elif input_type == "tuple": + translation = tuple(translation_vals) + orientation = tuple(orientation_vals) + scale = tuple(scale_vals) + elif input_type == "numpy": + translation = np.array(translation_vals) + orientation = np.array(orientation_vals) + scale = np.array(scale_vals) + elif input_type == "torch_cpu": + translation = torch.tensor(translation_vals) + orientation = torch.tensor(orientation_vals) + scale = torch.tensor(scale_vals) + elif input_type == "torch_cuda": + if not torch.cuda.is_available(): + pytest.skip("CUDA not available") + translation = torch.tensor(translation_vals, device="cuda") + orientation = torch.tensor(orientation_vals, device="cuda") + scale = torch.tensor(scale_vals, device="cuda") + + # Create prim with translation (local space) + prim = sim_utils.create_prim( + f"/World/Test/Xform_{input_type}", + "Xform", + stage=stage, + translation=translation, + orientation=orientation, + scale=scale, + ) + + # Verify prim was created correctly + assert prim.IsValid() + assert prim.GetPrimPath() == f"/World/Test/Xform_{input_type}" + + # Verify transform values + assert prim.GetAttribute("xformOp:translate").Get() == Gf.Vec3d(*translation_vals) + assert_quat_close(prim.GetAttribute("xformOp:orient").Get(), Gf.Quatd(*orientation_vals)) + assert prim.GetAttribute("xformOp:scale").Get() == Gf.Vec3d(*scale_vals) + + # Verify xform operation order + op_names = [op.GetOpName() for op in UsdGeom.Xformable(prim).GetOrderedXformOps()] + assert op_names == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + +@pytest.mark.parametrize( + "input_type", + ["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], + ids=["list", "tuple", "numpy", "torch_cpu", "torch_cuda"], +) +def test_create_prim_with_world_position_different_types(input_type: str): + """Test create_prim() with world position using different input types.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a parent prim + _ = sim_utils.create_prim( + "/World/Parent", + "Xform", + stage=stage, + translation=(5.0, 10.0, 15.0), + orientation=(1.0, 0.0, 0.0, 0.0), + ) + + # Define world position and orientation values + world_pos_vals = [10.0, 20.0, 30.0] + world_orient_vals = [0.7071068, 0.0, 0.7071068, 0.0] # 90 deg around Y + + # Convert to the specified input type + if input_type == "list": + world_pos = world_pos_vals + world_orient = world_orient_vals + elif input_type == "tuple": + world_pos = tuple(world_pos_vals) + world_orient = tuple(world_orient_vals) + elif input_type == "numpy": + world_pos = np.array(world_pos_vals) + world_orient = np.array(world_orient_vals) + elif input_type == "torch_cpu": + world_pos = torch.tensor(world_pos_vals) + world_orient = torch.tensor(world_orient_vals) + elif input_type == "torch_cuda": + if not torch.cuda.is_available(): + pytest.skip("CUDA not available") + world_pos = torch.tensor(world_pos_vals, device="cuda") + world_orient = torch.tensor(world_orient_vals, device="cuda") + + # Create child prim with world position + child = sim_utils.create_prim( + f"/World/Parent/Child_{input_type}", + "Xform", + stage=stage, + position=world_pos, # Using position (world space) + orientation=world_orient, + ) + + # Verify prim was created + assert child.IsValid() + + # Verify world pose matches what we specified + world_pose = sim_utils.resolve_prim_pose(child) + pos_result, quat_result = world_pose + + # Check position (should be close to world_pos_vals) + for i in range(3): + assert math.isclose(pos_result[i], world_pos_vals[i], abs_tol=1e-4) + + # Check orientation (quaternions may have sign flipped) + quat_match = all(math.isclose(quat_result[i], world_orient_vals[i], abs_tol=1e-4) for i in range(4)) + quat_match_neg = all(math.isclose(quat_result[i], -world_orient_vals[i], abs_tol=1e-4) for i in range(4)) + assert quat_match or quat_match_neg + + def test_delete_prim(): """Test delete_prim() function.""" # obtain stage handle From 263054df71f819387471b326333f434fde810004 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 21:13:34 +0100 Subject: [PATCH 012/100] adds test for get usd references --- source/isaaclab/isaaclab/sim/utils/prims.py | 3 ++- source/isaaclab/test/sim/test_utils_prims.py | 24 ++++++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 9696f6af061..04c2cfc1ba1 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -883,7 +883,8 @@ def get_usd_references(prim_path: str, stage: Usd.Stage | None = None) -> list[s # get USD references references = [] for prim_spec in prim.GetPrimStack(): - references.extend(prim_spec.referenceList.prependedItems.assetPath) + for ref in prim_spec.referenceList.prependedItems: + references.append(str(ref.assetPath)) return references diff --git a/source/isaaclab/test/sim/test_utils_prims.py b/source/isaaclab/test/sim/test_utils_prims.py index d8ac9c474ac..512048d115c 100644 --- a/source/isaaclab/test/sim/test_utils_prims.py +++ b/source/isaaclab/test/sim/test_utils_prims.py @@ -323,6 +323,30 @@ def test_move_prim(): """ +def test_get_usd_references(): + """Test get_usd_references() function.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim without USD reference + sim_utils.create_prim("/World/NoReference", "Xform", stage=stage) + # Check that it has no references + refs = sim_utils.get_usd_references("/World/NoReference", stage=stage) + assert len(refs) == 0 + + # Create a prim with a USD reference + franka_usd = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd" + sim_utils.create_prim("/World/WithReference", usd_path=franka_usd, stage=stage) + # Check that it has the expected reference + refs = sim_utils.get_usd_references("/World/WithReference", stage=stage) + assert len(refs) == 1 + assert refs == [franka_usd] + + # Test with invalid prim path + with pytest.raises(ValueError, match="not valid"): + sim_utils.get_usd_references("/World/NonExistent", stage=stage) + + def test_select_usd_variants(): """Test select_usd_variants() function.""" stage = sim_utils.get_current_stage() From 53a8ddb02f8c86e8fa5e0fc05940d9c1bf659f9c Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 21:14:07 +0100 Subject: [PATCH 013/100] adds error code --- source/isaaclab/isaaclab/sim/utils/prims.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 04c2cfc1ba1..7da3412b62b 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -873,6 +873,9 @@ def get_usd_references(prim_path: str, stage: Usd.Stage | None = None) -> list[s Returns: A list of USD reference paths. + + Raises: + ValueError: If the prim at the specified path is not valid. """ # get stage handle stage = get_current_stage() if stage is None else stage From 590ff75fc4b51a205087a603f7b620c4496447c8 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 21:19:16 +0100 Subject: [PATCH 014/100] adds example for all transform calls --- .../isaaclab/isaaclab/sim/utils/transforms.py | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 1df8c9c58ee..b4eeb47d782 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -237,6 +237,25 @@ def resolve_prim_pose( Raises: ValueError: If the prim or ref prim is not valid. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Usd, UsdGeom + >>> + >>> # Get prim + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/ImportedAsset") + >>> + >>> # Resolve pose + >>> pos, quat = sim_utils.resolve_prim_pose(prim) + >>> print(f"Position: {pos}") + >>> print(f"Orientation: {quat}") + >>> + >>> # Resolve pose with respect to another prim + >>> ref_prim = stage.GetPrimAtPath("/World/Reference") + >>> pos, quat = sim_utils.resolve_prim_pose(prim, ref_prim) + >>> print(f"Position: {pos}") + >>> print(f"Orientation: {quat}") """ # check if prim is valid if not prim.IsValid(): @@ -285,6 +304,18 @@ def resolve_prim_scale(prim: Usd.Prim) -> tuple[float, float, float]: Raises: ValueError: If the prim is not valid. + + Example: + >>> import isaaclab.sim as sim_utils + >>> from pxr import Usd, UsdGeom + >>> + >>> # Get prim + >>> stage = sim_utils.get_current_stage() + >>> prim = stage.GetPrimAtPath("/World/ImportedAsset") + >>> + >>> # Resolve scale + >>> scale = sim_utils.resolve_prim_scale(prim) + >>> print(f"Scale: {scale}") """ # check if prim is valid if not prim.IsValid(): From 8e5153fbeefc80165291e87579ff78415b8c92e7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 21:30:51 +0100 Subject: [PATCH 015/100] runs formatter --- source/isaaclab/test/sim/test_utils_transforms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 2890acc6123..933900a02ed 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -852,7 +852,7 @@ def test_convert_world_pose_to_local_complex_hierarchy(): stage = sim_utils.get_current_stage() # Create a complex hierarchy - grandparent = sim_utils.create_prim( + _ = sim_utils.create_prim( "/World/Grandparent", "Xform", translation=(10.0, 0.0, 0.0), From 17d6936d798ee176b373e3d8723fbde79da19d69 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 02:11:30 +0100 Subject: [PATCH 016/100] fixes type issues and adds more tests for to_tuple --- source/isaaclab/isaaclab/sim/utils/prims.py | 60 ++++++++++----- .../isaaclab/isaaclab/sim/utils/transforms.py | 19 +++-- source/isaaclab/test/sim/test_utils_prims.py | 76 +++++++++++++++++++ 3 files changed, 129 insertions(+), 26 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 7da3412b62b..d8775e15e2e 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 @@ -11,6 +11,7 @@ import inspect import logging import re +import torch from collections.abc import Callable, Sequence from typing import TYPE_CHECKING, Any @@ -972,28 +973,51 @@ class TableVariants: def _to_tuple(value: Any) -> tuple[float, ...]: - """Convert various sequence types (list, tuple, numpy array, torch tensor) to tuple. + """Convert various sequence types to a Python tuple of floats. - This function handles conversion from different array-like types to Python tuples. - It validates dimensionality and automatically moves CUDA tensors to CPU if necessary. + This function provides robust conversion from different array-like types (list, tuple, numpy array, + torch tensor) to Python tuples. It handles edge cases like malformed sequences, CUDA tensors, + and arrays with singleton dimensions. Args: - value: A sequence-like object containing floats. It can be a list, tuple, - numpy array, or a torch tensor. + value: A sequence-like object containing floats. Supported types include: + - Python list or tuple + - NumPy array (any device) + - PyTorch tensor (CPU or CUDA) + - Mixed sequences with numpy/torch scalar items and float values Returns: - A tuple of floats. + A one-dimensional tuple of floats. Raises: - ValueError: If the input value is not one dimensional. - """ - # check if it is a torch tensor or numpy array (both have tolist()) - if hasattr(value, "tolist"): - # ensure that it is one dimensional - if hasattr(value, "ndim") and value.ndim != 1: - raise ValueError(f"Input value is not one dimensional: {value.shape}") + ValueError: If the input value is not one-dimensional after squeezing singleton dimensions. - return tuple(value.tolist()) # type: ignore - else: - # otherwise assume it is already a sequence (list, tuple, etc.) - return tuple(value) + Example: + >>> import torch + >>> import numpy as np + >>> + >>> _to_tuple([1.0, 2.0, 3.0]) + (1.0, 2.0, 3.0) + >>> _to_tuple(torch.tensor([[1.0, 2.0]])) # Squeezes first dimension + (1.0, 2.0) + >>> _to_tuple(np.array([1.0, 2.0, 3.0])) + (1.0, 2.0, 3.0) + >>> _to_tuple((1.0, 2.0, 3.0)) + (1.0, 2.0, 3.0) + + """ + # Normalize to tensor if value is a plain sequence (list with mixed types, etc.) + # This handles cases like [np.float32(1.0), 2.0, torch.tensor(3.0)] + if not hasattr(value, "tolist"): + value = torch.tensor(value, device="cpu", dtype=torch.float) + + # Remove leading singleton dimension if present (e.g., shape (1, 3) -> (3,)) + # This is common when batched operations produce single-item batches + if value.ndim != 1: + value = value.squeeze() + # Validate that the result is one-dimensional + if value.ndim != 1: + raise ValueError(f"Input value is not one dimensional: {value.shape}") + + # Convert to tuple - works for both numpy arrays and torch tensors + return tuple(value.tolist()) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index b4eeb47d782..010425ccdfe 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -152,7 +152,7 @@ def standardize_xform_ops( # Handle scale resolution if scale is not None: # User provided scale - xform_scale = scale + xform_scale = Gf.Vec3d(scale) elif "xformOp:scale" in prop_names: # Handle unit resolution for scale if present # This occurs when assets are imported with different unit scales @@ -161,8 +161,6 @@ def standardize_xform_ops( units_resolve = prim.GetAttribute("xformOp:scale:unitsResolve").Get() for i in range(3): xform_scale[i] = xform_scale[i] * units_resolve[i] - # Convert to tuple - xform_scale = tuple(xform_scale) else: # No scale exists, use default uniform scale xform_scale = Gf.Vec3d(1.0, 1.0, 1.0) @@ -197,11 +195,16 @@ def standardize_xform_ops( if not xform_op_orient: xform_op_orient = xformable.AddXformOp(UsdGeom.XformOp.TypeOrient, UsdGeom.XformOp.PrecisionDouble, "") - # Set the transform values using the new standardized transform operations - # Convert tuples to Gf types for USD - xform_op_translate.Set(xform_pos) - xform_op_orient.Set(xform_quat) - xform_op_scale.Set(xform_scale) + # Handle different floating point precisions + # Existing Xform operations might have floating or double precision. + # We need to cast the data to the correct type to avoid setting the wrong type. + xform_ops = [xform_op_translate, xform_op_orient, xform_op_scale] + xform_values = [xform_pos, xform_quat, xform_scale] + for xform_op, value in zip(xform_ops, xform_values): + # Get current value to determine precision type + current_value = xform_op.Get() + # Cast to existing type to preserve precision (float/double) + xform_op.Set(type(current_value)(value) if current_value is not None else value) # Set the transform operation order: translate -> orient -> scale # This is the standard USD convention and ensures consistent behavior diff --git a/source/isaaclab/test/sim/test_utils_prims.py b/source/isaaclab/test/sim/test_utils_prims.py index 512048d115c..4aed743ebb4 100644 --- a/source/isaaclab/test/sim/test_utils_prims.py +++ b/source/isaaclab/test/sim/test_utils_prims.py @@ -21,6 +21,7 @@ from pxr import Gf, Sdf, Usd, UsdGeom import isaaclab.sim as sim_utils +from isaaclab.sim.utils.prims import _to_tuple # type: ignore[reportPrivateUsage] from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR @@ -364,3 +365,78 @@ def test_select_usd_variants(): # Check if the variant selection is correct assert variant_set.GetVariantSelection() == "red" + + +""" +Internal Helpers. +""" + + +def test_to_tuple_basic(): + """Test _to_tuple() with basic input types.""" + # Test with list + result = _to_tuple([1.0, 2.0, 3.0]) + assert result == (1.0, 2.0, 3.0) + assert isinstance(result, tuple) + + # Test with tuple + result = _to_tuple((1.0, 2.0, 3.0)) + assert result == (1.0, 2.0, 3.0) + + # Test with numpy array + result = _to_tuple(np.array([1.0, 2.0, 3.0])) + assert result == (1.0, 2.0, 3.0) + + # Test with torch tensor (CPU) + result = _to_tuple(torch.tensor([1.0, 2.0, 3.0])) + assert result == (1.0, 2.0, 3.0) + + # Test squeezing first dimension (batch size 1) + result = _to_tuple(torch.tensor([[1.0, 2.0]])) + assert result == (1.0, 2.0) + + result = _to_tuple(np.array([[1.0, 2.0, 3.0]])) + assert result == (1.0, 2.0, 3.0) + + +def test_to_tuple_raises_error(): + """Test _to_tuple() raises an error for N-dimensional arrays.""" + + with pytest.raises(ValueError, match="not one dimensional"): + _to_tuple(np.array([[1.0, 2.0], [3.0, 4.0]])) + + with pytest.raises(ValueError, match="not one dimensional"): + _to_tuple(torch.tensor([[[1.0, 2.0]], [[3.0, 4.0]]])) + + with pytest.raises(ValueError, match="only one element tensors can be converted"): + _to_tuple((torch.tensor([1.0, 2.0]), 3.0)) + + +def test_to_tuple_mixed_sequences(): + """Test _to_tuple() with mixed type sequences.""" + + # Mixed list with numpy and floats + result = _to_tuple([np.float32(1.0), 2.0, 3.0]) + assert len(result) == 3 + assert all(isinstance(x, float) for x in result) + + # Mixed tuple with torch tensor items and floats + result = _to_tuple([torch.tensor(1.0), 2.0, 3.0]) + assert result == (1.0, 2.0, 3.0) + + # Mixed tuple with numpy array items and torch tensor + result = _to_tuple((np.float32(1.0), 2.0, torch.tensor(3.0))) + assert result == (1.0, 2.0, 3.0) + + +def test_to_tuple_precision(): + """Test _to_tuple() maintains numerical precision.""" + from isaaclab.sim.utils.prims import _to_tuple + + # Test with high precision values + high_precision = [1.123456789, 2.987654321, 3.141592653] + result = _to_tuple(torch.tensor(high_precision, dtype=torch.float64)) + + # Check that precision is maintained reasonably well + for i, val in enumerate(high_precision): + assert math.isclose(result[i], val, abs_tol=1e-6) From 62361ac44ca57078e0a68874453ad0218da95702 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 02:16:27 +0100 Subject: [PATCH 017/100] adds test for flaot precision in standardize --- .../isaaclab/isaaclab/sim/utils/transforms.py | 2 +- .../test/sim/test_utils_transforms.py | 65 ++++++++++++++++++- 2 files changed, 65 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 010425ccdfe..1bad98104c9 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 933900a02ed..df942f3aa26 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 @@ -668,6 +668,69 @@ def test_standardize_xform_ops_with_complex_hierarchy(): assert_quat_close(Gf.Quatd(*quat_before), quat_after, eps=1e-5) +def test_standardize_xform_ops_preserves_float_precision(): + """Test that standardize_xform_ops preserves float precision when it already exists.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim manually with FLOAT precision operations (not double) + prim_path = "/World/TestFloatPrecision" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add xform operations with FLOAT precision (not the default double) + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionFloat) + translate_op.Set(Gf.Vec3f(1.0, 2.0, 3.0)) + + orient_op = xformable.AddOrientOp(UsdGeom.XformOp.PrecisionFloat) + orient_op.Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0)) + + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionFloat) + scale_op.Set(Gf.Vec3f(1.0, 1.0, 1.0)) + + # Verify operations exist with float precision + assert translate_op.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert orient_op.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert scale_op.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + + # Now apply standardize_xform_ops with new values (provided as double precision Python floats) + new_translation = (5.0, 10.0, 15.0) + new_orientation = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + new_scale = (2.0, 3.0, 4.0) + + result = sim_utils.standardize_xform_ops( + prim, translation=new_translation, orientation=new_orientation, scale=new_scale + ) + assert result is True + + # Verify the precision is STILL float (not converted to double) + translate_op_after = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + orient_op_after = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + scale_op_after = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) + + assert translate_op_after.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert orient_op_after.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + assert scale_op_after.GetPrecision() == UsdGeom.XformOp.PrecisionFloat + + # Verify the VALUES are set correctly (cast to float, so they're Gf.Vec3f and Gf.Quatf) + translate_value = prim.GetAttribute("xformOp:translate").Get() + assert isinstance(translate_value, Gf.Vec3f), f"Expected Gf.Vec3f, got {type(translate_value)}" + assert_vec3_close(translate_value, new_translation, eps=1e-5) + + orient_value = prim.GetAttribute("xformOp:orient").Get() + assert isinstance(orient_value, Gf.Quatf), f"Expected Gf.Quatf, got {type(orient_value)}" + assert_quat_close(orient_value, new_orientation, eps=1e-5) + + scale_value = prim.GetAttribute("xformOp:scale").Get() + assert isinstance(scale_value, Gf.Vec3f), f"Expected Gf.Vec3f, got {type(scale_value)}" + assert_vec3_close(scale_value, new_scale, eps=1e-5) + + # Verify the world pose matches what we set + pos_after, quat_after = sim_utils.resolve_prim_pose(prim) + assert_vec3_close(Gf.Vec3d(*pos_after), new_translation, eps=1e-4) + assert_quat_close(Gf.Quatd(*quat_after), new_orientation, eps=1e-4) + + def test_convert_world_pose_to_local_basic(): """Test basic world-to-local pose conversion.""" # obtain stage handle From 113ac86c2608e89a9950848423962115e0db98ac Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 04:38:59 +0100 Subject: [PATCH 018/100] removes benchmark test --- .../test/sim/test_utils_transforms.py | 40 ------------------- 1 file changed, 40 deletions(-) diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index df942f3aa26..1cb4182a81f 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -959,46 +959,6 @@ def test_convert_world_pose_to_local_complex_hierarchy(): assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) -""" -Performance Benchmarking Tests -""" - -import time - - -def test_standardize_xform_ops_performance_batch(): - """Benchmark standardize_xform_ops performance on multiple prims.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Create many test prims - num_prims = 1024 - prims = [] - - for i in range(num_prims): - prim = stage.DefinePrim(f"/World/PerfTestBatch/Prim_{i:03d}", "Xform") - xformable = UsdGeom.Xformable(prim) - # Add various deprecated operations - xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(i * 1.0, i * 2.0, i * 3.0)) - xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(i, i, i)) - prims.append(prim) - - # Benchmark batch operation - start_time = time.perf_counter() - for prim in prims: - result = sim_utils.standardize_xform_ops(prim) - assert result is True - end_time = time.perf_counter() - - # Print timing - elapsed_ms = (end_time - start_time) * 1000 - avg_ms = elapsed_ms / num_prims - print(f"\n Batch standardization ({num_prims} prims): {elapsed_ms:.4f} ms total, {avg_ms:.4f} ms/prim") - - # Verify operation is reasonably fast - assert avg_ms < 0.1, f"Average operation took {avg_ms:.2f}ms/prim, expected < 0.1ms/prim" - - def test_resolve_prim_pose(): """Test resolve_prim_pose() function.""" # number of objects From b33eb9a95f5d81fffc229a14547852492a050dc3 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 04:53:31 +0100 Subject: [PATCH 019/100] makes a habit of sending stage --- .../isaaclab/isaaclab/sim/spawners/from_files/from_files.py | 3 ++- source/isaaclab/isaaclab/sim/spawners/lights/lights.py | 4 +++- source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py | 4 ++-- source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py | 4 ++-- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index d2583d8c9e3..17d69d7da11 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -198,7 +198,7 @@ def spawn_ground_plane( # Spawn Ground-plane if not stage.GetPrimAtPath(prim_path).IsValid(): - create_prim(prim_path, usd_path=cfg.usd_path, translation=translation, orientation=orientation) + create_prim(prim_path, usd_path=cfg.usd_path, translation=translation, orientation=orientation, stage=stage) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") @@ -327,6 +327,7 @@ def _spawn_from_usd_file( translation=translation, orientation=orientation, scale=cfg.scale, + stage=stage, ) else: logger.warning(f"A prim already exists at prim path: '{prim_path}'.") diff --git a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py index 51a57c61ba4..9b0106c6ecd 100644 --- a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py +++ b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py @@ -50,7 +50,9 @@ def spawn_light( if stage.GetPrimAtPath(prim_path).IsValid(): raise ValueError(f"A prim already exists at path: '{prim_path}'.") # create the prim - prim = create_prim(prim_path, prim_type=cfg.prim_type, translation=translation, orientation=orientation) + prim = create_prim( + prim_path, prim_type=cfg.prim_type, translation=translation, orientation=orientation, stage=stage + ) # convert to dict cfg = cfg.to_dict() diff --git a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py index 6ae610c1bc2..b701b904ea2 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py @@ -282,7 +282,7 @@ def _spawn_geom_from_prim_type( # spawn geometry if it doesn't exist. if not stage.GetPrimAtPath(prim_path).IsValid(): - create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation) + create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation, stage=stage) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") @@ -291,7 +291,7 @@ def _spawn_geom_from_prim_type( mesh_prim_path = geom_prim_path + "/mesh" # create the geometry prim - create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes) + create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes, stage=stage) # apply collision properties if cfg.collision_props is not None: schemas.define_collision_properties(mesh_prim_path, cfg.collision_props) diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py index 2e5141096f1..64d0c4f4ab9 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py @@ -67,7 +67,7 @@ def spawn_multi_asset( # find a free prim path to hold all the template prims template_prim_path = sim_utils.get_next_free_prim_path("/World/Template", stage=stage) - sim_utils.create_prim(template_prim_path, "Scope") + sim_utils.create_prim(template_prim_path, "Scope", stage=stage) # spawn everything first in a "Dataset" prim proto_prim_paths = list() @@ -116,7 +116,7 @@ def spawn_multi_asset( Sdf.CopySpec(env_spec.layer, Sdf.Path(proto_path), env_spec.layer, Sdf.Path(prim_path)) # delete the dataset prim after spawning - sim_utils.delete_prim(template_prim_path) + sim_utils.delete_prim(template_prim_path, stage=stage) # set carb setting to indicate Isaac Lab's environments that different prims have been spawned # at varying prim paths. In this case, PhysX parser shouldn't optimize the stage parsing. From c7f6b236d2a61dc8878454a6628434e3b559532a Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 04:59:19 +0100 Subject: [PATCH 020/100] use stage attributes --- .../isaaclab/markers/visualization_markers.py | 1 + .../sim/spawners/from_files/from_files.py | 8 +++++--- .../isaaclab/sim/spawners/lights/lights.py | 4 +++- .../isaaclab/sim/spawners/meshes/meshes.py | 15 ++++++++------- .../isaaclab/sim/spawners/sensors/sensors.py | 1 + .../isaaclab/sim/spawners/shapes/shapes.py | 14 +++++++------- .../isaaclab/sim/spawners/wrappers/wrappers.py | 4 ++-- 7 files changed, 27 insertions(+), 20 deletions(-) diff --git a/source/isaaclab/isaaclab/markers/visualization_markers.py b/source/isaaclab/isaaclab/markers/visualization_markers.py index 77421657612..86514d45a8c 100644 --- a/source/isaaclab/isaaclab/markers/visualization_markers.py +++ b/source/isaaclab/isaaclab/markers/visualization_markers.py @@ -407,6 +407,7 @@ def _process_prototype_prim(self, prim: Usd.Prim): value=True, prev=None, type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, + usd_context_name=prim.GetStage(), ) # add children to list all_prims += child_prim.GetChildren() diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index d2583d8c9e3..65738606793 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -198,7 +198,7 @@ def spawn_ground_plane( # Spawn Ground-plane if not stage.GetPrimAtPath(prim_path).IsValid(): - create_prim(prim_path, usd_path=cfg.usd_path, translation=translation, orientation=orientation) + create_prim(prim_path, usd_path=cfg.usd_path, translation=translation, orientation=orientation, stage=stage) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") @@ -215,7 +215,7 @@ def spawn_ground_plane( raise ValueError(f"No collision prim found at path: '{prim_path}'.") # bind physics material to the collision prim collision_prim_path = str(collision_prim.GetPath()) - bind_physics_material(collision_prim_path, f"{prim_path}/physicsMaterial") + bind_physics_material(collision_prim_path, f"{prim_path}/physicsMaterial", stage=stage) # Obtain environment prim environment_prim = stage.GetPrimAtPath(f"{prim_path}/Environment") @@ -247,6 +247,7 @@ def spawn_ground_plane( value=Gf.Vec3f(*cfg.color), prev=None, type_to_create_if_not_exist=Sdf.ValueTypeNames.Color3f, + usd_context_name=stage, ) # Remove the light from the ground plane # It isn't bright enough and messes up with the user's lighting settings @@ -327,6 +328,7 @@ def _spawn_from_usd_file( translation=translation, orientation=orientation, scale=cfg.scale, + stage=stage, ) else: logger.warning(f"A prim already exists at prim path: '{prim_path}'.") @@ -372,7 +374,7 @@ def _spawn_from_usd_file( # create material cfg.visual_material.func(material_path, cfg.visual_material) # apply material - bind_visual_material(prim_path, material_path) + bind_visual_material(prim_path, material_path, stage=stage) # return the prim return stage.GetPrimAtPath(prim_path) diff --git a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py index 51a57c61ba4..9b0106c6ecd 100644 --- a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py +++ b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py @@ -50,7 +50,9 @@ def spawn_light( if stage.GetPrimAtPath(prim_path).IsValid(): raise ValueError(f"A prim already exists at path: '{prim_path}'.") # create the prim - prim = create_prim(prim_path, prim_type=cfg.prim_type, translation=translation, orientation=orientation) + prim = create_prim( + prim_path, prim_type=cfg.prim_type, translation=translation, orientation=orientation, stage=stage + ) # convert to dict cfg = cfg.to_dict() diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index fbac584a208..eafe906be4e 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -338,6 +338,7 @@ def _spawn_mesh_geom_from_mesh( "faceVertexCounts": np.asarray([3] * len(mesh.faces)), "subdivisionScheme": "bilinear", }, + stage=stage, ) # note: in case of deformable objects, we need to apply the deformable properties to the mesh prim. @@ -345,9 +346,9 @@ def _spawn_mesh_geom_from_mesh( if cfg.deformable_props is not None: # apply mass properties if cfg.mass_props is not None: - schemas.define_mass_properties(mesh_prim_path, cfg.mass_props) + schemas.define_mass_properties(mesh_prim_path, cfg.mass_props, stage=stage) # apply deformable body properties - schemas.define_deformable_body_properties(mesh_prim_path, cfg.deformable_props) + schemas.define_deformable_body_properties(mesh_prim_path, cfg.deformable_props, stage=stage) elif cfg.collision_props is not None: # decide on type of collision approximation based on the mesh if cfg.__class__.__name__ == "MeshSphereCfg": @@ -362,7 +363,7 @@ def _spawn_mesh_geom_from_mesh( mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(mesh_prim) mesh_collision_api.GetApproximationAttr().Set(collision_approximation) # apply collision properties - schemas.define_collision_properties(mesh_prim_path, cfg.collision_props) + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props, stage=stage) # apply visual material if cfg.visual_material is not None: @@ -373,7 +374,7 @@ def _spawn_mesh_geom_from_mesh( # create material cfg.visual_material.func(material_path, cfg.visual_material) # apply material - bind_visual_material(mesh_prim_path, material_path) + bind_visual_material(mesh_prim_path, material_path, stage=stage) # apply physics material if cfg.physics_material is not None: @@ -384,12 +385,12 @@ def _spawn_mesh_geom_from_mesh( # create material cfg.physics_material.func(material_path, cfg.physics_material) # apply material - bind_physics_material(mesh_prim_path, material_path) + bind_physics_material(mesh_prim_path, material_path, stage=stage) # note: we apply the rigid properties to the parent prim in case of rigid objects. if cfg.rigid_props is not None: # apply mass properties if cfg.mass_props is not None: - schemas.define_mass_properties(prim_path, cfg.mass_props) + schemas.define_mass_properties(prim_path, cfg.mass_props, stage=stage) # apply rigid properties - schemas.define_rigid_body_properties(prim_path, cfg.rigid_props) + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props, stage=stage) diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index a3c2518da15..3e4d7635a45 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -104,6 +104,7 @@ def spawn_camera( value=True, prev=None, type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, + usd_context_name=stage, ) # decide the custom attributes to add if cfg.projection_type == "pinhole": diff --git a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py index 6ae610c1bc2..a7780c25596 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py @@ -282,7 +282,7 @@ def _spawn_geom_from_prim_type( # spawn geometry if it doesn't exist. if not stage.GetPrimAtPath(prim_path).IsValid(): - create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation) + create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation, stage=stage) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") @@ -291,10 +291,10 @@ def _spawn_geom_from_prim_type( mesh_prim_path = geom_prim_path + "/mesh" # create the geometry prim - create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes) + create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes, stage=stage) # apply collision properties if cfg.collision_props is not None: - schemas.define_collision_properties(mesh_prim_path, cfg.collision_props) + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props, stage=stage) # apply visual material if cfg.visual_material is not None: if not cfg.visual_material_path.startswith("/"): @@ -304,7 +304,7 @@ def _spawn_geom_from_prim_type( # create material cfg.visual_material.func(material_path, cfg.visual_material) # apply material - bind_visual_material(mesh_prim_path, material_path) + bind_visual_material(mesh_prim_path, material_path, stage=stage) # apply physics material if cfg.physics_material is not None: if not cfg.physics_material_path.startswith("/"): @@ -314,12 +314,12 @@ def _spawn_geom_from_prim_type( # create material cfg.physics_material.func(material_path, cfg.physics_material) # apply material - bind_physics_material(mesh_prim_path, material_path) + bind_physics_material(mesh_prim_path, material_path, stage=stage) # note: we apply rigid properties in the end to later make the instanceable prim # apply mass properties if cfg.mass_props is not None: - schemas.define_mass_properties(prim_path, cfg.mass_props) + schemas.define_mass_properties(prim_path, cfg.mass_props, stage=stage) # apply rigid body properties if cfg.rigid_props is not None: - schemas.define_rigid_body_properties(prim_path, cfg.rigid_props) + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props, stage=stage) diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py index 2e5141096f1..64d0c4f4ab9 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py @@ -67,7 +67,7 @@ def spawn_multi_asset( # find a free prim path to hold all the template prims template_prim_path = sim_utils.get_next_free_prim_path("/World/Template", stage=stage) - sim_utils.create_prim(template_prim_path, "Scope") + sim_utils.create_prim(template_prim_path, "Scope", stage=stage) # spawn everything first in a "Dataset" prim proto_prim_paths = list() @@ -116,7 +116,7 @@ def spawn_multi_asset( Sdf.CopySpec(env_spec.layer, Sdf.Path(proto_path), env_spec.layer, Sdf.Path(prim_path)) # delete the dataset prim after spawning - sim_utils.delete_prim(template_prim_path) + sim_utils.delete_prim(template_prim_path, stage=stage) # set carb setting to indicate Isaac Lab's environments that different prims have been spawned # at varying prim paths. In this case, PhysX parser shouldn't optimize the stage parsing. From b4065213878ac12a90b586fe8b2693c9e6e8b689 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 05:22:40 +0100 Subject: [PATCH 021/100] optimization for root parent --- source/isaaclab/isaaclab/sim/utils/transforms.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 1bad98104c9..a54a9ee10f7 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -274,13 +274,15 @@ def resolve_prim_pose( # check if ref prim is valid if not ref_prim.IsValid(): raise ValueError(f"Ref prim at path '{ref_prim.GetPath().pathString}' is not valid.") - # get ref prim xform - ref_xform = UsdGeom.Xformable(ref_prim) - ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - # make sure ref tf is orthonormal - ref_tf = ref_tf.GetOrthonormalized() - # compute relative transform to get prim in ref frame - prim_tf = prim_tf * ref_tf.GetInverse() + # if reference prim is the root, we can skip the computation + if ref_prim.GetPath() != Sdf.Path.absoluteRootPath: + # get ref prim xform + ref_xform = UsdGeom.Xformable(ref_prim) + ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # make sure ref tf is orthonormal + ref_tf = ref_tf.GetOrthonormalized() + # compute relative transform to get prim in ref frame + prim_tf = prim_tf * ref_tf.GetInverse() # extract position and orientation prim_pos = [*prim_tf.ExtractTranslation()] From dc62696ef514aebc921a4c0e136125fad7d1cb86 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 06:06:35 +0100 Subject: [PATCH 022/100] adds test to validate xform ops order --- .../isaaclab/isaaclab/sim/utils/transforms.py | 32 +++ .../test/sim/test_utils_transforms.py | 214 ++++++++++++++++++ 2 files changed, 246 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index a54a9ee10f7..248a2e1186d 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -213,6 +213,38 @@ def standardize_xform_ops( return True +def validate_standard_xform_ops(prim: Usd.Prim) -> bool: + """Validate if the transform operations on a prim are standardized. + + This function checks if the transform operations on a prim are standardized to the canonical form: + [translate, orient, scale]. + + Args: + prim: The USD prim to validate. + """ + # check if prim is valid + if not prim.IsValid(): + logger.error(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + return False + # check if prim is an xformable + if not prim.IsA(UsdGeom.Xformable): + logger.error(f"Prim at path '{prim.GetPath().pathString}' is not an xformable.") + return False + # get the xformable interface + xformable = UsdGeom.Xformable(prim) + # get the xform operation order + xform_op_order = xformable.GetOrderedXformOps() + xform_op_order = [op.GetOpName() for op in xform_op_order] + # check if the xform operation order is the canonical form + if xform_op_order != ["xformOp:translate", "xformOp:orient", "xformOp:scale"]: + msg = f"Xform operation order for prim at path '{prim.GetPath().pathString}' is not the canonical form." + msg += f" Received order: {xform_op_order}" + msg += " Expected order: ['xformOp:translate', 'xformOp:orient', 'xformOp:scale']" + logger.error(msg) + return False + return True + + def resolve_prim_pose( prim: Usd.Prim, ref_prim: Usd.Prim | None = None ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 1cb4182a81f..23e0c7af6a4 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -1054,6 +1054,220 @@ def test_resolve_prim_pose(): np.testing.assert_allclose(quat, gt_quat, atol=1e-3) +def test_validate_standard_xform_ops_valid(): + """Test validate_standard_xform_ops returns True for standardized prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations + prim = sim_utils.create_prim( + "/World/TestValid", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Standardize the prim + sim_utils.standardize_xform_ops(prim) + + # Validate it + assert sim_utils.validate_standard_xform_ops(prim) is True + + +def test_validate_standard_xform_ops_invalid_order(): + """Test validate_standard_xform_ops returns False for non-standard operation order.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim and manually set up xform ops in wrong order + prim_path = "/World/TestInvalidOrder" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add operations in wrong order: scale, translate, orient (should be translate, orient, scale) + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(1.0, 1.0, 1.0)) + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + orient_op = xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble) + orient_op.Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_with_deprecated_ops(): + """Test validate_standard_xform_ops returns False when deprecated operations exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with deprecated rotateXYZ operation + prim_path = "/World/TestDeprecated" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add deprecated rotateXYZ operation + rotate_xyz_op = xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_xyz_op.Set(Gf.Vec3d(45.0, 30.0, 60.0)) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_missing_operations(): + """Test validate_standard_xform_ops returns False when standard operations are missing.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with only translate operation (missing orient and scale) + prim_path = "/World/TestMissing" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Validate it - should return False (missing orient and scale) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_invalid_prim(): + """Test validate_standard_xform_ops returns False for invalid prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(invalid_prim) is False + + +def test_validate_standard_xform_ops_non_xformable(): + """Test validate_standard_xform_ops returns False for non-Xformable prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a Material prim (not Xformable) + from pxr import UsdShade + + material_prim = UsdShade.Material.Define(stage, "/World/TestMaterial").GetPrim() + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(material_prim) is False + + +def test_validate_standard_xform_ops_with_transform_matrix(): + """Test validate_standard_xform_ops returns False when transform matrix operation exists.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with transform matrix + prim_path = "/World/TestTransformMatrix" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add transform matrix operation + transform_op = xformable.AddTransformOp(UsdGeom.XformOp.PrecisionDouble) + matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(5.0, 10.0, 15.0)) + transform_op.Set(matrix) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_extra_operations(): + """Test validate_standard_xform_ops returns False when extra operations exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations + prim = sim_utils.create_prim( + "/World/TestExtra", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Standardize it + sim_utils.standardize_xform_ops(prim) + + # Add an extra operation + xformable = UsdGeom.Xformable(prim) + extra_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + extra_op.Set(45.0) + + # Validate it - should return False (has extra operation) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_after_standardization(): + """Test validate_standard_xform_ops returns True after standardization of non-standard prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with non-standard operations + prim_path = "/World/TestBeforeAfter" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add deprecated operations + rotate_x_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + rotate_x_op.Set(45.0) + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Validate before standardization - should be False + assert sim_utils.validate_standard_xform_ops(prim) is False + + # Standardize the prim + sim_utils.standardize_xform_ops(prim) + + # Validate after standardization - should be True + assert sim_utils.validate_standard_xform_ops(prim) is True + + +def test_validate_standard_xform_ops_on_geometry(): + """Test validate_standard_xform_ops works correctly on geometry prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a cube with standard operations + cube_prim = sim_utils.create_prim( + "/World/TestCube", + "Cube", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Standardize it + sim_utils.standardize_xform_ops(cube_prim) + + # Validate it - should be True + assert sim_utils.validate_standard_xform_ops(cube_prim) is True + + +def test_validate_standard_xform_ops_empty_prim(): + """Test validate_standard_xform_ops on prim with no xform operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a bare prim with no xform operations + prim_path = "/World/TestEmpty" + prim = stage.DefinePrim(prim_path, "Xform") + + # Validate it - should return False (no operations at all) + assert sim_utils.validate_standard_xform_ops(prim) is False + + def test_resolve_prim_scale(): """Test resolve_prim_scale() function. From 083a2c37396e7e5cfdfd4405e996289395e1ec64 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 06:23:08 +0100 Subject: [PATCH 023/100] adds benchmark script --- .../benchmarks/benchmark_xform_prim_view.py | 367 ++++++++++++++++++ .../isaaclab/sim/utils/xform_prim_view.py | 270 +++++++++++++ 2 files changed, 637 insertions(+) create mode 100644 scripts/benchmarks/benchmark_xform_prim_view.py create mode 100644 source/isaaclab/isaaclab/sim/utils/xform_prim_view.py diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py new file mode 100644 index 00000000000..ff100a2829e --- /dev/null +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -0,0 +1,367 @@ +#!/usr/bin/env python3 + +# 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 + +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Benchmark script comparing Isaac Lab's XFormPrimView against Isaac Sim's XFormPrimView. + +This script tests the performance of batched transform operations using either +Isaac Lab's implementation or Isaac Sim's implementation. + +Usage: + ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --device cuda:0 --headless +""" + +from __future__ import annotations + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# parse the arguments +args_cli = argparse.Namespace() + +parser = argparse.ArgumentParser(description="This script can help you benchmark the performance of XFormPrimView.") + +parser.add_argument("--num_envs", type=int, default=100, help="Number of environments to simulate.") +parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import time +import torch +from typing import Literal + +from isaacsim.core.prims import XFormPrim as IsaacSimXFormPrimView + +import isaaclab.sim as sim_utils +from isaaclab.sim.utils.xform_prim_view import XFormPrimView as IsaacLabXFormPrimView + + +@torch.no_grad() +def benchmark_xform_prim_view( + api: Literal["isaaclab", "isaacsim"], num_iterations: int +) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark the XFormPrimView class from either Isaac Lab or Isaac Sim. + + Args: + api: Which API to benchmark ("isaaclab" or "isaacsim"). + num_iterations: Number of iterations to run. + + Returns: + A tuple of (timing_results, computed_results) where: + - timing_results: Dictionary containing timing results for various operations + - computed_results: Dictionary containing the computed values for validation + """ + timing_results = {} + computed_results = {} + + # Setup scene + print(" Setting up scene") + # Clear stage + sim_utils.create_new_stage() + # Create simulation context + sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)) + stage = sim_utils.get_current_stage() + + # Create prims + prim_paths = [] + for i in range(args_cli.num_envs): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 1.0)) + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", stage=stage, translation=(0.0, 0.0, 0.0)) + prim_paths.append(f"/World/Env_{i}/Object") + # Play simulation + sim.reset() + + # Pattern to match all prims + pattern = "/World/Env_.*/Object" + print(f" Pattern: {pattern}") + + # Create view + start_time = time.perf_counter() + if api == "isaaclab": + xform_view = IsaacLabXFormPrimView(pattern, device=args_cli.device) + elif api == "isaacsim": + xform_view = IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + else: + raise ValueError(f"Invalid API: {api}") + timing_results["init"] = time.perf_counter() - start_time + + print(f" XFormPrimView managing {xform_view.count} prims") + + # Benchmark get_world_poses + start_time = time.perf_counter() + for _ in range(num_iterations): + positions, orientations = xform_view.get_world_poses() + timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Ensure tensors are torch tensors + if not isinstance(positions, torch.Tensor): + positions = torch.tensor(positions, dtype=torch.float32) + if not isinstance(orientations, torch.Tensor): + orientations = torch.tensor(orientations, dtype=torch.float32) + + # Store initial world poses + computed_results["initial_world_positions"] = positions.clone() + computed_results["initial_world_orientations"] = orientations.clone() + + # Benchmark set_world_poses + new_positions = positions.clone() + new_positions[:, 2] += 0.1 + start_time = time.perf_counter() + for _ in range(num_iterations): + xform_view.set_world_poses(new_positions, orientations) + timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Get world poses after setting to verify + positions_after_set, orientations_after_set = xform_view.get_world_poses() + if not isinstance(positions_after_set, torch.Tensor): + positions_after_set = torch.tensor(positions_after_set, dtype=torch.float32) + if not isinstance(orientations_after_set, torch.Tensor): + orientations_after_set = torch.tensor(orientations_after_set, dtype=torch.float32) + computed_results["world_positions_after_set"] = positions_after_set.clone() + computed_results["world_orientations_after_set"] = orientations_after_set.clone() + + # Benchmark get_local_poses + start_time = time.perf_counter() + for _ in range(num_iterations): + translations, orientations_local = xform_view.get_local_poses() + timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Ensure tensors are torch tensors + if not isinstance(translations, torch.Tensor): + translations = torch.tensor(translations, dtype=torch.float32) + if not isinstance(orientations_local, torch.Tensor): + orientations_local = torch.tensor(orientations_local, dtype=torch.float32) + + # Store initial local poses + computed_results["initial_local_translations"] = translations.clone() + computed_results["initial_local_orientations"] = orientations_local.clone() + + # Benchmark set_local_poses + new_translations = translations.clone() + new_translations[:, 2] += 0.1 + start_time = time.perf_counter() + for _ in range(num_iterations): + xform_view.set_local_poses(new_translations, orientations_local) + timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations + + # Get local poses after setting to verify + translations_after_set, orientations_local_after_set = xform_view.get_local_poses() + if not isinstance(translations_after_set, torch.Tensor): + translations_after_set = torch.tensor(translations_after_set, dtype=torch.float32) + if not isinstance(orientations_local_after_set, torch.Tensor): + orientations_local_after_set = torch.tensor(orientations_local_after_set, dtype=torch.float32) + computed_results["local_translations_after_set"] = translations_after_set.clone() + computed_results["local_orientations_after_set"] = orientations_local_after_set.clone() + + # Benchmark combined get operation + start_time = time.perf_counter() + for _ in range(num_iterations): + positions, orientations = xform_view.get_world_poses() + translations, local_orientations = xform_view.get_local_poses() + timing_results["get_both"] = (time.perf_counter() - start_time) / num_iterations + + # close simulation + sim.clear() + sim.clear_all_callbacks() + sim.clear_instance() + + return timing_results, computed_results + + +def compare_results( + isaaclab_computed: dict[str, torch.Tensor], isaacsim_computed: dict[str, torch.Tensor], tolerance: float = 1e-4 +) -> dict[str, dict[str, float]]: + """Compare computed results between Isaac Lab and Isaac Sim implementations. + + Args: + isaaclab_computed: Computed values from Isaac Lab's XFormPrimView. + isaacsim_computed: Computed values from Isaac Sim's XFormPrimView. + tolerance: Tolerance for numerical comparison. + + Returns: + Dictionary containing comparison statistics (max difference, mean difference, etc.) for each result. + """ + comparison_stats = {} + + for key in isaaclab_computed.keys(): + if key not in isaacsim_computed: + print(f" Warning: Key '{key}' not found in Isaac Sim results") + continue + + isaaclab_val = isaaclab_computed[key] + isaacsim_val = isaacsim_computed[key] + + # Compute differences + diff = torch.abs(isaaclab_val - isaacsim_val) + max_diff = torch.max(diff).item() + mean_diff = torch.mean(diff).item() + + # Check if within tolerance + all_close = torch.allclose(isaaclab_val, isaacsim_val, atol=tolerance, rtol=0) + + comparison_stats[key] = { + "max_diff": max_diff, + "mean_diff": mean_diff, + "all_close": all_close, + } + + return comparison_stats + + +def print_comparison_results(comparison_stats: dict[str, dict[str, float]], tolerance: float): + """Print comparison results between implementations. + + Args: + comparison_stats: Dictionary containing comparison statistics. + tolerance: Tolerance used for comparison. + """ + # Check if all results match + all_match = all(stats["all_close"] for stats in comparison_stats.values()) + + if all_match: + # Compact output when everything matches + print("\n" + "=" * 100) + print("RESULT COMPARISON: Isaac Lab vs Isaac Sim") + print("=" * 100) + print(f"✓ All computed values match within tolerance ({tolerance})") + print("=" * 100) + print() + else: + # Detailed output when there are mismatches + print("\n" + "=" * 100) + print("RESULT COMPARISON: Isaac Lab vs Isaac Sim") + print("=" * 100) + print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") + print("-" * 100) + + for key, stats in comparison_stats.items(): + # Format the key for display + display_key = key.replace("_", " ").title() + match_str = "✓ Yes" if stats["all_close"] else "✗ No" + + print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") + + print("=" * 100) + print(f"\n✗ Some results differ beyond tolerance ({tolerance})") + print(" This may indicate implementation differences between Isaac Lab and Isaac Sim") + print() + + +def print_results( + isaaclab_results: dict[str, float], isaacsim_results: dict[str, float], num_prims: int, num_iterations: int +): + """Print benchmark results in a formatted table. + + Args: + isaaclab_results: Results from Isaac Lab's XFormPrimView benchmark. + isaacsim_results: Results from Isaac Sim's XFormPrimView benchmark. + num_prims: Number of prims tested. + num_iterations: Number of iterations run. + """ + print("\n" + "=" * 100) + print(f"BENCHMARK RESULTS: {num_prims} prims, {num_iterations} iterations") + print("=" * 100) + + # Print header + print(f"{'Operation':<25} {'Isaac Lab (ms)':<25} {'Isaac Sim (ms)':<25} {'Speedup':<15}") + print("-" * 100) + + # Print each operation + operations = [ + ("Initialization", "init"), + ("Get World Poses", "get_world_poses"), + ("Set World Poses", "set_world_poses"), + ("Get Local Poses", "get_local_poses"), + ("Set Local Poses", "set_local_poses"), + ("Get Both (World+Local)", "get_both"), + ] + + for op_name, op_key in operations: + isaaclab_time = isaaclab_results.get(op_key, 0) * 1000 # Convert to ms + isaacsim_time = isaacsim_results.get(op_key, 0) * 1000 # Convert to ms + + if isaaclab_time > 0 and isaacsim_time > 0: + speedup = isaacsim_time / isaaclab_time + print(f"{op_name:<25} {isaaclab_time:>20.4f} {isaacsim_time:>20.4f} {speedup:>10.2f}x") + else: + print(f"{op_name:<25} {isaaclab_time:>20.4f} {'N/A':<20} {'N/A':<15}") + + print("=" * 100) + + # Calculate and print total time + if isaaclab_results and isaacsim_results: + total_isaaclab = sum(isaaclab_results.values()) * 1000 + total_isaacsim = sum(isaacsim_results.values()) * 1000 + overall_speedup = total_isaacsim / total_isaaclab if total_isaaclab > 0 else 0 + print(f"\n{'Total Time':<25} {total_isaaclab:>20.4f} {total_isaacsim:>20.4f} {overall_speedup:>10.2f}x") + else: + total_isaaclab = sum(isaaclab_results.values()) * 1000 + print(f"\n{'Total Time':<25} {total_isaaclab:>20.4f} {'N/A':<20} {'N/A':<15}") + + print("\nNotes:") + print(" - Times are averaged over all iterations") + print(" - Speedup = (Isaac Sim time) / (Isaac Lab time)") + print(" - Speedup > 1.0 means Isaac Lab is faster") + print(" - Speedup < 1.0 means Isaac Sim is faster") + print() + + +def main(): + """Main benchmark function.""" + print("=" * 100) + print("XFormPrimView Benchmark") + print("=" * 100) + print("Configuration:") + print(f" Number of environments: {args_cli.num_envs}") + print(f" Iterations per test: {args_cli.num_iterations}") + print(f" Device: {args_cli.device}") + print() + + # Benchmark XFormPrimView + print("Benchmarking XFormPrimView from Isaac Lab...") + isaaclab_timing, isaaclab_computed = benchmark_xform_prim_view( + api="isaaclab", num_iterations=args_cli.num_iterations + ) + print(" Done!") + print() + + # Benchmark Isaac Sim XFormPrimView + print("Benchmarking Isaac Sim XFormPrimView...") + isaacsim_timing, isaacsim_computed = benchmark_xform_prim_view( + api="isaacsim", num_iterations=args_cli.num_iterations + ) + print(" Done!") + print() + + # Print timing results + print_results(isaaclab_timing, isaacsim_timing, args_cli.num_envs, args_cli.num_iterations) + + # Compare computed results + print("\nComparing computed results...") + comparison_stats = compare_results(isaaclab_computed, isaacsim_computed, tolerance=1e-6) + print_comparison_results(comparison_stats, tolerance=1e-4) + + # Clean up + sim_utils.SimulationContext.clear_instance() + + +if __name__ == "__main__": + main() diff --git a/source/isaaclab/isaaclab/sim/utils/xform_prim_view.py b/source/isaaclab/isaaclab/sim/utils/xform_prim_view.py new file mode 100644 index 00000000000..35d7ba5eae3 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/utils/xform_prim_view.py @@ -0,0 +1,270 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch + +from pxr import Sdf, Usd, UsdGeom + +import isaaclab.sim as sim_utils + + +class XFormPrimView: + """Optimized batched interface for reading and writing transforms of multiple USD prims. + + This class provides efficient batch operations for getting and setting poses (position and orientation) + of multiple prims at once using torch tensors. It is designed for scenarios where you need to manipulate + many prims simultaneously, such as in multi-agent simulations or large-scale procedural generation. + + The class supports both world-space and local-space pose operations: + + - **World poses**: Positions and orientations in the global world frame + - **Local poses**: Positions and orientations relative to each prim's parent + + Note: + All prims in the view must be Xformable (support transforms). Non-xformable prims + (such as materials or shaders) will raise a ValueError during initialization. + + """ + + def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None): + """Initialize the XFormPrimView with matching prims. + + Args: + prim_path: USD prim path pattern to match prims. + device: Device to place the tensors on. Defaults to "cpu". + stage: USD stage to search for prims. If None, uses the current active stage. + + Raises: + ValueError: If any matched prim is not Xformable. + """ + stage = sim_utils.get_current_stage() if stage is None else stage + + self._prim_path = prim_path + self._prims = sim_utils.find_matching_prims(prim_path) + self._device = device + + # check all prims are xformable with standard transform operations + for prim in self._prims: + if not sim_utils.validate_standard_xform_ops(prim): + raise ValueError( + f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" + f" operations. Received type: '{prim.GetTypeName()}'." + ) + + @property + def count(self) -> int: + """Number of prims in this view. + + Returns: + The number of prims being managed by this view. + """ + return len(self._prims) + + """ + Operations - Setters. + """ + + def set_world_poses(self, positions: torch.Tensor | None = None, orientations: torch.Tensor | None = None) -> None: + """Set world-space poses for all prims in the view. + + This method sets the position and/or orientation of each prim in world space. The world pose + is computed by considering the prim's parent transforms. If a prim has a parent, this method + will convert the world pose to the appropriate local pose before setting it. + + Note: + This operation writes to USD at the default time code. Any animation data will not be affected. + + Args: + positions: World-space positions as a tensor of shape (N, 3) where N is the number of prims. + If None, positions are not modified. Defaults to None. + orientations: World-space orientations as quaternions (w, x, y, z) with shape (N, 4). + If None, orientations are not modified. Defaults to None. + + Raises: + ValueError: If positions shape is not (N, 3) or orientations shape is not (N, 4). + ValueError: If the number of poses doesn't match the number of prims in the view. + """ + # Validate inputs + if positions is not None: + if positions.shape != (self.count, 3): + raise ValueError( + f"Expected positions shape ({self.count}, 3), got {positions.shape}. " + "Number of positions must match the number of prims in the view." + ) + positions_list = positions.tolist() if positions is not None else None + else: + positions_list = None + if orientations is not None: + if orientations.shape != (self.count, 4): + raise ValueError( + f"Expected orientations shape ({self.count}, 4), got {orientations.shape}. " + "Number of orientations must match the number of prims in the view." + ) + orientations_list = orientations.tolist() if orientations is not None else None + else: + orientations_list = None + + # Set poses for each prim + with Sdf.ChangeBlock(): + for idx, prim in enumerate(self._prims): + # Get parent prim for local space conversion + parent_prim = prim.GetParent() + + # Determine what to set + world_pos = tuple(positions_list[idx]) if positions_list is not None else None + world_quat = tuple(orientations_list[idx]) if orientations_list is not None else None + + # Convert world pose to local if we have a valid parent + if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: + # Get current world pose if we're only setting one component + if world_pos is None or world_quat is None: + current_pos, current_quat = sim_utils.resolve_prim_pose(prim) + + if world_pos is None: + world_pos = current_pos + if world_quat is None: + world_quat = current_quat + + # Convert to local space + local_pos, local_quat = sim_utils.convert_world_pose_to_local(world_pos, world_quat, parent_prim) + else: + # No parent or parent is root, world == local + local_pos = world_pos + local_quat = world_quat + + # Get or create the standard transform operations + xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + + # set the data + xform_ops = [xform_op_translate, xform_op_orient] + xform_values = [local_pos, local_quat] + for xform_op, value in zip(xform_ops, xform_values): + if value is not None: + current_value = xform_op.Get() + xform_op.Set(type(current_value)(*value) if current_value is not None else value) + + def set_local_poses( + self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None + ) -> None: + """Set local-space poses for all prims in the view. + + This method sets the position and/or orientation of each prim in local space (relative to + their parent prims). This is useful when you want to directly manipulate the prim's transform + attributes without considering the parent hierarchy. + + Note: + This operation writes to USD at the default time code. Any animation data will not be affected. + + Args: + translations: Local-space translations as a tensor of shape (N, 3) where N is the number of prims. + If None, translations are not modified. Defaults to None. + orientations: Local-space orientations as quaternions (w, x, y, z) with shape (N, 4). + If None, orientations are not modified. Defaults to None. + + Raises: + ValueError: If translations shape is not (N, 3) or orientations shape is not (N, 4). + ValueError: If the number of poses doesn't match the number of prims in the view. + """ + # Validate inputs + if translations is not None: + if translations.shape != (self.count, 3): + raise ValueError( + f"Expected translations shape ({self.count}, 3), got {translations.shape}. " + "Number of translations must match the number of prims in the view." + ) + translations_list = translations.tolist() if translations is not None else None + else: + translations_list = None + if orientations is not None: + if orientations.shape != (self.count, 4): + raise ValueError( + f"Expected orientations shape ({self.count}, 4), got {orientations.shape}. " + "Number of orientations must match the number of prims in the view." + ) + orientations_list = orientations.tolist() if orientations is not None else None + else: + orientations_list = None + # Set local poses for each prim + with Sdf.ChangeBlock(): + for idx, prim in enumerate(self._prims): + local_pos = tuple(translations_list[idx]) if translations_list is not None else None + local_quat = tuple(orientations_list[idx]) if orientations_list is not None else None + + # Get or create the standard transform operations + xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) + xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) + + # set the data + xform_ops = [xform_op_translate, xform_op_orient] + xform_values = [local_pos, local_quat] + for xform_op, value in zip(xform_ops, xform_values): + if value is not None: + current_value = xform_op.Get() + xform_op.Set(type(current_value)(*value) if current_value is not None else value) + + """ + Operations - Getters. + """ + + def get_world_poses(self) -> tuple[torch.Tensor, torch.Tensor]: + """Get world-space poses for all prims in the view. + + This method retrieves the position and orientation of each prim in world space by computing + the full transform hierarchy from the prim to the world root. + + Note: + Scale and skew are ignored. The returned poses contain only translation and rotation. + + Returns: + A tuple of (positions, orientations) where: + + - positions: Torch tensor of shape (N, 3) containing world-space positions (x, y, z) + - orientations: Torch tensor of shape (N, 4) containing world-space quaternions (w, x, y, z) + """ + positions = [] + orientations = [] + + for prim in self._prims: + pos, quat = sim_utils.resolve_prim_pose(prim) + positions.append(pos) + orientations.append(quat) + + # Convert to tensors + positions_tensor = torch.tensor(positions, dtype=torch.float32, device=self._device) + orientations_tensor = torch.tensor(orientations, dtype=torch.float32, device=self._device) + + return positions_tensor, orientations_tensor + + def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: + """Get local-space poses for all prims in the view. + + This method retrieves the position and orientation of each prim in local space (relative to + their parent prims). These are the raw transform values stored on each prim. + + Note: + Scale is ignored. The returned poses contain only translation and rotation. + + Returns: + A tuple of (translations, orientations) where: + + - translations: Torch tensor of shape (N, 3) containing local-space translations (x, y, z) + - orientations: Torch tensor of shape (N, 4) containing local-space quaternions (w, x, y, z) + """ + translations = [] + orientations = [] + + for prim in self._prims: + local_pos, local_quat = sim_utils.resolve_prim_pose(prim, ref_prim=prim.GetParent()) + translations.append(local_pos) + orientations.append(local_quat) + + # Convert to tensors + translations_tensor = torch.tensor(translations, dtype=torch.float32, device=self._device) + orientations_tensor = torch.tensor(orientations, dtype=torch.float32, device=self._device) + + return translations_tensor, orientations_tensor From 366f338b6f69aa6132075d667ad3ab29dabd44c2 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 07:28:38 +0100 Subject: [PATCH 024/100] moves xform prim view into views module --- .../benchmarks/benchmark_xform_prim_view.py | 71 ++++++++++- .../isaaclab/isaaclab/sim/views/__init__.py | 8 ++ .../sim/{utils => views}/xform_prim_view.py | 110 +++++++++++++++--- 3 files changed, 174 insertions(+), 15 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/views/__init__.py rename source/isaaclab/isaaclab/sim/{utils => views}/xform_prim_view.py (71%) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index ff100a2829e..99adf7cea2a 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -16,7 +16,15 @@ Isaac Lab's implementation or Isaac Sim's implementation. Usage: + # Basic benchmark ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --device cuda:0 --headless + + # With profiling enabled (for snakeviz visualization) + ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --profile --headless + + # Then visualize with snakeviz: + snakeviz profile_results/isaaclab_xformprimview.prof + snakeviz profile_results/isaacsim_xformprimview.prof """ from __future__ import annotations @@ -34,6 +42,17 @@ parser.add_argument("--num_envs", type=int, default=100, help="Number of environments to simulate.") parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") +parser.add_argument( + "--profile", + action="store_true", + help="Enable profiling with cProfile. Results saved as .prof files for snakeviz visualization.", +) +parser.add_argument( + "--profile-dir", + type=str, + default="./profile_results", + help="Directory to save profile results. Default: ./profile_results", +) AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() @@ -44,6 +63,7 @@ """Rest everything follows.""" +import cProfile import time import torch from typing import Literal @@ -51,7 +71,7 @@ from isaacsim.core.prims import XFormPrim as IsaacSimXFormPrimView import isaaclab.sim as sim_utils -from isaaclab.sim.utils.xform_prim_view import XFormPrimView as IsaacLabXFormPrimView +from isaaclab.sim.views import XFormPrimView as IsaacLabXFormPrimView @torch.no_grad() @@ -77,9 +97,12 @@ def benchmark_xform_prim_view( # Clear stage sim_utils.create_new_stage() # Create simulation context + start_time = time.perf_counter() sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)) stage = sim_utils.get_current_stage() + print(f" Time taken to create simulation context: {time.perf_counter() - start_time} seconds") + # Create prims prim_paths = [] for i in range(args_cli.num_envs): @@ -333,21 +356,52 @@ def main(): print(f" Number of environments: {args_cli.num_envs}") print(f" Iterations per test: {args_cli.num_iterations}") print(f" Device: {args_cli.device}") + print(f" Profiling: {'Enabled' if args_cli.profile else 'Disabled'}") + if args_cli.profile: + print(f" Profile directory: {args_cli.profile_dir}") print() - # Benchmark XFormPrimView + # Create profile directory if profiling is enabled + if args_cli.profile: + import os + + os.makedirs(args_cli.profile_dir, exist_ok=True) + + # Benchmark Isaac Lab XFormPrimView print("Benchmarking XFormPrimView from Isaac Lab...") + if args_cli.profile: + profiler_isaaclab = cProfile.Profile() + profiler_isaaclab.enable() + isaaclab_timing, isaaclab_computed = benchmark_xform_prim_view( api="isaaclab", num_iterations=args_cli.num_iterations ) + + if args_cli.profile: + profiler_isaaclab.disable() + profile_file_isaaclab = f"{args_cli.profile_dir}/isaaclab_xformprimview.prof" + profiler_isaaclab.dump_stats(profile_file_isaaclab) + print(f" Profile saved to: {profile_file_isaaclab}") + print(" Done!") print() # Benchmark Isaac Sim XFormPrimView print("Benchmarking Isaac Sim XFormPrimView...") + if args_cli.profile: + profiler_isaacsim = cProfile.Profile() + profiler_isaacsim.enable() + isaacsim_timing, isaacsim_computed = benchmark_xform_prim_view( api="isaacsim", num_iterations=args_cli.num_iterations ) + + if args_cli.profile: + profiler_isaacsim.disable() + profile_file_isaacsim = f"{args_cli.profile_dir}/isaacsim_xformprimview.prof" + profiler_isaacsim.dump_stats(profile_file_isaacsim) + print(f" Profile saved to: {profile_file_isaacsim}") + print(" Done!") print() @@ -359,6 +413,19 @@ def main(): comparison_stats = compare_results(isaaclab_computed, isaacsim_computed, tolerance=1e-6) print_comparison_results(comparison_stats, tolerance=1e-4) + # Print profiling instructions if enabled + if args_cli.profile: + print("\n" + "=" * 100) + print("PROFILING RESULTS") + print("=" * 100) + print("Profile files have been saved. To visualize with snakeviz, run:") + print(f" snakeviz {profile_file_isaaclab}") + print(f" snakeviz {profile_file_isaacsim}") + print("\nAlternatively, use pstats to analyze in terminal:") + print(f" python -m pstats {profile_file_isaaclab}") + print("=" * 100) + print() + # Clean up sim_utils.SimulationContext.clear_instance() diff --git a/source/isaaclab/isaaclab/sim/views/__init__.py b/source/isaaclab/isaaclab/sim/views/__init__.py new file mode 100644 index 00000000000..2f1d56ed69f --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/__init__.py @@ -0,0 +1,8 @@ +# 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 + +"""Views for manipulating USD prims.""" + +from .xform_prim_view import XFormPrimView diff --git a/source/isaaclab/isaaclab/sim/utils/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py similarity index 71% rename from source/isaaclab/isaaclab/sim/utils/xform_prim_view.py rename to source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 35d7ba5eae3..355d7eaea31 100644 --- a/source/isaaclab/isaaclab/sim/utils/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -7,7 +7,7 @@ import torch -from pxr import Sdf, Usd, UsdGeom +from pxr import Gf, Sdf, Usd, UsdGeom import isaaclab.sim as sim_utils @@ -24,35 +24,63 @@ class XFormPrimView: - **World poses**: Positions and orientations in the global world frame - **Local poses**: Positions and orientations relative to each prim's parent - Note: - All prims in the view must be Xformable (support transforms). Non-xformable prims - (such as materials or shaders) will raise a ValueError during initialization. + .. note:: + **Performance Considerations:** + * Tensor operations are performed on the specified device (CPU/CUDA) + * USD write operations use ``Sdf.ChangeBlock`` for batched updates + * Getting poses involves USD API calls and cannot be fully accelerated on GPU + * For maximum performance, minimize get/set operations within tight loops + + .. note:: + **Transform Requirements:** + + All prims in the view must be Xformable and have standardized transform operations: + ``[translate, orient, scale]``. Non-standard prims will raise a ValueError during + initialization. Use :func:`isaaclab.sim.utils.standardize_xform_ops` to prepare prims. + + .. warning:: + This class operates at the USD default time code. Any animation or time-sampled data + will not be affected by write operations. For animated transforms, you need to handle + time-sampled keyframes separately. """ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None): """Initialize the XFormPrimView with matching prims. + This method searches the USD stage for all prims matching the provided path pattern, + validates that they are Xformable with standard transform operations, and stores + references for efficient batch operations. + Args: - prim_path: USD prim path pattern to match prims. - device: Device to place the tensors on. Defaults to "cpu". - stage: USD stage to search for prims. If None, uses the current active stage. + prim_path: USD prim path pattern to match prims. Supports wildcards (``*``) and + regex patterns (e.g., ``"/World/Env_.*/Robot"``). See + :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. + device: Device to place the tensors on. Can be ``"cpu"`` or CUDA devices like + ``"cuda:0"``. Defaults to ``"cpu"``. + stage: USD stage to search for prims. If None, uses the current active stage + from the simulation context. Defaults to None. Raises: - ValueError: If any matched prim is not Xformable. + ValueError: If any matched prim is not Xformable or doesn't have standardized + transform operations (translate, orient, scale in that order). """ stage = sim_utils.get_current_stage() if stage is None else stage + # Store configuration self._prim_path = prim_path - self._prims = sim_utils.find_matching_prims(prim_path) self._device = device - # check all prims are xformable with standard transform operations + # Find and validate matching prims + self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) + + # Validate all prims have standard xform operations for prim in self._prims: if not sim_utils.validate_standard_xform_ops(prim): raise ValueError( f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" - f" operations. Received type: '{prim.GetTypeName()}'." + f" operations [translate, orient, scale]. Received type: '{prim.GetTypeName()}'." + " Use sim_utils.standardize_xform_ops() to prepare the prim." ) @property @@ -64,6 +92,21 @@ def count(self) -> int: """ return len(self._prims) + @property + def prim_path(self) -> str: + """Prim path pattern used to match prims.""" + return self._prim_path + + @property + def prims(self) -> list[Usd.Prim]: + """List of USD prims being managed by this view.""" + return self._prims + + @property + def device(self) -> str: + """Device where tensors are allocated (cpu or cuda).""" + return self._device + """ Operations - Setters. """ @@ -109,6 +152,7 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t orientations_list = None # Set poses for each prim + # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): for idx, prim in enumerate(self._prims): # Get parent prim for local space conversion @@ -190,10 +234,11 @@ def set_local_poses( else: orientations_list = None # Set local poses for each prim + # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): for idx, prim in enumerate(self._prims): - local_pos = tuple(translations_list[idx]) if translations_list is not None else None - local_quat = tuple(orientations_list[idx]) if orientations_list is not None else None + local_pos = Gf.Vec3d(*translations_list[idx]) if translations_list is not None else None + local_quat = Gf.Quatd(*orientations_list[idx]) if orientations_list is not None else None # Get or create the standard transform operations xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) @@ -207,6 +252,28 @@ def set_local_poses( current_value = xform_op.Get() xform_op.Set(type(current_value)(*value) if current_value is not None else value) + def set_scales(self, scales: torch.Tensor): + """Set scales for all prims in the view. + + This method sets the scale of each prim in the view. + + Args: + scales: Scales as a tensor of shape (N, 3) where N is the number of prims. + """ + # Validate inputs + if scales.shape != (self.count, 3): + raise ValueError(f"Expected scales shape ({self.count}, 3), got {scales.shape}.") + + scales_list = scales.tolist() + # Set scales for each prim + # We use Sdf.ChangeBlock to minimize notification overhead. + with Sdf.ChangeBlock(): + for idx, prim in enumerate(self._prims): + scale = Gf.Vec3d(*scales_list[idx]) + xform_op_scale = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) + current_value = xform_op_scale.Get() + xform_op_scale.Set(type(current_value)(*scale) if current_value is not None else scale) + """ Operations - Getters. """ @@ -268,3 +335,20 @@ def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: orientations_tensor = torch.tensor(orientations, dtype=torch.float32, device=self._device) return translations_tensor, orientations_tensor + + def get_scales(self) -> torch.Tensor: + """Get scales for all prims in the view. + + This method retrieves the scale of each prim in the view. + + Returns: + A tensor of shape (N, 3) containing the scales of each prim. + """ + scales = [] + for prim in self._prims: + scale = sim_utils.resolve_prim_scale(prim) + scales.append(scale) + + # Convert to tensor + scales_tensor = torch.tensor(scales, dtype=torch.float32, device=self._device) + return scales_tensor From 8c5e7c1e4438374fa16f4db0e5b7e39591e0ebeb Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 08:10:45 +0100 Subject: [PATCH 025/100] fixes type error --- source/isaaclab/isaaclab/sim/views/xform_prim_view.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 355d7eaea31..20a2095edbf 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -186,11 +186,11 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t # set the data xform_ops = [xform_op_translate, xform_op_orient] - xform_values = [local_pos, local_quat] + xform_values = [Gf.Vec3d(*local_pos), Gf.Quatd(*local_quat)] # type: ignore for xform_op, value in zip(xform_ops, xform_values): if value is not None: current_value = xform_op.Get() - xform_op.Set(type(current_value)(*value) if current_value is not None else value) + xform_op.Set(type(current_value)(value) if current_value is not None else value) def set_local_poses( self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None @@ -250,7 +250,7 @@ def set_local_poses( for xform_op, value in zip(xform_ops, xform_values): if value is not None: current_value = xform_op.Get() - xform_op.Set(type(current_value)(*value) if current_value is not None else value) + xform_op.Set(type(current_value)(value) if current_value is not None else value) def set_scales(self, scales: torch.Tensor): """Set scales for all prims in the view. From 618e2f13958e695dd169b2f5e7a4ff5bbfc80e17 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 13:41:44 +0100 Subject: [PATCH 026/100] adds test for xform prim view --- .../isaaclab/test/sim/test_xform_prim_view.py | 790 ++++++++++++++++++ 1 file changed, 790 insertions(+) create mode 100644 source/isaaclab/test/sim/test_xform_prim_view.py diff --git a/source/isaaclab/test/sim/test_xform_prim_view.py b/source/isaaclab/test/sim/test_xform_prim_view.py new file mode 100644 index 00000000000..44abac10019 --- /dev/null +++ b/source/isaaclab/test/sim/test_xform_prim_view.py @@ -0,0 +1,790 @@ +# 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 + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import torch + +import pytest + +try: + from isaacsim.core.prims import XFormPrim as _IsaacSimXFormPrimView +except (ModuleNotFoundError, ImportError): + _IsaacSimXFormPrimView = None + +import isaaclab.sim as sim_utils +from isaaclab.sim.views import XFormPrimView as XFormPrimView + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + # Setup: Create a new stage + sim_utils.create_new_stage() + sim_utils.update_stage() + + # Yield for the test + yield + + # Teardown: Clear stage after each test + sim_utils.clear_stage() + + +def assert_tensors_close( + t1: torch.Tensor, t2: torch.Tensor, rtol: float = 1e-5, atol: float = 1e-5, check_quat_sign: bool = False +): + """Assert two tensors are close, with optional quaternion sign handling.""" + if check_quat_sign: + # For quaternions, q and -q represent the same rotation + # Try both normal and sign-flipped comparison + try: + torch.testing.assert_close(t1, t2, rtol=rtol, atol=atol) + except AssertionError: + # Try with sign flipped + torch.testing.assert_close(t1, -t2, rtol=rtol, atol=atol) + else: + torch.testing.assert_close(t1, t2, rtol=rtol, atol=atol) + + +""" +Tests - Initialization. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_single_prim(device): + """Test XFormPrimView initialization with a single prim.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + # Create a single xform prim + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Object", "Xform", translation=(1.0, 2.0, 3.0), stage=stage) + + # Create view + view = XFormPrimView("/World/Object", device=device) + + # Verify properties + assert view.count == 1 + assert view.prim_path == "/World/Object" + assert view.device == device + assert len(view.prims) == 1 + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_multiple_prims(device): + """Test XFormPrimView initialization with multiple prims using pattern matching.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + # Create multiple prims + num_prims = 10 + stage = sim_utils.get_current_stage() + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(i * 2.0, 0.0, 1.0), stage=stage) + + # Create view with pattern + view = XFormPrimView("/World/Env_.*/Object", device=device) + + # Verify properties + assert view.count == num_prims + assert view.device == device + assert len(view.prims) == num_prims + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_invalid_prim(device): + """Test XFormPrimView initialization fails for non-xformable prims.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create a prim with non-standard xform operations + stage.DefinePrim("/World/InvalidPrim", "Xform") + + # XFormPrimView should raise ValueError because prim doesn't have standard operations + with pytest.raises(ValueError, match="not a xformable prim"): + XFormPrimView("/World/InvalidPrim", device=device) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_xform_prim_view_initialization_empty_pattern(device): + """Test XFormPrimView initialization with pattern that matches no prims.""" + # check if CUDA is available + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + sim_utils.create_new_stage() + + # Create view with pattern that matches nothing + view = XFormPrimView("/World/NonExistent_.*", device=device) + + # Should have zero count + assert view.count == 0 + assert len(view.prims) == 0 + + +""" +Tests - Get/Set World Poses. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_world_poses(device): + """Test getting world poses from XFormPrimView.""" + if device.startswith("cuda") and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims with known world poses + expected_positions = [(1.0, 2.0, 3.0), (4.0, 5.0, 6.0), (7.0, 8.0, 9.0)] + expected_orientations = [(1.0, 0.0, 0.0, 0.0), (0.7071068, 0.0, 0.0, 0.7071068), (0.7071068, 0.7071068, 0.0, 0.0)] + + for i, (pos, quat) in enumerate(zip(expected_positions, expected_orientations)): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=pos, orientation=quat, stage=stage) + + # Create view + view = XFormPrimView("/World/Object_.*", device=device) + + # Get world poses + positions, orientations = view.get_world_poses() + + # Verify shapes + assert positions.shape == (3, 3) + assert orientations.shape == (3, 4) + + # Convert expected values to tensors + expected_positions_tensor = torch.tensor(expected_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_orientations, dtype=torch.float32, device=device) + + # Verify positions + torch.testing.assert_close(positions, expected_positions_tensor, atol=1e-5, rtol=0) + + # Verify orientations (allow for quaternion sign ambiguity) + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses(device): + """Test setting world poses in XFormPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view + view = XFormPrimView("/World/Object_.*", device=device) + + # Set new world poses + new_positions = torch.tensor( + [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0], [13.0, 14.0, 15.0]], device=device + ) + new_orientations = torch.tensor( + [ + [1.0, 0.0, 0.0, 0.0], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.7071068, 0.7071068, 0.0, 0.0], + [0.9238795, 0.3826834, 0.0, 0.0], + [0.7071068, 0.0, 0.7071068, 0.0], + ], + device=device, + ) + + view.set_world_poses(new_positions, new_orientations) + + # Get the poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Verify they match + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + # Check quaternions (allow sign flip) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses_only_positions(device): + """Test setting only positions, leaving orientations unchanged.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims with specific orientations + initial_quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around Z + for i in range(3): + sim_utils.create_prim( + f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage + ) + + # Create view + view = XFormPrimView("/World/Object_.*", device=device) + + # Get initial orientations + _, initial_orientations = view.get_world_poses() + + # Set only positions + new_positions = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) + view.set_world_poses(positions=new_positions, orientations=None) + + # Get poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Positions should be updated + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + + # Orientations should be unchanged + try: + torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses_only_orientations(device): + """Test setting only orientations, leaving positions unchanged.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims with specific positions + for i in range(3): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + + # Create view + view = XFormPrimView("/World/Object_.*", device=device) + + # Get initial positions + initial_positions, _ = view.get_world_poses() + + # Set only orientations + new_orientations = torch.tensor( + [[0.7071068, 0.0, 0.0, 0.7071068], [0.7071068, 0.7071068, 0.0, 0.0], [0.9238795, 0.3826834, 0.0, 0.0]], + device=device, + ) + view.set_world_poses(positions=None, orientations=new_orientations) + + # Get poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Positions should be unchanged + torch.testing.assert_close(retrieved_positions, initial_positions, atol=1e-5, rtol=0) + + # Orientations should be updated + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses_with_hierarchy(device): + """Test setting world poses correctly handles parent transformations.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create parent prims + for i in range(3): + parent_pos = (i * 10.0, 0.0, 0.0) + parent_quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around Z + sim_utils.create_prim( + f"/World/Parent_{i}", "Xform", translation=parent_pos, orientation=parent_quat, stage=stage + ) + # Create child prims + sim_utils.create_prim(f"/World/Parent_{i}/Child", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view for children + view = XFormPrimView("/World/Parent_.*/Child", device=device) + + # Set world poses for children + desired_world_positions = torch.tensor([[5.0, 5.0, 0.0], [15.0, 5.0, 0.0], [25.0, 5.0, 0.0]], device=device) + desired_world_orientations = torch.tensor( + [[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]], device=device + ) + + view.set_world_poses(desired_world_positions, desired_world_orientations) + + # Get world poses back + retrieved_positions, retrieved_orientations = view.get_world_poses() + + # Should match desired world poses + torch.testing.assert_close(retrieved_positions, desired_world_positions, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, desired_world_orientations, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -desired_world_orientations, atol=1e-4, rtol=0) + + +""" +Tests - Get/Set Local Poses. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_local_poses(device): + """Test getting local poses from XFormPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create parent and child prims + sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) + + # Children with different local poses + expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] + expected_local_orientations = [ + (1.0, 0.0, 0.0, 0.0), + (0.7071068, 0.0, 0.0, 0.7071068), + (0.7071068, 0.7071068, 0.0, 0.0), + ] + + for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=pos, orientation=quat, stage=stage) + + # Create view + view = XFormPrimView("/World/Parent/Child_.*", device=device) + + # Get local poses + translations, orientations = view.get_local_poses() + + # Verify shapes + assert translations.shape == (3, 3) + assert orientations.shape == (3, 4) + + # Convert expected values to tensors + expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) + + # Verify translations + torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) + + # Verify orientations (allow for quaternion sign ambiguity) + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_local_poses(device): + """Test setting local poses in XFormPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create parent + sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 5.0), stage=stage) + + # Create children + num_prims = 4 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + # Create view + view = XFormPrimView("/World/Parent/Child_.*", device=device) + + # Set new local poses + new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0], [4.0, 4.0, 4.0]], device=device) + new_orientations = torch.tensor( + [ + [1.0, 0.0, 0.0, 0.0], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.7071068, 0.7071068, 0.0, 0.0], + [0.9238795, 0.3826834, 0.0, 0.0], + ], + device=device, + ) + + view.set_local_poses(new_translations, new_orientations) + + # Get local poses back + retrieved_translations, retrieved_orientations = view.get_local_poses() + + # Verify they match + torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_local_poses_only_translations(device): + """Test setting only local translations.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create parent and children with specific orientations + sim_utils.create_prim("/World/Parent", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + initial_quat = (0.7071068, 0.0, 0.0, 0.7071068) + + for i in range(3): + sim_utils.create_prim( + f"/World/Parent/Child_{i}", "Xform", translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage + ) + + # Create view + view = XFormPrimView("/World/Parent/Child_.*", device=device) + + # Get initial orientations + _, initial_orientations = view.get_local_poses() + + # Set only translations + new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) + view.set_local_poses(translations=new_translations, orientations=None) + + # Get poses back + retrieved_translations, retrieved_orientations = view.get_local_poses() + + # Translations should be updated + torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) + + # Orientations should be unchanged + try: + torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) + + +""" +Tests - Get/Set Scales. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_scales(device): + """Test getting scales from XFormPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims with different scales + expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] + + for i, scale in enumerate(expected_scales): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) + + # Create view + view = XFormPrimView("/World/Object_.*", device=device) + + # Get scales + scales = view.get_scales() + + # Verify shape and values + assert scales.shape == (3, 3) + expected_scales_tensor = torch.tensor(expected_scales, dtype=torch.float32, device=device) + torch.testing.assert_close(scales, expected_scales_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_scales(device): + """Test setting scales in XFormPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=(1.0, 1.0, 1.0), stage=stage) + + # Create view + view = XFormPrimView("/World/Object_.*", device=device) + + # Set new scales + new_scales = torch.tensor( + [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device + ) + + view.set_scales(new_scales) + + # Get scales back + retrieved_scales = view.get_scales() + + # Verify they match + torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) + + +""" +Tests - Comparison with Isaac Sim Implementation. +""" + + +def test_compare_get_world_poses_with_isaacsim(): + """Compare get_world_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXFormPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create prims with various poses + num_prims = 10 + for i in range(num_prims): + pos = (i * 2.0, i * 0.5, i * 1.5) + # Vary orientations + if i % 3 == 0: + quat = (1.0, 0.0, 0.0, 0.0) # Identity + elif i % 3 == 1: + quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around Z + else: + quat = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=pos, orientation=quat, stage=stage) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XFormPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + + # Get world poses from both + isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() + isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_pos, torch.Tensor): + isaacsim_pos = torch.tensor(isaacsim_pos, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results + torch.testing.assert_close(isaaclab_pos, isaacsim_pos, atol=1e-5, rtol=0) + + # Compare quaternions (account for sign ambiguity) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-5, rtol=0) + + +def test_compare_set_world_poses_with_isaacsim(): + """Compare set_world_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXFormPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create prims + num_prims = 8 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XFormPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + + # Generate new poses + new_positions = torch.randn(num_prims, 3) * 10.0 + new_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32) + + # Set poses using both implementations + isaaclab_view.set_world_poses(new_positions.clone(), new_orientations.clone()) + isaacsim_view.set_world_poses(new_positions.clone(), new_orientations.clone()) + + # Get poses back from both + isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() + isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_pos, torch.Tensor): + isaacsim_pos = torch.tensor(isaacsim_pos, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results - both implementations should produce the same world poses + torch.testing.assert_close(isaaclab_pos, isaacsim_pos, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) + + +def test_compare_get_local_poses_with_isaacsim(): + """Compare get_local_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXFormPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create hierarchical prims + num_prims = 5 + for i in range(num_prims): + # Create parent + sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 5.0, 0.0, 0.0), stage=stage) + # Create child with local pose + local_pos = (1.0, float(i), 0.0) + local_quat = (1.0, 0.0, 0.0, 0.0) if i % 2 == 0 else (0.7071068, 0.0, 0.0, 0.7071068) + sim_utils.create_prim( + f"/World/Env_{i}/Object", "Xform", translation=local_pos, orientation=local_quat, stage=stage + ) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XFormPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + + # Get local poses from both + isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() + isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_trans, torch.Tensor): + isaacsim_trans = torch.tensor(isaacsim_trans, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results + torch.testing.assert_close(isaaclab_trans, isaacsim_trans, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-5, rtol=0) + + +def test_compare_set_local_poses_with_isaacsim(): + """Compare set_local_poses with Isaac Sim's implementation.""" + stage = sim_utils.get_current_stage() + + # Check if Isaac Sim is available + if _IsaacSimXFormPrimView is None: + pytest.skip("Isaac Sim is not available") + + # Create hierarchical prims + num_prims = 6 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0.0, 0.0), stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + + pattern = "/World/Env_.*/Object" + + # Create both views + isaaclab_view = XFormPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + + # Generate new local poses + new_translations = torch.randn(num_prims, 3) * 5.0 + new_orientations = torch.tensor( + [[1.0, 0.0, 0.0, 0.0], [0.7071068, 0.0, 0.0, 0.7071068]] * (num_prims // 2), dtype=torch.float32 + ) + + # Set local poses using both implementations + isaaclab_view.set_local_poses(new_translations.clone(), new_orientations.clone()) + isaacsim_view.set_local_poses(new_translations.clone(), new_orientations.clone()) + + # Get local poses back from both + isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() + isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() + + # Convert Isaac Sim results to torch tensors if needed + if not isinstance(isaacsim_trans, torch.Tensor): + isaacsim_trans = torch.tensor(isaacsim_trans, dtype=torch.float32) + if not isinstance(isaacsim_quat, torch.Tensor): + isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32) + + # Compare results + torch.testing.assert_close(isaaclab_trans, isaacsim_trans, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) + + +""" +Tests - Complex Scenarios. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_complex_hierarchy_world_local_consistency(device): + """Test that world and local poses are consistent in complex hierarchies.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create complex hierarchy: Grandparent -> Parent -> Child + num_envs = 3 + for i in range(num_envs): + # Grandparent + sim_utils.create_prim( + f"/World/Grandparent_{i}", + "Xform", + translation=(i * 20.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + # Parent + sim_utils.create_prim( + f"/World/Grandparent_{i}/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), + orientation=(0.7071068, 0.7071068, 0.0, 0.0), + stage=stage, + ) + # Child + sim_utils.create_prim(f"/World/Grandparent_{i}/Parent/Child", "Xform", translation=(1.0, 2.0, 3.0), stage=stage) + + # Create view for children + view = XFormPrimView("/World/Grandparent_.*/Parent/Child", device=device) + + # Get world and local poses + world_pos, world_quat = view.get_world_poses() + local_trans, local_quat = view.get_local_poses() + + # Change local poses + new_local_trans = torch.tensor([[2.0, 3.0, 4.0], [3.0, 4.0, 5.0], [4.0, 5.0, 6.0]], device=device) + view.set_local_poses(translations=new_local_trans, orientations=None) + + # Get world poses again + new_world_pos, new_world_quat = view.get_world_poses() + + # World poses should have changed when local poses changed + assert not torch.allclose(world_pos, new_world_pos, atol=1e-5) + + # Now set world poses back to original + view.set_world_poses(world_pos, world_quat) + + # Get world poses again + final_world_pos, final_world_quat = view.get_world_poses() + + # Should match original world poses + torch.testing.assert_close(final_world_pos, world_pos, atol=1e-4, rtol=0) From bd98ddafb0440cb8eddb2d080983a4d406b7f11b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 13:51:52 +0100 Subject: [PATCH 027/100] adds clear op --- .../isaaclab/sim/simulation_context.py | 16 ++++++ source/isaaclab/isaaclab/sim/utils/stage.py | 50 +++++++++---------- 2 files changed, 40 insertions(+), 26 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 61e6d174fce..9b23b29708c 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -624,6 +624,22 @@ def render(self, mode: RenderMode | None = None): if "cuda" in self.device: torch.cuda.set_device(self.device) + def clear(self): + """Clear the current USD stage.""" + + def _predicate(prim: Usd.Prim) -> bool: + """Check if the prim should be deleted. + + It adds a check for '/World' and 'PhysicsScene' prims. + """ + if prim.GetPath().pathString == "/World": + return False + if prim.GetTypeName() == "PhysicsScene": + return False + return True + + sim_utils.clear_stage(predicate=_predicate) + """ Operations - Override (extension) """ diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index b4a580fffe4..003863179dd 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -293,55 +293,53 @@ def clear_stage(predicate: Callable[[Usd.Prim], bool] | None = None) -> None: """Deletes all prims in the stage without populating the undo command buffer. The function will delete all prims in the stage that satisfy the predicate. If the predicate - is None, a default predicate will be used that deletes all prims. The default predicate deletes - all prims that are not the root prim, are not under the /Render namespace, have the ``no_delete`` - metadata, are not ancestral to any other prim, and are not hidden in the stage window. + is None, a default predicate will be used that deletes all prims except those that meet any + of the following criteria: + + * The root prim (``"/"``) or prims under the ``/Render`` namespace + * Prims with the ``no_delete`` metadata set + * Prims with the ``hide_in_stage_window`` metadata set + * Ancestral prims (prims that are ancestors of other prims in a reference/payload chain) Args: - predicate: A user defined function that takes the USD prim as an argument and - returns a boolean indicating if the prim should be deleted. If the predicate is None, - a default predicate will be used that deletes all prims. + predicate: A user-defined function that takes a USD prim as an argument and + returns a boolean indicating if the prim should be deleted. If None, all + prims will be considered for deletion (subject to the default exclusions above). + If provided, both the default exclusions and the predicate must be satisfied + for a prim to be deleted. Example: >>> import isaaclab.sim as sim_utils >>> - >>> # clear the whole stage + >>> # Clear the whole stage (except protected prims) >>> sim_utils.clear_stage() >>> - >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Given the stage: /World/Cube, /World/Cube_01, /World/Cube_02 >>> # Delete only the prims of type Cube >>> predicate = lambda _prim: _prim.GetTypeName() == "Cube" - >>> sim_utils.clear_stage(predicate) # after the execution the stage will be /World + >>> sim_utils.clear_stage(predicate) # After execution, the stage will only have /World """ # Note: Need to import this here to prevent circular dependencies. from .prims import delete_prim from .queries import get_all_matching_child_prims - def _default_predicate(prim: Usd.Prim) -> bool: + def _predicate(prim: Usd.Prim) -> bool: """Check if the prim should be deleted.""" prim_path = prim.GetPath().pathString - if prim_path == "/": - return False - if prim_path.startswith("/Render"): - return False - if prim.GetMetadata("no_delete"): + # Skip the root prim + if prim_path == "/" or prim_path == "/Render": return False - if prim.GetMetadata("hide_in_stage_window"): + # Skip prims with the "no_delete" metadata + if prim.GetMetadata("no_delete") or prim.GetMetadata("hide_in_stage_window"): return False + # Skip ancestral prims if omni.usd.check_ancestral(prim): return False - return True - - def _predicate_from_path(prim: Usd.Prim) -> bool: - if predicate is None: - return _default_predicate(prim) - return predicate(prim) + # Additional predicate check + return predicate is None or predicate(prim) # get all prims to delete - if predicate is None: - prims = get_all_matching_child_prims("/", _default_predicate) - else: - prims = get_all_matching_child_prims("/", _predicate_from_path) + prims = get_all_matching_child_prims("/", _predicate) # convert prims to prim paths prim_paths_to_delete = [prim.GetPath().pathString for prim in prims] # delete prims From 56cf54de822e8e08dedc40be81125b87c3192d3b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 14:04:38 +0100 Subject: [PATCH 028/100] adds franka test --- .../isaaclab/test/sim/test_xform_prim_view.py | 176 ++++++++++-------- 1 file changed, 97 insertions(+), 79 deletions(-) diff --git a/source/isaaclab/test/sim/test_xform_prim_view.py b/source/isaaclab/test/sim/test_xform_prim_view.py index 44abac10019..59c4c17824e 100644 --- a/source/isaaclab/test/sim/test_xform_prim_view.py +++ b/source/isaaclab/test/sim/test_xform_prim_view.py @@ -23,6 +23,7 @@ import isaaclab.sim as sim_utils from isaaclab.sim.views import XFormPrimView as XFormPrimView +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @pytest.fixture(autouse=True) @@ -39,22 +40,6 @@ def test_setup_teardown(): sim_utils.clear_stage() -def assert_tensors_close( - t1: torch.Tensor, t2: torch.Tensor, rtol: float = 1e-5, atol: float = 1e-5, check_quat_sign: bool = False -): - """Assert two tensors are close, with optional quaternion sign handling.""" - if check_quat_sign: - # For quaternions, q and -q represent the same rotation - # Try both normal and sign-flipped comparison - try: - torch.testing.assert_close(t1, t2, rtol=rtol, atol=atol) - except AssertionError: - # Try with sign flipped - torch.testing.assert_close(t1, -t2, rtol=rtol, atol=atol) - else: - torch.testing.assert_close(t1, t2, rtol=rtol, atol=atol) - - """ Tests - Initialization. """ @@ -538,6 +523,102 @@ def test_set_scales(device): torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) +""" +Tests - Integration. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_with_franka_robots(device): + """Test XFormPrimView with real Franka robot USD assets.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Load Franka robot assets + franka_usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka_instanceable.usd" + + # Add two Franka robots to the stage + sim_utils.create_prim("/World/Franka_1", "Xform", usd_path=franka_usd_path, stage=stage) + sim_utils.create_prim("/World/Franka_2", "Xform", usd_path=franka_usd_path, stage=stage) + + # Create view for both Frankas + frankas_view = XFormPrimView("/World/Franka_.*", device=device) + + # Verify count + assert frankas_view.count == 2 + + # Get initial world poses (should be at origin) + initial_positions, initial_orientations = frankas_view.get_world_poses() + + # Verify initial positions are at origin + expected_initial_positions = torch.zeros(2, 3, device=device) + torch.testing.assert_close(initial_positions, expected_initial_positions, atol=1e-5, rtol=0) + + # Verify initial orientations are identity + expected_initial_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]], device=device) + try: + torch.testing.assert_close(initial_orientations, expected_initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(initial_orientations, -expected_initial_orientations, atol=1e-5, rtol=0) + + # Set new world poses + new_positions = torch.tensor([[10.0, 10.0, 0.0], [-40.0, -40.0, 0.0]], device=device) + # 90° rotation around Z axis for first, -90° for second + new_orientations = torch.tensor( + [[0.7071068, 0.0, 0.0, 0.7071068], [0.7071068, 0.0, 0.0, -0.7071068]], device=device + ) + + frankas_view.set_world_poses(positions=new_positions, orientations=new_orientations) + + # Get poses back and verify + retrieved_positions, retrieved_orientations = frankas_view.get_world_poses() + + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_with_nested_targets(device): + """Test with nested frame/target structure similar to Isaac Sim tests.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create frames and targets + for i in range(1, 4): + sim_utils.create_prim(f"/World/Frame_{i}", "Xform", stage=stage) + sim_utils.create_prim(f"/World/Frame_{i}/Target", "Xform", stage=stage) + + # Create views + frames_view = XFormPrimView("/World/Frame_.*", device=device) + targets_view = XFormPrimView("/World/Frame_.*/Target", device=device) + + assert frames_view.count == 3 + assert targets_view.count == 3 + + # Set local poses for frames + frame_translations = torch.tensor([[0.0, 0.0, 0.0], [0.0, 10.0, 5.0], [0.0, 3.0, 5.0]], device=device) + frames_view.set_local_poses(translations=frame_translations) + + # Set local poses for targets + target_translations = torch.tensor([[0.0, 20.0, 10.0], [0.0, 30.0, 20.0], [0.0, 50.0, 10.0]], device=device) + targets_view.set_local_poses(translations=target_translations) + + # Get world poses of targets + world_positions, _ = targets_view.get_world_poses() + + # Expected world positions are frame_translation + target_translation + expected_positions = torch.tensor([[0.0, 20.0, 10.0], [0.0, 40.0, 25.0], [0.0, 53.0, 15.0]], device=device) + + torch.testing.assert_close(world_positions, expected_positions, atol=1e-5, rtol=0) + + """ Tests - Comparison with Isaac Sim Implementation. """ @@ -725,66 +806,3 @@ def test_compare_set_local_poses_with_isaacsim(): torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) except AssertionError: torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) - - -""" -Tests - Complex Scenarios. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_complex_hierarchy_world_local_consistency(device): - """Test that world and local poses are consistent in complex hierarchies.""" - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - stage = sim_utils.get_current_stage() - - # Create complex hierarchy: Grandparent -> Parent -> Child - num_envs = 3 - for i in range(num_envs): - # Grandparent - sim_utils.create_prim( - f"/World/Grandparent_{i}", - "Xform", - translation=(i * 20.0, 0.0, 0.0), - orientation=(0.7071068, 0.0, 0.0, 0.7071068), - scale=(2.0, 2.0, 2.0), - stage=stage, - ) - # Parent - sim_utils.create_prim( - f"/World/Grandparent_{i}/Parent", - "Xform", - translation=(5.0, 0.0, 0.0), - orientation=(0.7071068, 0.7071068, 0.0, 0.0), - stage=stage, - ) - # Child - sim_utils.create_prim(f"/World/Grandparent_{i}/Parent/Child", "Xform", translation=(1.0, 2.0, 3.0), stage=stage) - - # Create view for children - view = XFormPrimView("/World/Grandparent_.*/Parent/Child", device=device) - - # Get world and local poses - world_pos, world_quat = view.get_world_poses() - local_trans, local_quat = view.get_local_poses() - - # Change local poses - new_local_trans = torch.tensor([[2.0, 3.0, 4.0], [3.0, 4.0, 5.0], [4.0, 5.0, 6.0]], device=device) - view.set_local_poses(translations=new_local_trans, orientations=None) - - # Get world poses again - new_world_pos, new_world_quat = view.get_world_poses() - - # World poses should have changed when local poses changed - assert not torch.allclose(world_pos, new_world_pos, atol=1e-5) - - # Now set world poses back to original - view.set_world_poses(world_pos, world_quat) - - # Get world poses again - final_world_pos, final_world_quat = view.get_world_poses() - - # Should match original world poses - torch.testing.assert_close(final_world_pos, world_pos, atol=1e-4, rtol=0) From cbbccdf3a5ecbdfabad86ae64e873bb30ddf6a61 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 14:20:51 +0100 Subject: [PATCH 029/100] fixes check for non-xform xformable --- .../isaaclab/isaaclab/sim/utils/transforms.py | 10 +- .../test/sim/test_utils_transforms.py | 720 ++++++++++-------- 2 files changed, 401 insertions(+), 329 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 248a2e1186d..65aecaee951 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -379,15 +379,15 @@ def convert_world_pose_to_local( ``local_transform = world_transform * inverse(ref_world_transform)`` .. note:: - If the reference prim is invalid or is the root path, the position and orientation are returned + If the reference prim is the root prim ("/"), the position and orientation are returned unchanged, as they are already effectively in local/world space. Args: position: The world-space position as (x, y, z). orientation: The world-space orientation as quaternion (w, x, y, z). If None, only position is converted and None is returned for orientation. - ref_prim: The reference USD prim to compute the local transform relative to. If this is invalid or - is the root path, the world pose is returned unchanged. + ref_prim: The reference USD prim to compute the local transform relative to. If this is + the root prim ("/"), the world pose is returned unchanged. Returns: A tuple of (local_translation, local_orientation) where: @@ -397,7 +397,6 @@ def convert_world_pose_to_local( Raises: ValueError: If the reference prim is not a valid USD prim. - ValueError: If the reference prim is not a valid USD Xformable. Example: >>> import isaaclab.sim as sim_utils @@ -426,9 +425,6 @@ def convert_world_pose_to_local( # Check if reference prim is a valid xformable ref_xformable = UsdGeom.Xformable(ref_prim) - if not ref_xformable: - raise ValueError(f"Reference prim at path '{ref_prim.GetPath().pathString}' is not a valid xformable.") - # Get reference prim's world transform ref_world_tf = ref_xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 23e0c7af6a4..c3cbdbd3f74 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -68,7 +68,7 @@ def get_xform_ops(prim: Usd.Prim) -> list[str]: """ -Tests. +Test standardize_xform_ops() function. """ @@ -731,327 +731,9 @@ def test_standardize_xform_ops_preserves_float_precision(): assert_quat_close(Gf.Quatd(*quat_after), new_orientation, eps=1e-4) -def test_convert_world_pose_to_local_basic(): - """Test basic world-to-local pose conversion.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Create parent and child prims - parent_prim = sim_utils.create_prim( - "/World/Parent", - "Xform", - translation=(5.0, 0.0, 0.0), - orientation=(1.0, 0.0, 0.0, 0.0), # identity rotation - scale=(1.0, 1.0, 1.0), - stage=stage, - ) - - # World pose we want to achieve for a child - world_position = (10.0, 3.0, 0.0) - world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation - - # Convert to local space - local_translation, local_orientation = sim_utils.convert_world_pose_to_local( - world_position, world_orientation, parent_prim - ) - # Assert orientation is not None - assert local_orientation is not None - - # The expected local translation is world_position - parent_position = (10-5, 3-0, 0-0) = (5, 3, 0) - assert_vec3_close(Gf.Vec3d(*local_translation), (5.0, 3.0, 0.0), eps=1e-5) - assert_quat_close(Gf.Quatd(*local_orientation), (1.0, 0.0, 0.0, 0.0), eps=1e-5) - - -def test_convert_world_pose_to_local_with_rotation(): - """Test world-to-local conversion with parent rotation.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Create parent with 90-degree rotation around Z axis - parent_prim = sim_utils.create_prim( - "/World/RotatedParent", - "Xform", - translation=(0.0, 0.0, 0.0), - orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z - scale=(1.0, 1.0, 1.0), - stage=stage, - ) - - # World pose: position at (1, 0, 0) with identity rotation - world_position = (1.0, 0.0, 0.0) - world_orientation = (1.0, 0.0, 0.0, 0.0) - - # Convert to local space - local_translation, local_orientation = sim_utils.convert_world_pose_to_local( - world_position, world_orientation, parent_prim - ) - - # Create a child with the local transform and verify world pose - child_prim = sim_utils.create_prim( - "/World/RotatedParent/Child", - "Xform", - translation=local_translation, - orientation=local_orientation, - stage=stage, - ) - - # Get world pose of child - child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) - - # Verify it matches the desired world pose - assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-5) - assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-5) - - -def test_convert_world_pose_to_local_with_scale(): - """Test world-to-local conversion with parent scale.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Create parent with non-uniform scale - parent_prim = sim_utils.create_prim( - "/World/ScaledParent", - "Xform", - translation=(1.0, 2.0, 3.0), - orientation=(1.0, 0.0, 0.0, 0.0), - scale=(2.0, 2.0, 2.0), - stage=stage, - ) - - # World pose we want - world_position = (5.0, 6.0, 7.0) - world_orientation = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X - - # Convert to local space - local_translation, local_orientation = sim_utils.convert_world_pose_to_local( - world_position, world_orientation, parent_prim - ) - - # Create child and verify - child_prim = sim_utils.create_prim( - "/World/ScaledParent/Child", - "Xform", - translation=local_translation, - orientation=local_orientation, - stage=stage, - ) - - # Get world pose - child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) - - # Verify (may have some tolerance due to scale effects on rotation) - assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) - assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) - - -def test_convert_world_pose_to_local_invalid_parent(): - """Test world-to-local conversion with invalid parent returns world pose unchanged.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Get an invalid prim - invalid_prim = stage.GetPrimAtPath("/World/NonExistent") - assert not invalid_prim.IsValid() - - world_position = (10.0, 20.0, 30.0) - world_orientation = (0.7071068, 0.0, 0.7071068, 0.0) - - # Convert with invalid reference prim - with pytest.raises(ValueError): - sim_utils.convert_world_pose_to_local(world_position, world_orientation, invalid_prim) - - -def test_convert_world_pose_to_local_root_parent(): - """Test world-to-local conversion with root as parent returns world pose unchanged.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Get the pseudo-root prim - root_prim = stage.GetPrimAtPath("/") - - world_position = (15.0, 25.0, 35.0) - world_orientation = (0.9238795, 0.3826834, 0.0, 0.0) - - # Convert with root parent - local_translation, local_orientation = sim_utils.convert_world_pose_to_local( - world_position, world_orientation, root_prim - ) - # Assert orientation is not None - assert local_orientation is not None - - # Should return unchanged - assert_vec3_close(Gf.Vec3d(*local_translation), world_position, eps=1e-10) - assert_quat_close(Gf.Quatd(*local_orientation), world_orientation, eps=1e-10) - - -def test_convert_world_pose_to_local_none_orientation(): - """Test world-to-local conversion with None orientation.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Create parent - parent_prim = sim_utils.create_prim( - "/World/ParentNoOrient", - "Xform", - translation=(3.0, 4.0, 5.0), - orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z - stage=stage, - ) - - world_position = (10.0, 10.0, 10.0) - - # Convert with None orientation - local_translation, local_orientation = sim_utils.convert_world_pose_to_local(world_position, None, parent_prim) - - # Orientation should be None - assert local_orientation is None - # Translation should still be converted - assert local_translation is not None - - -def test_convert_world_pose_to_local_complex_hierarchy(): - """Test world-to-local conversion in a complex hierarchy.""" - # obtain stage handle - stage = sim_utils.get_current_stage() - - # Create a complex hierarchy - _ = sim_utils.create_prim( - "/World/Grandparent", - "Xform", - translation=(10.0, 0.0, 0.0), - orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z - scale=(2.0, 2.0, 2.0), - stage=stage, - ) - - parent = sim_utils.create_prim( - "/World/Grandparent/Parent", - "Xform", - translation=(5.0, 0.0, 0.0), # local to grandparent - orientation=(0.7071068, 0.7071068, 0.0, 0.0), # 90 deg around X - scale=(0.5, 0.5, 0.5), - stage=stage, - ) - - # World pose we want to achieve - world_position = (20.0, 15.0, 10.0) - world_orientation = (1.0, 0.0, 0.0, 0.0) - - # Convert to local space relative to parent - local_translation, local_orientation = sim_utils.convert_world_pose_to_local( - world_position, world_orientation, parent - ) - - # Create child with the computed local transform - child = sim_utils.create_prim( - "/World/Grandparent/Parent/Child", - "Xform", - translation=local_translation, - orientation=local_orientation, - stage=stage, - ) - - # Verify world pose - child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) - - # Should match the desired world pose (with some tolerance for complex transforms) - assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) - assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) - - -def test_resolve_prim_pose(): - """Test resolve_prim_pose() function.""" - # number of objects - num_objects = 20 - # sample random scales for x, y, z - rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) - rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) - # sample random positions - rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) - # sample random rotations - rand_quats = np.random.randn(num_objects, 3, 4) - rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) - - # create objects - for i in range(num_objects): - # simple cubes - cube_prim = sim_utils.create_prim( - f"/World/Cubes/instance_{i:02d}", - "Cube", - translation=rand_positions[i, 0], - orientation=rand_quats[i, 0], - scale=rand_scales[i, 0], - attributes={"size": rand_widths[i]}, - ) - # xform hierarchy - xform_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}", - "Xform", - translation=rand_positions[i, 1], - orientation=rand_quats[i, 1], - scale=rand_scales[i, 1], - ) - geometry_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/geometry", - "Sphere", - translation=rand_positions[i, 2], - orientation=rand_quats[i, 2], - scale=rand_scales[i, 2], - attributes={"radius": rand_widths[i]}, - ) - dummy_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/dummy", - "Sphere", - ) - - # cube prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(cube_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) - # xform prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(xform_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) - # dummy prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(dummy_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) - - # geometry prim w.r.t. xform prim - pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) - # TODO: Enabling scale causes the test to fail because the current implementation of - # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known - # limitation. Until this is fixed, the test is disabled here to ensure the test passes. - # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) - - # dummy prim w.r.t. xform prim - pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) - pos, quat = np.array(pos), np.array(quat) - np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) - np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) - # xform prim w.r.t. cube prim - pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) - pos, quat = np.array(pos), np.array(quat) - # -- compute ground truth values - gt_pos, gt_quat = math_utils.subtract_frame_transforms( - torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), - torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), - torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), - torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), - ) - gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() - quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, gt_pos, atol=1e-3) - np.testing.assert_allclose(quat, gt_quat, atol=1e-3) +""" +Test validate_standard_xform_ops() function. +""" def test_validate_standard_xform_ops_valid(): @@ -1268,6 +950,111 @@ def test_validate_standard_xform_ops_empty_prim(): assert sim_utils.validate_standard_xform_ops(prim) is False +""" +Test resolve_prim_pose() function. +""" + + +def test_resolve_prim_pose(): + """Test resolve_prim_pose() function.""" + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + # sample random rotations + rand_quats = np.random.randn(num_objects, 3, 4) + rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + orientation=rand_quats[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + orientation=rand_quats[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + orientation=rand_quats[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(cube_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) + # xform prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + # dummy prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(dummy_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + + # geometry prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) + # TODO: Enabling scale causes the test to fail because the current implementation of + # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known + # limitation. Until this is fixed, the test is disabled here to ensure the test passes. + # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) + + # dummy prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) + np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) + # xform prim w.r.t. cube prim + pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) + pos, quat = np.array(pos), np.array(quat) + # -- compute ground truth values + gt_pos, gt_quat = math_utils.subtract_frame_transforms( + torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), + torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), + ) + gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() + quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, gt_pos, atol=1e-3) + np.testing.assert_allclose(quat, gt_quat, atol=1e-3) + + +""" +Test resolve_prim_scale() function. +""" + + def test_resolve_prim_scale(): """Test resolve_prim_scale() function. @@ -1333,3 +1120,292 @@ def test_resolve_prim_scale(): scale = sim_utils.resolve_prim_scale(dummy_prim) scale = np.array(scale) np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + + +""" +Test convert_world_pose_to_local() function. +""" + + +def test_convert_world_pose_to_local_basic(): + """Test basic world-to-local pose conversion.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent and child prims + parent_prim = sim_utils.create_prim( + "/World/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), + orientation=(1.0, 0.0, 0.0, 0.0), # identity rotation + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # World pose we want to achieve for a child + world_position = (10.0, 3.0, 0.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + # Assert orientation is not None + assert local_orientation is not None + + # The expected local translation is world_position - parent_position = (10-5, 3-0, 0-0) = (5, 3, 0) + assert_vec3_close(Gf.Vec3d(*local_translation), (5.0, 3.0, 0.0), eps=1e-5) + assert_quat_close(Gf.Quatd(*local_orientation), (1.0, 0.0, 0.0, 0.0), eps=1e-5) + + +def test_convert_world_pose_to_local_with_rotation(): + """Test world-to-local conversion with parent rotation.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent with 90-degree rotation around Z axis + parent_prim = sim_utils.create_prim( + "/World/RotatedParent", + "Xform", + translation=(0.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # World pose: position at (1, 0, 0) with identity rotation + world_position = (1.0, 0.0, 0.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + + # Create a child with the local transform and verify world pose + child_prim = sim_utils.create_prim( + "/World/RotatedParent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Get world pose of child + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) + + # Verify it matches the desired world pose + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-5) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-5) + + +def test_convert_world_pose_to_local_with_scale(): + """Test world-to-local conversion with parent scale.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent with non-uniform scale + parent_prim = sim_utils.create_prim( + "/World/ScaledParent", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # World pose we want + world_position = (5.0, 6.0, 7.0) + world_orientation = (0.7071068, 0.7071068, 0.0, 0.0) # 90 deg around X + + # Convert to local space + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent_prim + ) + + # Create child and verify + child_prim = sim_utils.create_prim( + "/World/ScaledParent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Get world pose + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child_prim) + + # Verify (may have some tolerance due to scale effects on rotation) + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) + + +def test_convert_world_pose_to_local_invalid_parent(): + """Test world-to-local conversion with invalid parent returns world pose unchanged.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + assert not invalid_prim.IsValid() + + world_position = (10.0, 20.0, 30.0) + world_orientation = (0.7071068, 0.0, 0.7071068, 0.0) + + # Convert with invalid reference prim + with pytest.raises(ValueError): + sim_utils.convert_world_pose_to_local(world_position, world_orientation, invalid_prim) + + +def test_convert_world_pose_to_local_root_parent(): + """Test world-to-local conversion with root as parent returns world pose unchanged.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get the pseudo-root prim + root_prim = stage.GetPrimAtPath("/") + + world_position = (15.0, 25.0, 35.0) + world_orientation = (0.9238795, 0.3826834, 0.0, 0.0) + + # Convert with root parent + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, root_prim + ) + # Assert orientation is not None + assert local_orientation is not None + + # Should return unchanged + assert_vec3_close(Gf.Vec3d(*local_translation), world_position, eps=1e-10) + assert_quat_close(Gf.Quatd(*local_orientation), world_orientation, eps=1e-10) + + +def test_convert_world_pose_to_local_none_orientation(): + """Test world-to-local conversion with None orientation.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create parent + parent_prim = sim_utils.create_prim( + "/World/ParentNoOrient", + "Xform", + translation=(3.0, 4.0, 5.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + stage=stage, + ) + + world_position = (10.0, 10.0, 10.0) + + # Convert with None orientation + local_translation, local_orientation = sim_utils.convert_world_pose_to_local(world_position, None, parent_prim) + + # Orientation should be None + assert local_orientation is None + # Translation should still be converted + assert local_translation is not None + + +def test_convert_world_pose_to_local_complex_hierarchy(): + """Test world-to-local conversion in a complex hierarchy.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a complex hierarchy + _ = sim_utils.create_prim( + "/World/Grandparent", + "Xform", + translation=(10.0, 0.0, 0.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + parent = sim_utils.create_prim( + "/World/Grandparent/Parent", + "Xform", + translation=(5.0, 0.0, 0.0), # local to grandparent + orientation=(0.7071068, 0.7071068, 0.0, 0.0), # 90 deg around X + scale=(0.5, 0.5, 0.5), + stage=stage, + ) + + # World pose we want to achieve + world_position = (20.0, 15.0, 10.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) + + # Convert to local space relative to parent + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, parent + ) + + # Create child with the computed local transform + child = sim_utils.create_prim( + "/World/Grandparent/Parent/Child", + "Xform", + translation=local_translation, + orientation=local_orientation, + stage=stage, + ) + + # Verify world pose + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) + + # Should match the desired world pose (with some tolerance for complex transforms) + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-4) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) + + +def test_convert_world_pose_to_local_with_mixed_prim_types(): + """Test world-to-local conversion with mixed prim types (Xform, Scope, Mesh).""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a hierarchy with different prim types + # Grandparent: Xform with transform + sim_utils.create_prim( + "/World/Grandparent", + "Xform", + translation=(5.0, 3.0, 2.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Parent: Scope prim (organizational, typically has no transform) + parent = stage.DefinePrim("/World/Grandparent/Parent", "Scope") + + # Obtain parent prim pose (should be grandparent's transform) + parent_pos, parent_quat = sim_utils.resolve_prim_pose(parent) + assert_vec3_close(Gf.Vec3d(*parent_pos), (5.0, 3.0, 2.0), eps=1e-5) + assert_quat_close(Gf.Quatd(*parent_quat), (0.7071068, 0.0, 0.0, 0.7071068), eps=1e-5) + + # Child: Mesh prim (geometry) + child = sim_utils.create_prim("/World/Grandparent/Parent/Child", "Mesh", stage=stage) + + # World pose we want to achieve for the child + world_position = (10.0, 5.0, 3.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation + + # Convert to local space relative to parent (Scope) + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, child + ) + + # Verify orientation is not None + assert local_orientation is not None, "Expected orientation to be computed" + + # Set the local transform on the child (Mesh) + xformable = UsdGeom.Xformable(child) + translate_op = xformable.GetTranslateOp() + translate_op.Set(Gf.Vec3d(*local_translation)) + orient_op = xformable.GetOrientOp() + orient_op.Set(Gf.Quatd(*local_orientation)) + + # Verify world pose of child + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) + + # Should match the desired world pose + # Note: Scope prims typically have no transform, so the child's world pose should account + # for the grandparent's transform + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-10) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-10) From 33576d47549f4b644e2da61ced4b2e18b89f57ca Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 14:21:00 +0100 Subject: [PATCH 030/100] fixes usd path --- source/isaaclab/test/sim/test_xform_prim_view.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/test/sim/test_xform_prim_view.py b/source/isaaclab/test/sim/test_xform_prim_view.py index 59c4c17824e..57e8d5e05c4 100644 --- a/source/isaaclab/test/sim/test_xform_prim_view.py +++ b/source/isaaclab/test/sim/test_xform_prim_view.py @@ -537,7 +537,7 @@ def test_with_franka_robots(device): stage = sim_utils.get_current_stage() # Load Franka robot assets - franka_usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka_instanceable.usd" + franka_usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" # Add two Franka robots to the stage sim_utils.create_prim("/World/Franka_1", "Xform", usd_path=franka_usd_path, stage=stage) From 381d489dc38fa089f9d929494de5582ba0b6490a Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 14:28:01 +0100 Subject: [PATCH 031/100] switches internals to use new xformview --- source/isaaclab/isaaclab/scene/interactive_scene.py | 10 ++++------ source/isaaclab/isaaclab/sensors/camera/camera.py | 4 ++-- .../isaaclab/isaaclab/sensors/camera/tiled_camera.py | 4 ++-- .../isaaclab/isaaclab/sensors/ray_caster/ray_caster.py | 6 +++--- 4 files changed, 11 insertions(+), 13 deletions(-) diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 775df1640f4..7bfaff816ab 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -10,7 +10,6 @@ import carb from isaacsim.core.cloner import GridCloner -from isaacsim.core.prims import XFormPrim from pxr import PhysxSchema import isaaclab.sim as sim_utils @@ -30,6 +29,7 @@ from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg from isaaclab.sim import SimulationContext from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id +from isaaclab.sim.views import XFormPrimView from isaaclab.terrains import TerrainImporter, TerrainImporterCfg from isaaclab.utils.version import get_isaac_sim_version @@ -406,10 +406,10 @@ def surface_grippers(self) -> dict[str, SurfaceGripper]: return self._surface_grippers @property - def extras(self) -> dict[str, XFormPrim]: + def extras(self) -> dict[str, XFormPrimView]: """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. - The keys are the names of the miscellaneous objects, and the values are the `XFormPrim`_ + The keys are the names of the miscellaneous objects, and the values are the :class:`~isaaclab.sim.utils.views.XFormPrimView` of the corresponding prims. As an example, lights or other props in the scene that do not have any attributes or properties that you @@ -419,8 +419,6 @@ def extras(self) -> dict[str, XFormPrim]: These are not reset or updated by the scene. They are mainly other prims that are not necessarily handled by the interactive scene, but are useful to be accessed by the user. - .. _XFormPrim: https://docs.isaacsim.omniverse.nvidia.com/latest/py/source/extensions/isaacsim.core.prims/docs/index.html#isaacsim.core.prims.XFormPrim - """ return self._extras @@ -779,7 +777,7 @@ def _add_entities_from_cfg(self): ) # store xform prim view corresponding to this asset # all prims in the scene are Xform prims (i.e. have a transform component) - self._extras[asset_name] = XFormPrim(asset_cfg.prim_path, reset_xform_properties=False) + self._extras[asset_name] = XFormPrimView(asset_cfg.prim_path, reset_xform_properties=False) else: raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") # store global collision paths diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 90f8bdef955..87636cc5bb4 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -17,11 +17,11 @@ import carb import omni.kit.commands import omni.usd -from isaacsim.core.prims import XFormPrim from pxr import Sdf, UsdGeom import isaaclab.sim as sim_utils import isaaclab.utils.sensors as sensor_utils +from isaaclab.sim.views import XFormPrimView from isaaclab.utils import to_camel_case from isaaclab.utils.array import convert_to_torch from isaaclab.utils.math import ( @@ -405,7 +405,7 @@ def _initialize_impl(self): # Initialize parent class super()._initialize_impl() # Create a view for the sensor - self._view = XFormPrim(self.cfg.prim_path, reset_xform_properties=False) + self._view = XFormPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) self._view.initialize() # Check that sizes are correct if self._view.count != self._num_envs: diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index 7c04ccc5bde..4f1a65a3d37 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -14,9 +14,9 @@ import carb import warp as wp -from isaacsim.core.prims import XFormPrim from pxr import UsdGeom +from isaaclab.sim.views import XFormPrimView from isaaclab.utils.warp.kernels import reshape_tiled_image from ..sensor_base import SensorBase @@ -150,7 +150,7 @@ def _initialize_impl(self): # Initialize parent class SensorBase._initialize_impl(self) # Create a view for the sensor - self._view = XFormPrim(self.cfg.prim_path, reset_xform_properties=False) + self._view = XFormPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) self._view.initialize() # Check that sizes are correct if self._view.count != self._num_envs: diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 4470f9145c7..2b273fd57fe 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -14,13 +14,13 @@ import omni import warp as wp -from isaacsim.core.prims import XFormPrim from isaacsim.core.simulation_manager import SimulationManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.markers import VisualizationMarkers +from isaaclab.sim.views import XFormPrimView from isaaclab.terrains.trimesh.utils import make_plane from isaaclab.utils.math import quat_apply, quat_apply_yaw from isaaclab.utils.warp import convert_to_warp_mesh, raycast_mesh @@ -333,7 +333,7 @@ def _debug_vis_callback(self, event): def _obtain_trackable_prim_view( self, target_prim_path: str - ) -> tuple[XFormPrim | any, tuple[torch.Tensor, torch.Tensor]]: + ) -> tuple[XFormPrimView | any, tuple[torch.Tensor, torch.Tensor]]: """Obtain a prim view that can be used to track the pose of the parget prim. The target prim path is a regex expression that matches one or more mesh prims. While we can track its @@ -376,7 +376,7 @@ def _obtain_trackable_prim_view( new_root_prim = current_prim.GetParent() current_path_expr = current_path_expr.rsplit("/", 1)[0] if not new_root_prim.IsValid(): - prim_view = XFormPrim(target_prim_path, reset_xform_properties=False) + prim_view = XFormPrimView(target_prim_path, device=self._device, stage=self.stage) current_path_expr = target_prim_path logger.warning( f"The prim at path {target_prim_path} which is used for raycasting is not a physics prim." From b1b5350bacd962699c8a8445f8767044e159acd8 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 14:30:00 +0100 Subject: [PATCH 032/100] adds views to docs --- docs/source/api/index.rst | 1 + docs/source/api/lab/isaaclab.sim.views.rst | 17 +++++++++++++++++ 2 files changed, 18 insertions(+) create mode 100644 docs/source/api/lab/isaaclab.sim.views.rst diff --git a/docs/source/api/index.rst b/docs/source/api/index.rst index 1f735ace00b..d4f962b0b47 100644 --- a/docs/source/api/index.rst +++ b/docs/source/api/index.rst @@ -36,6 +36,7 @@ The following modules are available in the ``isaaclab`` extension: lab/isaaclab.sim.converters lab/isaaclab.sim.schemas lab/isaaclab.sim.spawners + lab/isaaclab.sim.views lab/isaaclab.sim.utils diff --git a/docs/source/api/lab/isaaclab.sim.views.rst b/docs/source/api/lab/isaaclab.sim.views.rst new file mode 100644 index 00000000000..cf850154193 --- /dev/null +++ b/docs/source/api/lab/isaaclab.sim.views.rst @@ -0,0 +1,17 @@ +isaaclab.sim.views +================== + +.. automodule:: isaaclab.sim.views + + .. rubric:: Classes + + .. autosummary:: + + XFormPrimView + +XForm Prim View +--------------- + +.. autoclass:: XFormPrimView + :members: + :show-inheritance: From 3a085d17b193346ce0a5459f288cb13b7f1031e4 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 16:04:51 +0100 Subject: [PATCH 033/100] adds new implementations --- .../isaaclab/isaaclab/sim/utils/transforms.py | 58 +- .../test/sim/test_utils_transforms.py | 592 +++++++++++++----- 2 files changed, 485 insertions(+), 165 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 1bad98104c9..65aecaee951 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -213,6 +213,38 @@ def standardize_xform_ops( return True +def validate_standard_xform_ops(prim: Usd.Prim) -> bool: + """Validate if the transform operations on a prim are standardized. + + This function checks if the transform operations on a prim are standardized to the canonical form: + [translate, orient, scale]. + + Args: + prim: The USD prim to validate. + """ + # check if prim is valid + if not prim.IsValid(): + logger.error(f"Prim at path '{prim.GetPath().pathString}' is not valid.") + return False + # check if prim is an xformable + if not prim.IsA(UsdGeom.Xformable): + logger.error(f"Prim at path '{prim.GetPath().pathString}' is not an xformable.") + return False + # get the xformable interface + xformable = UsdGeom.Xformable(prim) + # get the xform operation order + xform_op_order = xformable.GetOrderedXformOps() + xform_op_order = [op.GetOpName() for op in xform_op_order] + # check if the xform operation order is the canonical form + if xform_op_order != ["xformOp:translate", "xformOp:orient", "xformOp:scale"]: + msg = f"Xform operation order for prim at path '{prim.GetPath().pathString}' is not the canonical form." + msg += f" Received order: {xform_op_order}" + msg += " Expected order: ['xformOp:translate', 'xformOp:orient', 'xformOp:scale']" + logger.error(msg) + return False + return True + + def resolve_prim_pose( prim: Usd.Prim, ref_prim: Usd.Prim | None = None ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: @@ -274,13 +306,15 @@ def resolve_prim_pose( # check if ref prim is valid if not ref_prim.IsValid(): raise ValueError(f"Ref prim at path '{ref_prim.GetPath().pathString}' is not valid.") - # get ref prim xform - ref_xform = UsdGeom.Xformable(ref_prim) - ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) - # make sure ref tf is orthonormal - ref_tf = ref_tf.GetOrthonormalized() - # compute relative transform to get prim in ref frame - prim_tf = prim_tf * ref_tf.GetInverse() + # if reference prim is the root, we can skip the computation + if ref_prim.GetPath() != Sdf.Path.absoluteRootPath: + # get ref prim xform + ref_xform = UsdGeom.Xformable(ref_prim) + ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # make sure ref tf is orthonormal + ref_tf = ref_tf.GetOrthonormalized() + # compute relative transform to get prim in ref frame + prim_tf = prim_tf * ref_tf.GetInverse() # extract position and orientation prim_pos = [*prim_tf.ExtractTranslation()] @@ -345,15 +379,15 @@ def convert_world_pose_to_local( ``local_transform = world_transform * inverse(ref_world_transform)`` .. note:: - If the reference prim is invalid or is the root path, the position and orientation are returned + If the reference prim is the root prim ("/"), the position and orientation are returned unchanged, as they are already effectively in local/world space. Args: position: The world-space position as (x, y, z). orientation: The world-space orientation as quaternion (w, x, y, z). If None, only position is converted and None is returned for orientation. - ref_prim: The reference USD prim to compute the local transform relative to. If this is invalid or - is the root path, the world pose is returned unchanged. + ref_prim: The reference USD prim to compute the local transform relative to. If this is + the root prim ("/"), the world pose is returned unchanged. Returns: A tuple of (local_translation, local_orientation) where: @@ -363,7 +397,6 @@ def convert_world_pose_to_local( Raises: ValueError: If the reference prim is not a valid USD prim. - ValueError: If the reference prim is not a valid USD Xformable. Example: >>> import isaaclab.sim as sim_utils @@ -392,9 +425,6 @@ def convert_world_pose_to_local( # Check if reference prim is a valid xformable ref_xformable = UsdGeom.Xformable(ref_prim) - if not ref_xformable: - raise ValueError(f"Reference prim at path '{ref_prim.GetPath().pathString}' is not a valid xformable.") - # Get reference prim's world transform ref_world_tf = ref_xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) diff --git a/source/isaaclab/test/sim/test_utils_transforms.py b/source/isaaclab/test/sim/test_utils_transforms.py index 1cb4182a81f..c3cbdbd3f74 100644 --- a/source/isaaclab/test/sim/test_utils_transforms.py +++ b/source/isaaclab/test/sim/test_utils_transforms.py @@ -68,7 +68,7 @@ def get_xform_ops(prim: Usd.Prim) -> list[str]: """ -Tests. +Test standardize_xform_ops() function. """ @@ -731,6 +731,402 @@ def test_standardize_xform_ops_preserves_float_precision(): assert_quat_close(Gf.Quatd(*quat_after), new_orientation, eps=1e-4) +""" +Test validate_standard_xform_ops() function. +""" + + +def test_validate_standard_xform_ops_valid(): + """Test validate_standard_xform_ops returns True for standardized prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations + prim = sim_utils.create_prim( + "/World/TestValid", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Standardize the prim + sim_utils.standardize_xform_ops(prim) + + # Validate it + assert sim_utils.validate_standard_xform_ops(prim) is True + + +def test_validate_standard_xform_ops_invalid_order(): + """Test validate_standard_xform_ops returns False for non-standard operation order.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim and manually set up xform ops in wrong order + prim_path = "/World/TestInvalidOrder" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add operations in wrong order: scale, translate, orient (should be translate, orient, scale) + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(1.0, 1.0, 1.0)) + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + orient_op = xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble) + orient_op.Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_with_deprecated_ops(): + """Test validate_standard_xform_ops returns False when deprecated operations exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with deprecated rotateXYZ operation + prim_path = "/World/TestDeprecated" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add deprecated rotateXYZ operation + rotate_xyz_op = xformable.AddRotateXYZOp(UsdGeom.XformOp.PrecisionDouble) + rotate_xyz_op.Set(Gf.Vec3d(45.0, 30.0, 60.0)) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_missing_operations(): + """Test validate_standard_xform_ops returns False when standard operations are missing.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with only translate operation (missing orient and scale) + prim_path = "/World/TestMissing" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Validate it - should return False (missing orient and scale) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_invalid_prim(): + """Test validate_standard_xform_ops returns False for invalid prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Get an invalid prim + invalid_prim = stage.GetPrimAtPath("/World/NonExistent") + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(invalid_prim) is False + + +def test_validate_standard_xform_ops_non_xformable(): + """Test validate_standard_xform_ops returns False for non-Xformable prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a Material prim (not Xformable) + from pxr import UsdShade + + material_prim = UsdShade.Material.Define(stage, "/World/TestMaterial").GetPrim() + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(material_prim) is False + + +def test_validate_standard_xform_ops_with_transform_matrix(): + """Test validate_standard_xform_ops returns False when transform matrix operation exists.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with transform matrix + prim_path = "/World/TestTransformMatrix" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add transform matrix operation + transform_op = xformable.AddTransformOp(UsdGeom.XformOp.PrecisionDouble) + matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(5.0, 10.0, 15.0)) + transform_op.Set(matrix) + + # Validate it - should return False + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_extra_operations(): + """Test validate_standard_xform_ops returns False when extra operations exist.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with standard operations + prim = sim_utils.create_prim( + "/World/TestExtra", + "Xform", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), + stage=stage, + ) + + # Standardize it + sim_utils.standardize_xform_ops(prim) + + # Add an extra operation + xformable = UsdGeom.Xformable(prim) + extra_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + extra_op.Set(45.0) + + # Validate it - should return False (has extra operation) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +def test_validate_standard_xform_ops_after_standardization(): + """Test validate_standard_xform_ops returns True after standardization of non-standard prim.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a prim with non-standard operations + prim_path = "/World/TestBeforeAfter" + prim = stage.DefinePrim(prim_path, "Xform") + xformable = UsdGeom.Xformable(prim) + + # Add deprecated operations + rotate_x_op = xformable.AddRotateXOp(UsdGeom.XformOp.PrecisionDouble) + rotate_x_op.Set(45.0) + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(1.0, 2.0, 3.0)) + + # Validate before standardization - should be False + assert sim_utils.validate_standard_xform_ops(prim) is False + + # Standardize the prim + sim_utils.standardize_xform_ops(prim) + + # Validate after standardization - should be True + assert sim_utils.validate_standard_xform_ops(prim) is True + + +def test_validate_standard_xform_ops_on_geometry(): + """Test validate_standard_xform_ops works correctly on geometry prims.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a cube with standard operations + cube_prim = sim_utils.create_prim( + "/World/TestCube", + "Cube", + translation=(1.0, 2.0, 3.0), + orientation=(1.0, 0.0, 0.0, 0.0), + scale=(2.0, 2.0, 2.0), + stage=stage, + ) + + # Standardize it + sim_utils.standardize_xform_ops(cube_prim) + + # Validate it - should be True + assert sim_utils.validate_standard_xform_ops(cube_prim) is True + + +def test_validate_standard_xform_ops_empty_prim(): + """Test validate_standard_xform_ops on prim with no xform operations.""" + # obtain stage handle + stage = sim_utils.get_current_stage() + + # Create a bare prim with no xform operations + prim_path = "/World/TestEmpty" + prim = stage.DefinePrim(prim_path, "Xform") + + # Validate it - should return False (no operations at all) + assert sim_utils.validate_standard_xform_ops(prim) is False + + +""" +Test resolve_prim_pose() function. +""" + + +def test_resolve_prim_pose(): + """Test resolve_prim_pose() function.""" + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + # sample random rotations + rand_quats = np.random.randn(num_objects, 3, 4) + rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + orientation=rand_quats[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + orientation=rand_quats[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + orientation=rand_quats[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(cube_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) + # xform prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + # dummy prim w.r.t. world frame + pos, quat = sim_utils.resolve_prim_pose(dummy_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) + np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + + # geometry prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) + # TODO: Enabling scale causes the test to fail because the current implementation of + # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known + # limitation. Until this is fixed, the test is disabled here to ensure the test passes. + # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) + + # dummy prim w.r.t. xform prim + pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) + pos, quat = np.array(pos), np.array(quat) + np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) + np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) + # xform prim w.r.t. cube prim + pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) + pos, quat = np.array(pos), np.array(quat) + # -- compute ground truth values + gt_pos, gt_quat = math_utils.subtract_frame_transforms( + torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), + torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), + torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), + ) + gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() + quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat + np.testing.assert_allclose(pos, gt_pos, atol=1e-3) + np.testing.assert_allclose(quat, gt_quat, atol=1e-3) + + +""" +Test resolve_prim_scale() function. +""" + + +def test_resolve_prim_scale(): + """Test resolve_prim_scale() function. + + To simplify the test, we assume that the effective scale at a prim + is the product of the scales of the prims in the hierarchy: + + scale = scale_of_xform * scale_of_geometry_prim + + This is only true when rotations are identity or the transforms are + orthogonal and uniformly scaled. Otherwise, scale is not composable + like that in local component-wise fashion. + """ + # number of objects + num_objects = 20 + # sample random scales for x, y, z + rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) + rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) + # sample random positions + rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + + # create objects + for i in range(num_objects): + # simple cubes + cube_prim = sim_utils.create_prim( + f"/World/Cubes/instance_{i:02d}", + "Cube", + translation=rand_positions[i, 0], + scale=rand_scales[i, 0], + attributes={"size": rand_widths[i]}, + ) + # xform hierarchy + xform_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}", + "Xform", + translation=rand_positions[i, 1], + scale=rand_scales[i, 1], + ) + geometry_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/geometry", + "Sphere", + translation=rand_positions[i, 2], + scale=rand_scales[i, 2], + attributes={"radius": rand_widths[i]}, + ) + dummy_prim = sim_utils.create_prim( + f"/World/Xform/instance_{i:02d}/dummy", + "Sphere", + ) + + # cube prim + scale = sim_utils.resolve_prim_scale(cube_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 0], atol=1e-5) + # xform prim + scale = sim_utils.resolve_prim_scale(xform_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + # geometry prim + scale = sim_utils.resolve_prim_scale(geometry_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1] * rand_scales[i, 2], atol=1e-5) + # dummy prim + scale = sim_utils.resolve_prim_scale(dummy_prim) + scale = np.array(scale) + np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + + +""" +Test convert_world_pose_to_local() function. +""" + + def test_convert_world_pose_to_local_basic(): """Test basic world-to-local pose conversion.""" # obtain stage handle @@ -959,163 +1355,57 @@ def test_convert_world_pose_to_local_complex_hierarchy(): assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-4) -def test_resolve_prim_pose(): - """Test resolve_prim_pose() function.""" - # number of objects - num_objects = 20 - # sample random scales for x, y, z - rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) - rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) - # sample random positions - rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) - # sample random rotations - rand_quats = np.random.randn(num_objects, 3, 4) - rand_quats /= np.linalg.norm(rand_quats, axis=2, keepdims=True) - - # create objects - for i in range(num_objects): - # simple cubes - cube_prim = sim_utils.create_prim( - f"/World/Cubes/instance_{i:02d}", - "Cube", - translation=rand_positions[i, 0], - orientation=rand_quats[i, 0], - scale=rand_scales[i, 0], - attributes={"size": rand_widths[i]}, - ) - # xform hierarchy - xform_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}", - "Xform", - translation=rand_positions[i, 1], - orientation=rand_quats[i, 1], - scale=rand_scales[i, 1], - ) - geometry_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/geometry", - "Sphere", - translation=rand_positions[i, 2], - orientation=rand_quats[i, 2], - scale=rand_scales[i, 2], - attributes={"radius": rand_widths[i]}, - ) - dummy_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/dummy", - "Sphere", - ) +def test_convert_world_pose_to_local_with_mixed_prim_types(): + """Test world-to-local conversion with mixed prim types (Xform, Scope, Mesh).""" + # obtain stage handle + stage = sim_utils.get_current_stage() - # cube prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(cube_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 0, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 0], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 0], atol=1e-3) - # xform prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(xform_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) - # dummy prim w.r.t. world frame - pos, quat = sim_utils.resolve_prim_pose(dummy_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 1, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 1], atol=1e-3) - np.testing.assert_allclose(quat, rand_quats[i, 1], atol=1e-3) + # Create a hierarchy with different prim types + # Grandparent: Xform with transform + sim_utils.create_prim( + "/World/Grandparent", + "Xform", + translation=(5.0, 3.0, 2.0), + orientation=(0.7071068, 0.0, 0.0, 0.7071068), # 90 deg around Z + scale=(2.0, 2.0, 2.0), + stage=stage, + ) - # geometry prim w.r.t. xform prim - pos, quat = sim_utils.resolve_prim_pose(geometry_prim, ref_prim=xform_prim) - pos, quat = np.array(pos), np.array(quat) - quat = quat if np.sign(rand_quats[i, 2, 0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, rand_positions[i, 2] * rand_scales[i, 1], atol=1e-3) - # TODO: Enabling scale causes the test to fail because the current implementation of - # resolve_prim_pose does not correctly handle non-identity scales on Xform prims. This is a known - # limitation. Until this is fixed, the test is disabled here to ensure the test passes. - # np.testing.assert_allclose(quat, rand_quats[i, 2], atol=1e-3) + # Parent: Scope prim (organizational, typically has no transform) + parent = stage.DefinePrim("/World/Grandparent/Parent", "Scope") - # dummy prim w.r.t. xform prim - pos, quat = sim_utils.resolve_prim_pose(dummy_prim, ref_prim=xform_prim) - pos, quat = np.array(pos), np.array(quat) - np.testing.assert_allclose(pos, np.zeros(3), atol=1e-3) - np.testing.assert_allclose(quat, np.array([1, 0, 0, 0]), atol=1e-3) - # xform prim w.r.t. cube prim - pos, quat = sim_utils.resolve_prim_pose(xform_prim, ref_prim=cube_prim) - pos, quat = np.array(pos), np.array(quat) - # -- compute ground truth values - gt_pos, gt_quat = math_utils.subtract_frame_transforms( - torch.from_numpy(rand_positions[i, 0]).unsqueeze(0), - torch.from_numpy(rand_quats[i, 0]).unsqueeze(0), - torch.from_numpy(rand_positions[i, 1]).unsqueeze(0), - torch.from_numpy(rand_quats[i, 1]).unsqueeze(0), - ) - gt_pos, gt_quat = gt_pos.squeeze(0).numpy(), gt_quat.squeeze(0).numpy() - quat = quat if np.sign(gt_quat[0]) == np.sign(quat[0]) else -quat - np.testing.assert_allclose(pos, gt_pos, atol=1e-3) - np.testing.assert_allclose(quat, gt_quat, atol=1e-3) + # Obtain parent prim pose (should be grandparent's transform) + parent_pos, parent_quat = sim_utils.resolve_prim_pose(parent) + assert_vec3_close(Gf.Vec3d(*parent_pos), (5.0, 3.0, 2.0), eps=1e-5) + assert_quat_close(Gf.Quatd(*parent_quat), (0.7071068, 0.0, 0.0, 0.7071068), eps=1e-5) + # Child: Mesh prim (geometry) + child = sim_utils.create_prim("/World/Grandparent/Parent/Child", "Mesh", stage=stage) -def test_resolve_prim_scale(): - """Test resolve_prim_scale() function. + # World pose we want to achieve for the child + world_position = (10.0, 5.0, 3.0) + world_orientation = (1.0, 0.0, 0.0, 0.0) # identity rotation - To simplify the test, we assume that the effective scale at a prim - is the product of the scales of the prims in the hierarchy: + # Convert to local space relative to parent (Scope) + local_translation, local_orientation = sim_utils.convert_world_pose_to_local( + world_position, world_orientation, child + ) - scale = scale_of_xform * scale_of_geometry_prim + # Verify orientation is not None + assert local_orientation is not None, "Expected orientation to be computed" - This is only true when rotations are identity or the transforms are - orthogonal and uniformly scaled. Otherwise, scale is not composable - like that in local component-wise fashion. - """ - # number of objects - num_objects = 20 - # sample random scales for x, y, z - rand_scales = np.random.uniform(0.5, 1.5, size=(num_objects, 3, 3)) - rand_widths = np.random.uniform(0.1, 10.0, size=(num_objects,)) - # sample random positions - rand_positions = np.random.uniform(-100, 100, size=(num_objects, 3, 3)) + # Set the local transform on the child (Mesh) + xformable = UsdGeom.Xformable(child) + translate_op = xformable.GetTranslateOp() + translate_op.Set(Gf.Vec3d(*local_translation)) + orient_op = xformable.GetOrientOp() + orient_op.Set(Gf.Quatd(*local_orientation)) - # create objects - for i in range(num_objects): - # simple cubes - cube_prim = sim_utils.create_prim( - f"/World/Cubes/instance_{i:02d}", - "Cube", - translation=rand_positions[i, 0], - scale=rand_scales[i, 0], - attributes={"size": rand_widths[i]}, - ) - # xform hierarchy - xform_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}", - "Xform", - translation=rand_positions[i, 1], - scale=rand_scales[i, 1], - ) - geometry_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/geometry", - "Sphere", - translation=rand_positions[i, 2], - scale=rand_scales[i, 2], - attributes={"radius": rand_widths[i]}, - ) - dummy_prim = sim_utils.create_prim( - f"/World/Xform/instance_{i:02d}/dummy", - "Sphere", - ) + # Verify world pose of child + child_world_pos, child_world_quat = sim_utils.resolve_prim_pose(child) - # cube prim - scale = sim_utils.resolve_prim_scale(cube_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 0], atol=1e-5) - # xform prim - scale = sim_utils.resolve_prim_scale(xform_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) - # geometry prim - scale = sim_utils.resolve_prim_scale(geometry_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 1] * rand_scales[i, 2], atol=1e-5) - # dummy prim - scale = sim_utils.resolve_prim_scale(dummy_prim) - scale = np.array(scale) - np.testing.assert_allclose(scale, rand_scales[i, 1], atol=1e-5) + # Should match the desired world pose + # Note: Scope prims typically have no transform, so the child's world pose should account + # for the grandparent's transform + assert_vec3_close(Gf.Vec3d(*child_world_pos), world_position, eps=1e-10) + assert_quat_close(Gf.Quatd(*child_world_quat), world_orientation, eps=1e-10) From a5a9648af096494abc0df04c118dcafbabb4aa18 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 16:06:53 +0100 Subject: [PATCH 034/100] removes unused initialize --- source/isaaclab/isaaclab/sensors/camera/camera.py | 1 - source/isaaclab/isaaclab/sensors/camera/tiled_camera.py | 1 - 2 files changed, 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 87636cc5bb4..7e12d6d4238 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -406,7 +406,6 @@ def _initialize_impl(self): super()._initialize_impl() # Create a view for the sensor self._view = XFormPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) - self._view.initialize() # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index 4f1a65a3d37..69e01874586 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -151,7 +151,6 @@ def _initialize_impl(self): SensorBase._initialize_impl(self) # Create a view for the sensor self._view = XFormPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) - self._view.initialize() # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( From 98d3388becbcce4f632d18742a7888bb2edab956 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:25:07 +0100 Subject: [PATCH 035/100] switches to comparing with experimental api of isaacsim --- .../benchmarks/benchmark_xform_prim_view.py | 74 +++++--- .../isaaclab/sim/views/xform_prim_view.py | 175 ++++++++++-------- 2 files changed, 137 insertions(+), 112 deletions(-) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 99adf7cea2a..9e538d9c537 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -68,17 +68,21 @@ import torch from typing import Literal -from isaacsim.core.prims import XFormPrim as IsaacSimXFormPrimView +from isaacsim.core.utils.extensions import enable_extension + +# compare against latest Isaac Sim implementation +enable_extension("isaacsim.core.experimental.prims") +from isaacsim.core.experimental.prims import XformPrim as IsaacSimXformPrimView import isaaclab.sim as sim_utils -from isaaclab.sim.views import XFormPrimView as IsaacLabXFormPrimView +from isaaclab.sim.views import XformPrimView as IsaacLabXformPrimView @torch.no_grad() def benchmark_xform_prim_view( api: Literal["isaaclab", "isaacsim"], num_iterations: int ) -> tuple[dict[str, float], dict[str, torch.Tensor]]: - """Benchmark the XFormPrimView class from either Isaac Lab or Isaac Sim. + """Benchmark the Xform view class from either Isaac Lab or Isaac Sim. Args: api: Which API to benchmark ("isaaclab" or "isaacsim"). @@ -121,24 +125,28 @@ def benchmark_xform_prim_view( if api == "isaaclab": xform_view = IsaacLabXFormPrimView(pattern, device=args_cli.device) elif api == "isaacsim": - xform_view = IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + xform_view = IsaacSimXFormPrimView(pattern) else: raise ValueError(f"Invalid API: {api}") timing_results["init"] = time.perf_counter() - start_time - print(f" XFormPrimView managing {xform_view.count} prims") + if api == "isaaclab": + num_prims = xform_view.count + elif api == "isaacsim": + num_prims = len(xform_view.prims) + print(f" XformPrimView managing {num_prims} prims") # Benchmark get_world_poses start_time = time.perf_counter() for _ in range(num_iterations): positions, orientations = xform_view.get_world_poses() - timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations + # Ensure tensors are torch tensors + if not isinstance(positions, torch.Tensor): + positions = torch.tensor(positions, dtype=torch.float32) + if not isinstance(orientations, torch.Tensor): + orientations = torch.tensor(orientations, dtype=torch.float32) - # Ensure tensors are torch tensors - if not isinstance(positions, torch.Tensor): - positions = torch.tensor(positions, dtype=torch.float32) - if not isinstance(orientations, torch.Tensor): - orientations = torch.tensor(orientations, dtype=torch.float32) + timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations # Store initial world poses computed_results["initial_world_positions"] = positions.clone() @@ -149,7 +157,10 @@ def benchmark_xform_prim_view( new_positions[:, 2] += 0.1 start_time = time.perf_counter() for _ in range(num_iterations): - xform_view.set_world_poses(new_positions, orientations) + if api == "isaaclab": + xform_view.set_world_poses(new_positions, orientations) + elif api == "isaacsim": + xform_view.set_world_poses(new_positions.cpu().numpy(), orientations.cpu().numpy()) timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations # Get world poses after setting to verify @@ -165,13 +176,13 @@ def benchmark_xform_prim_view( start_time = time.perf_counter() for _ in range(num_iterations): translations, orientations_local = xform_view.get_local_poses() - timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations + # Ensure tensors are torch tensors + if not isinstance(translations, torch.Tensor): + translations = torch.tensor(translations, dtype=torch.float32, device=args_cli.device) + if not isinstance(orientations_local, torch.Tensor): + orientations_local = torch.tensor(orientations_local, dtype=torch.float32, device=args_cli.device) - # Ensure tensors are torch tensors - if not isinstance(translations, torch.Tensor): - translations = torch.tensor(translations, dtype=torch.float32) - if not isinstance(orientations_local, torch.Tensor): - orientations_local = torch.tensor(orientations_local, dtype=torch.float32) + timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations # Store initial local poses computed_results["initial_local_translations"] = translations.clone() @@ -182,7 +193,10 @@ def benchmark_xform_prim_view( new_translations[:, 2] += 0.1 start_time = time.perf_counter() for _ in range(num_iterations): - xform_view.set_local_poses(new_translations, orientations_local) + if api == "isaaclab": + xform_view.set_local_poses(new_translations, orientations_local) + elif api == "isaacsim": + xform_view.set_local_poses(new_translations.cpu().numpy(), orientations_local.cpu().numpy()) timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations # Get local poses after setting to verify @@ -215,8 +229,8 @@ def compare_results( """Compare computed results between Isaac Lab and Isaac Sim implementations. Args: - isaaclab_computed: Computed values from Isaac Lab's XFormPrimView. - isaacsim_computed: Computed values from Isaac Sim's XFormPrimView. + isaaclab_computed: Computed values from Isaac Lab's XformPrimView. + isaacsim_computed: Computed values from Isaac Sim's XformPrimView. tolerance: Tolerance for numerical comparison. Returns: @@ -294,8 +308,8 @@ def print_results( """Print benchmark results in a formatted table. Args: - isaaclab_results: Results from Isaac Lab's XFormPrimView benchmark. - isaacsim_results: Results from Isaac Sim's XFormPrimView benchmark. + isaaclab_results: Results from Isaac Lab's XformPrimView benchmark. + isaacsim_results: Results from Isaac Sim's XformPrimView benchmark. num_prims: Number of prims tested. num_iterations: Number of iterations run. """ @@ -350,7 +364,7 @@ def print_results( def main(): """Main benchmark function.""" print("=" * 100) - print("XFormPrimView Benchmark") + print("XformPrimView Benchmark") print("=" * 100) print("Configuration:") print(f" Number of environments: {args_cli.num_envs}") @@ -367,8 +381,8 @@ def main(): os.makedirs(args_cli.profile_dir, exist_ok=True) - # Benchmark Isaac Lab XFormPrimView - print("Benchmarking XFormPrimView from Isaac Lab...") + # Benchmark Isaac Lab XformPrimView + print("Benchmarking XformPrimView from Isaac Lab...") if args_cli.profile: profiler_isaaclab = cProfile.Profile() profiler_isaaclab.enable() @@ -379,15 +393,15 @@ def main(): if args_cli.profile: profiler_isaaclab.disable() - profile_file_isaaclab = f"{args_cli.profile_dir}/isaaclab_xformprimview.prof" + profile_file_isaaclab = f"{args_cli.profile_dir}/isaaclab_XformPrimView.prof" profiler_isaaclab.dump_stats(profile_file_isaaclab) print(f" Profile saved to: {profile_file_isaaclab}") print(" Done!") print() - # Benchmark Isaac Sim XFormPrimView - print("Benchmarking Isaac Sim XFormPrimView...") + # Benchmark Isaac Sim XformPrimView + print("Benchmarking Isaac Sim XformPrimView...") if args_cli.profile: profiler_isaacsim = cProfile.Profile() profiler_isaacsim.enable() @@ -398,7 +412,7 @@ def main(): if args_cli.profile: profiler_isaacsim.disable() - profile_file_isaacsim = f"{args_cli.profile_dir}/isaacsim_xformprimview.prof" + profile_file_isaacsim = f"{args_cli.profile_dir}/isaacsim_XformPrimView.prof" profiler_isaacsim.dump_stats(profile_file_isaacsim) print(f" Profile saved to: {profile_file_isaacsim}") diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 20a2095edbf..9965fbbbb6b 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -5,12 +5,13 @@ from __future__ import annotations +import numpy as np import torch -from pxr import Gf, Sdf, Usd, UsdGeom +from pxr import Gf, Sdf, Usd, UsdGeom, Vt import isaaclab.sim as sim_utils - +import isaaclab.utils.math as math_utils class XFormPrimView: """Optimized batched interface for reading and writing transforms of multiple USD prims. @@ -46,7 +47,7 @@ class XFormPrimView: """ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None): - """Initialize the XFormPrimView with matching prims. + """Initialize the view with matching prims. This method searches the USD stage for all prims matching the provided path pattern, validates that they are Xformable with standard transform operations, and stores @@ -138,18 +139,18 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t f"Expected positions shape ({self.count}, 3), got {positions.shape}. " "Number of positions must match the number of prims in the view." ) - positions_list = positions.tolist() if positions is not None else None + positions_array = Vt.Vec3dArray.FromNumpy(positions.cpu().numpy()) else: - positions_list = None + positions_array = None if orientations is not None: if orientations.shape != (self.count, 4): raise ValueError( f"Expected orientations shape ({self.count}, 4), got {orientations.shape}. " "Number of orientations must match the number of prims in the view." ) - orientations_list = orientations.tolist() if orientations is not None else None + orientations_array = Vt.QuatdArray.FromNumpy(math_utils.convert_quat(orientations, to="xyzw").cpu().numpy()) else: - orientations_list = None + orientations_array = None # Set poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. @@ -159,38 +160,46 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t parent_prim = prim.GetParent() # Determine what to set - world_pos = tuple(positions_list[idx]) if positions_list is not None else None - world_quat = tuple(orientations_list[idx]) if orientations_list is not None else None + world_pos = positions_array[idx] if positions_array is not None else None + world_quat = orientations_array[idx] if orientations_array is not None else None # Convert world pose to local if we have a valid parent if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: # Get current world pose if we're only setting one component - if world_pos is None or world_quat is None: - current_pos, current_quat = sim_utils.resolve_prim_pose(prim) - - if world_pos is None: - world_pos = current_pos - if world_quat is None: - world_quat = current_quat + if positions_array is None or orientations_array is None: + # get prim xform + prim_tf = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf.Orthonormalize() + # populate desired world transform + if world_pos is not None: + prim_tf.SetTranslateOnly(world_pos) + if world_quat is not None: + prim_tf.SetRotateOnly(world_quat) + else: + # Both position and orientation are provided, create new transform + prim_tf = Gf.Matrix4d() + prim_tf.SetTranslateOnly(world_pos) + prim_tf.SetRotateOnly(world_quat) # Convert to local space - local_pos, local_quat = sim_utils.convert_world_pose_to_local(world_pos, world_quat, parent_prim) + parent_world_tf = UsdGeom.Xformable(parent_prim).ComputeLocalToWorldTransform( + Usd.TimeCode.Default() + ) + local_tf = prim_tf * parent_world_tf.GetInverse() + local_pos = local_tf.ExtractTranslation() + local_quat = local_tf.ExtractRotationQuat() else: # No parent or parent is root, world == local local_pos = world_pos local_quat = world_quat # Get or create the standard transform operations - xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) - xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) - - # set the data - xform_ops = [xform_op_translate, xform_op_orient] - xform_values = [Gf.Vec3d(*local_pos), Gf.Quatd(*local_quat)] # type: ignore - for xform_op, value in zip(xform_ops, xform_values): - if value is not None: - current_value = xform_op.Get() - xform_op.Set(type(current_value)(value) if current_value is not None else value) + if local_pos is not None: + prim.GetAttribute("xformOp:translate").Set(local_pos) + if local_quat is not None: + prim.GetAttribute("xformOp:orient").Set(local_quat) def set_local_poses( self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None @@ -221,36 +230,28 @@ def set_local_poses( f"Expected translations shape ({self.count}, 3), got {translations.shape}. " "Number of translations must match the number of prims in the view." ) - translations_list = translations.tolist() if translations is not None else None + translations_array = Vt.Vec3dArray.FromNumpy(translations.cpu().numpy()) else: - translations_list = None + translations_array = None if orientations is not None: if orientations.shape != (self.count, 4): raise ValueError( f"Expected orientations shape ({self.count}, 4), got {orientations.shape}. " "Number of orientations must match the number of prims in the view." ) - orientations_list = orientations.tolist() if orientations is not None else None + orientations_array = Vt.QuatdArray.FromNumpy(math_utils.convert_quat(orientations, to="xyzw").cpu().numpy()) else: - orientations_list = None + orientations_array = None # Set local poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): for idx, prim in enumerate(self._prims): - local_pos = Gf.Vec3d(*translations_list[idx]) if translations_list is not None else None - local_quat = Gf.Quatd(*orientations_list[idx]) if orientations_list is not None else None - - # Get or create the standard transform operations - xform_op_translate = UsdGeom.XformOp(prim.GetAttribute("xformOp:translate")) - xform_op_orient = UsdGeom.XformOp(prim.GetAttribute("xformOp:orient")) - - # set the data - xform_ops = [xform_op_translate, xform_op_orient] - xform_values = [local_pos, local_quat] - for xform_op, value in zip(xform_ops, xform_values): - if value is not None: - current_value = xform_op.Get() - xform_op.Set(type(current_value)(value) if current_value is not None else value) + if translations_array is not None: + local_pos = translations_array[idx] + prim.GetAttribute("xformOp:translate").Set(local_pos) + if orientations_array is not None: + local_quat = orientations_array[idx] + prim.GetAttribute("xformOp:orient").Set(local_quat) def set_scales(self, scales: torch.Tensor): """Set scales for all prims in the view. @@ -264,15 +265,13 @@ def set_scales(self, scales: torch.Tensor): if scales.shape != (self.count, 3): raise ValueError(f"Expected scales shape ({self.count}, 3), got {scales.shape}.") - scales_list = scales.tolist() + scales_array = Vt.Vec3dArray.FromNumpy(scales.cpu().numpy()) # Set scales for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): for idx, prim in enumerate(self._prims): - scale = Gf.Vec3d(*scales_list[idx]) - xform_op_scale = UsdGeom.XformOp(prim.GetAttribute("xformOp:scale")) - current_value = xform_op_scale.Get() - xform_op_scale.Set(type(current_value)(*scale) if current_value is not None else scale) + scale = scales_array[idx] + prim.GetAttribute("xformOp:scale").Set(scale) """ Operations - Getters. @@ -293,19 +292,25 @@ def get_world_poses(self) -> tuple[torch.Tensor, torch.Tensor]: - positions: Torch tensor of shape (N, 3) containing world-space positions (x, y, z) - orientations: Torch tensor of shape (N, 4) containing world-space quaternions (w, x, y, z) """ - positions = [] - orientations = [] - - for prim in self._prims: - pos, quat = sim_utils.resolve_prim_pose(prim) - positions.append(pos) - orientations.append(quat) - - # Convert to tensors - positions_tensor = torch.tensor(positions, dtype=torch.float32, device=self._device) - orientations_tensor = torch.tensor(orientations, dtype=torch.float32, device=self._device) - - return positions_tensor, orientations_tensor + positions = Vt.Vec3dArray(self.count) + orientations = Vt.QuatdArray(self.count) + + for idx, prim in enumerate(self._prims): + # get prim xform + prim_tf = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf.Orthonormalize() + # extract position and orientation + positions[idx] = prim_tf.ExtractTranslation() + orientations[idx] = prim_tf.ExtractRotationQuat() + + # move to torch tensors + positions = torch.tensor(np.array(positions), dtype=torch.float32, device=self._device) + orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) + orientations = math_utils.convert_quat(orientations, to="wxyz") + + return positions, orientations def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: """Get local-space poses for all prims in the view. @@ -322,19 +327,25 @@ def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: - translations: Torch tensor of shape (N, 3) containing local-space translations (x, y, z) - orientations: Torch tensor of shape (N, 4) containing local-space quaternions (w, x, y, z) """ - translations = [] - orientations = [] - - for prim in self._prims: - local_pos, local_quat = sim_utils.resolve_prim_pose(prim, ref_prim=prim.GetParent()) - translations.append(local_pos) - orientations.append(local_quat) - - # Convert to tensors - translations_tensor = torch.tensor(translations, dtype=torch.float32, device=self._device) - orientations_tensor = torch.tensor(orientations, dtype=torch.float32, device=self._device) - - return translations_tensor, orientations_tensor + translations = Vt.Vec3dArray(self.count) + orientations = Vt.QuatdArray(self.count) + + for idx, prim in enumerate(self._prims): + # get prim xform + prim_tf = UsdGeom.Xformable(prim).GetLocalTransformation(Usd.TimeCode.Default()) + # sanitize quaternion + # this is needed, otherwise the quaternion might be non-normalized + prim_tf.Orthonormalize() + # extract position and orientation + translations[idx] = prim_tf.ExtractTranslation() + orientations[idx] = prim_tf.ExtractRotationQuat() + + # move to torch tensors + translations = torch.tensor(np.array(translations), dtype=torch.float32, device=self._device) + orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) + orientations = math_utils.convert_quat(orientations, to="wxyz") + + return translations, orientations def get_scales(self) -> torch.Tensor: """Get scales for all prims in the view. @@ -344,11 +355,11 @@ def get_scales(self) -> torch.Tensor: Returns: A tensor of shape (N, 3) containing the scales of each prim. """ - scales = [] - for prim in self._prims: - scale = sim_utils.resolve_prim_scale(prim) - scales.append(scale) + scales = Vt.Vec3dArray(self.count) + + for idx, prim in enumerate(self._prims): + scales[idx] = prim.GetAttribute("xformOp:scale").Get() # Convert to tensor - scales_tensor = torch.tensor(scales, dtype=torch.float32, device=self._device) - return scales_tensor + scales = torch.tensor(np.array(scales), dtype=torch.float32, device=self._device) + return scales From e4eee14d9cf278f80a7fe36a267de059f007d801 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:25:31 +0100 Subject: [PATCH 036/100] renames to XformPrimView --- docs/source/api/lab/isaaclab.sim.views.rst | 4 +- .../benchmarks/benchmark_xform_prim_view.py | 12 +-- .../isaaclab/scene/interactive_scene.py | 8 +- .../isaaclab/sensors/camera/camera.py | 4 +- .../isaaclab/sensors/camera/tiled_camera.py | 4 +- .../isaaclab/sensors/ray_caster/ray_caster.py | 6 +- .../isaaclab/isaaclab/sim/views/__init__.py | 2 +- .../isaaclab/sim/views/xform_prim_view.py | 3 +- .../isaaclab/test/sim/test_xform_prim_view.py | 88 +++++++++---------- 9 files changed, 66 insertions(+), 65 deletions(-) diff --git a/docs/source/api/lab/isaaclab.sim.views.rst b/docs/source/api/lab/isaaclab.sim.views.rst index cf850154193..3a5f9bdecfe 100644 --- a/docs/source/api/lab/isaaclab.sim.views.rst +++ b/docs/source/api/lab/isaaclab.sim.views.rst @@ -7,11 +7,11 @@ .. autosummary:: - XFormPrimView + XformPrimView XForm Prim View --------------- -.. autoclass:: XFormPrimView +.. autoclass:: XformPrimView :members: :show-inheritance: diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 9e538d9c537..e32e79a1194 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -10,7 +10,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Benchmark script comparing Isaac Lab's XFormPrimView against Isaac Sim's XFormPrimView. +"""Benchmark script comparing Isaac Lab's XformPrimView against Isaac Sim's XformPrimView. This script tests the performance of batched transform operations using either Isaac Lab's implementation or Isaac Sim's implementation. @@ -23,8 +23,8 @@ ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --profile --headless # Then visualize with snakeviz: - snakeviz profile_results/isaaclab_xformprimview.prof - snakeviz profile_results/isaacsim_xformprimview.prof + snakeviz profile_results/isaaclab_XformPrimView.prof + snakeviz profile_results/isaacsim_XformPrimView.prof """ from __future__ import annotations @@ -38,7 +38,7 @@ # parse the arguments args_cli = argparse.Namespace() -parser = argparse.ArgumentParser(description="This script can help you benchmark the performance of XFormPrimView.") +parser = argparse.ArgumentParser(description="This script can help you benchmark the performance of XformPrimView.") parser.add_argument("--num_envs", type=int, default=100, help="Number of environments to simulate.") parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") @@ -123,9 +123,9 @@ def benchmark_xform_prim_view( # Create view start_time = time.perf_counter() if api == "isaaclab": - xform_view = IsaacLabXFormPrimView(pattern, device=args_cli.device) + xform_view = IsaacLabXformPrimView(pattern, device=args_cli.device) elif api == "isaacsim": - xform_view = IsaacSimXFormPrimView(pattern) + xform_view = IsaacSimXformPrimView(pattern) else: raise ValueError(f"Invalid API: {api}") timing_results["init"] = time.perf_counter() - start_time diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 7bfaff816ab..3a884f3e6be 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -29,7 +29,7 @@ from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg from isaaclab.sim import SimulationContext from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id -from isaaclab.sim.views import XFormPrimView +from isaaclab.sim.views import XformPrimView from isaaclab.terrains import TerrainImporter, TerrainImporterCfg from isaaclab.utils.version import get_isaac_sim_version @@ -406,10 +406,10 @@ def surface_grippers(self) -> dict[str, SurfaceGripper]: return self._surface_grippers @property - def extras(self) -> dict[str, XFormPrimView]: + def extras(self) -> dict[str, XformPrimView]: """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. - The keys are the names of the miscellaneous objects, and the values are the :class:`~isaaclab.sim.utils.views.XFormPrimView` + The keys are the names of the miscellaneous objects, and the values are the :class:`~isaaclab.sim.utils.views.XformPrimView` of the corresponding prims. As an example, lights or other props in the scene that do not have any attributes or properties that you @@ -777,7 +777,7 @@ def _add_entities_from_cfg(self): ) # store xform prim view corresponding to this asset # all prims in the scene are Xform prims (i.e. have a transform component) - self._extras[asset_name] = XFormPrimView(asset_cfg.prim_path, reset_xform_properties=False) + self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, reset_xform_properties=False) else: raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") # store global collision paths diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 7e12d6d4238..25a53b7f2fa 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -21,7 +21,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.sensors as sensor_utils -from isaaclab.sim.views import XFormPrimView +from isaaclab.sim.views import XformPrimView from isaaclab.utils import to_camel_case from isaaclab.utils.array import convert_to_torch from isaaclab.utils.math import ( @@ -405,7 +405,7 @@ def _initialize_impl(self): # Initialize parent class super()._initialize_impl() # Create a view for the sensor - self._view = XFormPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) + self._view = XformPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index 69e01874586..a4b4024750d 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -16,7 +16,7 @@ import warp as wp from pxr import UsdGeom -from isaaclab.sim.views import XFormPrimView +from isaaclab.sim.views import XformPrimView from isaaclab.utils.warp.kernels import reshape_tiled_image from ..sensor_base import SensorBase @@ -150,7 +150,7 @@ def _initialize_impl(self): # Initialize parent class SensorBase._initialize_impl(self) # Create a view for the sensor - self._view = XFormPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) + self._view = XformPrimView(self.cfg.prim_path, device=self._device, stage=self.stage) # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 2b273fd57fe..64bd3e7e7dd 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -20,7 +20,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.markers import VisualizationMarkers -from isaaclab.sim.views import XFormPrimView +from isaaclab.sim.views import XformPrimView from isaaclab.terrains.trimesh.utils import make_plane from isaaclab.utils.math import quat_apply, quat_apply_yaw from isaaclab.utils.warp import convert_to_warp_mesh, raycast_mesh @@ -333,7 +333,7 @@ def _debug_vis_callback(self, event): def _obtain_trackable_prim_view( self, target_prim_path: str - ) -> tuple[XFormPrimView | any, tuple[torch.Tensor, torch.Tensor]]: + ) -> tuple[XformPrimView | any, tuple[torch.Tensor, torch.Tensor]]: """Obtain a prim view that can be used to track the pose of the parget prim. The target prim path is a regex expression that matches one or more mesh prims. While we can track its @@ -376,7 +376,7 @@ def _obtain_trackable_prim_view( new_root_prim = current_prim.GetParent() current_path_expr = current_path_expr.rsplit("/", 1)[0] if not new_root_prim.IsValid(): - prim_view = XFormPrimView(target_prim_path, device=self._device, stage=self.stage) + prim_view = XformPrimView(target_prim_path, device=self._device, stage=self.stage) current_path_expr = target_prim_path logger.warning( f"The prim at path {target_prim_path} which is used for raycasting is not a physics prim." diff --git a/source/isaaclab/isaaclab/sim/views/__init__.py b/source/isaaclab/isaaclab/sim/views/__init__.py index 2f1d56ed69f..eb5bea7690c 100644 --- a/source/isaaclab/isaaclab/sim/views/__init__.py +++ b/source/isaaclab/isaaclab/sim/views/__init__.py @@ -5,4 +5,4 @@ """Views for manipulating USD prims.""" -from .xform_prim_view import XFormPrimView +from .xform_prim_view import XformPrimView diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 9965fbbbb6b..a93f3cb7af2 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -13,7 +13,8 @@ import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils -class XFormPrimView: + +class XformPrimView: """Optimized batched interface for reading and writing transforms of multiple USD prims. This class provides efficient batch operations for getting and setting poses (position and orientation) diff --git a/source/isaaclab/test/sim/test_xform_prim_view.py b/source/isaaclab/test/sim/test_xform_prim_view.py index 57e8d5e05c4..d3407611e51 100644 --- a/source/isaaclab/test/sim/test_xform_prim_view.py +++ b/source/isaaclab/test/sim/test_xform_prim_view.py @@ -17,12 +17,12 @@ import pytest try: - from isaacsim.core.prims import XFormPrim as _IsaacSimXFormPrimView + from isaacsim.core.prims import XFormPrim as _IsaacSimXformPrimView except (ModuleNotFoundError, ImportError): - _IsaacSimXFormPrimView = None + _IsaacSimXformPrimView = None import isaaclab.sim as sim_utils -from isaaclab.sim.views import XFormPrimView as XFormPrimView +from isaaclab.sim.views import XformPrimView as XformPrimView from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -47,7 +47,7 @@ def test_setup_teardown(): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_xform_prim_view_initialization_single_prim(device): - """Test XFormPrimView initialization with a single prim.""" + """Test XformPrimView initialization with a single prim.""" # check if CUDA is available if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -57,7 +57,7 @@ def test_xform_prim_view_initialization_single_prim(device): sim_utils.create_prim("/World/Object", "Xform", translation=(1.0, 2.0, 3.0), stage=stage) # Create view - view = XFormPrimView("/World/Object", device=device) + view = XformPrimView("/World/Object", device=device) # Verify properties assert view.count == 1 @@ -68,7 +68,7 @@ def test_xform_prim_view_initialization_single_prim(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_xform_prim_view_initialization_multiple_prims(device): - """Test XFormPrimView initialization with multiple prims using pattern matching.""" + """Test XformPrimView initialization with multiple prims using pattern matching.""" # check if CUDA is available if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -80,7 +80,7 @@ def test_xform_prim_view_initialization_multiple_prims(device): sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(i * 2.0, 0.0, 1.0), stage=stage) # Create view with pattern - view = XFormPrimView("/World/Env_.*/Object", device=device) + view = XformPrimView("/World/Env_.*/Object", device=device) # Verify properties assert view.count == num_prims @@ -90,7 +90,7 @@ def test_xform_prim_view_initialization_multiple_prims(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_xform_prim_view_initialization_invalid_prim(device): - """Test XFormPrimView initialization fails for non-xformable prims.""" + """Test XformPrimView initialization fails for non-xformable prims.""" # check if CUDA is available if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -100,14 +100,14 @@ def test_xform_prim_view_initialization_invalid_prim(device): # Create a prim with non-standard xform operations stage.DefinePrim("/World/InvalidPrim", "Xform") - # XFormPrimView should raise ValueError because prim doesn't have standard operations + # XformPrimView should raise ValueError because prim doesn't have standard operations with pytest.raises(ValueError, match="not a xformable prim"): - XFormPrimView("/World/InvalidPrim", device=device) + XformPrimView("/World/InvalidPrim", device=device) @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_xform_prim_view_initialization_empty_pattern(device): - """Test XFormPrimView initialization with pattern that matches no prims.""" + """Test XformPrimView initialization with pattern that matches no prims.""" # check if CUDA is available if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -115,7 +115,7 @@ def test_xform_prim_view_initialization_empty_pattern(device): sim_utils.create_new_stage() # Create view with pattern that matches nothing - view = XFormPrimView("/World/NonExistent_.*", device=device) + view = XformPrimView("/World/NonExistent_.*", device=device) # Should have zero count assert view.count == 0 @@ -129,7 +129,7 @@ def test_xform_prim_view_initialization_empty_pattern(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_get_world_poses(device): - """Test getting world poses from XFormPrimView.""" + """Test getting world poses from XformPrimView.""" if device.startswith("cuda") and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -143,7 +143,7 @@ def test_get_world_poses(device): sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=pos, orientation=quat, stage=stage) # Create view - view = XFormPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Object_.*", device=device) # Get world poses positions, orientations = view.get_world_poses() @@ -168,7 +168,7 @@ def test_get_world_poses(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_set_world_poses(device): - """Test setting world poses in XFormPrimView.""" + """Test setting world poses in XformPrimView.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -180,7 +180,7 @@ def test_set_world_poses(device): sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) # Create view - view = XFormPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Object_.*", device=device) # Set new world poses new_positions = torch.tensor( @@ -227,7 +227,7 @@ def test_set_world_poses_only_positions(device): ) # Create view - view = XFormPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Object_.*", device=device) # Get initial orientations _, initial_orientations = view.get_world_poses() @@ -262,7 +262,7 @@ def test_set_world_poses_only_orientations(device): sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) # Create view - view = XFormPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Object_.*", device=device) # Get initial positions initial_positions, _ = view.get_world_poses() @@ -306,7 +306,7 @@ def test_set_world_poses_with_hierarchy(device): sim_utils.create_prim(f"/World/Parent_{i}/Child", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) # Create view for children - view = XFormPrimView("/World/Parent_.*/Child", device=device) + view = XformPrimView("/World/Parent_.*/Child", device=device) # Set world poses for children desired_world_positions = torch.tensor([[5.0, 5.0, 0.0], [15.0, 5.0, 0.0], [25.0, 5.0, 0.0]], device=device) @@ -334,7 +334,7 @@ def test_set_world_poses_with_hierarchy(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_get_local_poses(device): - """Test getting local poses from XFormPrimView.""" + """Test getting local poses from XformPrimView.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -355,7 +355,7 @@ def test_get_local_poses(device): sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=pos, orientation=quat, stage=stage) # Create view - view = XFormPrimView("/World/Parent/Child_.*", device=device) + view = XformPrimView("/World/Parent/Child_.*", device=device) # Get local poses translations, orientations = view.get_local_poses() @@ -380,7 +380,7 @@ def test_get_local_poses(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_set_local_poses(device): - """Test setting local poses in XFormPrimView.""" + """Test setting local poses in XformPrimView.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -395,7 +395,7 @@ def test_set_local_poses(device): sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) # Create view - view = XFormPrimView("/World/Parent/Child_.*", device=device) + view = XformPrimView("/World/Parent/Child_.*", device=device) # Set new local poses new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0], [4.0, 4.0, 4.0]], device=device) @@ -440,7 +440,7 @@ def test_set_local_poses_only_translations(device): ) # Create view - view = XFormPrimView("/World/Parent/Child_.*", device=device) + view = XformPrimView("/World/Parent/Child_.*", device=device) # Get initial orientations _, initial_orientations = view.get_local_poses() @@ -469,7 +469,7 @@ def test_set_local_poses_only_translations(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_get_scales(device): - """Test getting scales from XFormPrimView.""" + """Test getting scales from XformPrimView.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -482,7 +482,7 @@ def test_get_scales(device): sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) # Create view - view = XFormPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Object_.*", device=device) # Get scales scales = view.get_scales() @@ -495,7 +495,7 @@ def test_get_scales(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_set_scales(device): - """Test setting scales in XFormPrimView.""" + """Test setting scales in XformPrimView.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -507,7 +507,7 @@ def test_set_scales(device): sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=(1.0, 1.0, 1.0), stage=stage) # Create view - view = XFormPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Object_.*", device=device) # Set new scales new_scales = torch.tensor( @@ -530,7 +530,7 @@ def test_set_scales(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_with_franka_robots(device): - """Test XFormPrimView with real Franka robot USD assets.""" + """Test XformPrimView with real Franka robot USD assets.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -544,7 +544,7 @@ def test_with_franka_robots(device): sim_utils.create_prim("/World/Franka_2", "Xform", usd_path=franka_usd_path, stage=stage) # Create view for both Frankas - frankas_view = XFormPrimView("/World/Franka_.*", device=device) + frankas_view = XformPrimView("/World/Franka_.*", device=device) # Verify count assert frankas_view.count == 2 @@ -596,8 +596,8 @@ def test_with_nested_targets(device): sim_utils.create_prim(f"/World/Frame_{i}/Target", "Xform", stage=stage) # Create views - frames_view = XFormPrimView("/World/Frame_.*", device=device) - targets_view = XFormPrimView("/World/Frame_.*/Target", device=device) + frames_view = XformPrimView("/World/Frame_.*", device=device) + targets_view = XformPrimView("/World/Frame_.*/Target", device=device) assert frames_view.count == 3 assert targets_view.count == 3 @@ -629,7 +629,7 @@ def test_compare_get_world_poses_with_isaacsim(): stage = sim_utils.get_current_stage() # Check if Isaac Sim is available - if _IsaacSimXFormPrimView is None: + if _IsaacSimXformPrimView is None: pytest.skip("Isaac Sim is not available") # Create prims with various poses @@ -648,8 +648,8 @@ def test_compare_get_world_poses_with_isaacsim(): pattern = "/World/Env_.*/Object" # Create both views - isaaclab_view = XFormPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) # Get world poses from both isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() @@ -676,7 +676,7 @@ def test_compare_set_world_poses_with_isaacsim(): stage = sim_utils.get_current_stage() # Check if Isaac Sim is available - if _IsaacSimXFormPrimView is None: + if _IsaacSimXformPrimView is None: pytest.skip("Isaac Sim is not available") # Create prims @@ -687,8 +687,8 @@ def test_compare_set_world_poses_with_isaacsim(): pattern = "/World/Env_.*/Object" # Create both views - isaaclab_view = XFormPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) # Generate new poses new_positions = torch.randn(num_prims, 3) * 10.0 @@ -721,7 +721,7 @@ def test_compare_get_local_poses_with_isaacsim(): stage = sim_utils.get_current_stage() # Check if Isaac Sim is available - if _IsaacSimXFormPrimView is None: + if _IsaacSimXformPrimView is None: pytest.skip("Isaac Sim is not available") # Create hierarchical prims @@ -739,8 +739,8 @@ def test_compare_get_local_poses_with_isaacsim(): pattern = "/World/Env_.*/Object" # Create both views - isaaclab_view = XFormPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) # Get local poses from both isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() @@ -765,7 +765,7 @@ def test_compare_set_local_poses_with_isaacsim(): stage = sim_utils.get_current_stage() # Check if Isaac Sim is available - if _IsaacSimXFormPrimView is None: + if _IsaacSimXformPrimView is None: pytest.skip("Isaac Sim is not available") # Create hierarchical prims @@ -777,8 +777,8 @@ def test_compare_set_local_poses_with_isaacsim(): pattern = "/World/Env_.*/Object" # Create both views - isaaclab_view = XFormPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXFormPrimView(pattern, reset_xform_properties=False) + isaaclab_view = XformPrimView(pattern, device="cpu") + isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) # Generate new local poses new_translations = torch.randn(num_prims, 3) * 5.0 From 4aab3c5f5b84370753e3682625e14991b06f1157 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:31:19 +0100 Subject: [PATCH 037/100] adds type ignore --- source/isaaclab/isaaclab/sim/views/xform_prim_view.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index a93f3cb7af2..6ee7bd9eeee 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -85,6 +85,10 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None " Use sim_utils.standardize_xform_ops() to prepare the prim." ) + """ + Properties. + """ + @property def count(self) -> int: """Number of prims in this view. @@ -311,7 +315,7 @@ def get_world_poses(self) -> tuple[torch.Tensor, torch.Tensor]: orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) orientations = math_utils.convert_quat(orientations, to="wxyz") - return positions, orientations + return positions, orientations # type: ignore def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: """Get local-space poses for all prims in the view. @@ -346,7 +350,7 @@ def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) orientations = math_utils.convert_quat(orientations, to="wxyz") - return translations, orientations + return translations, orientations # type: ignore def get_scales(self) -> torch.Tensor: """Get scales for all prims in the view. From 9de9feab0e874fcf0d182def8b6faf632cfc3910 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:33:47 +0100 Subject: [PATCH 038/100] adds validation arg --- .../isaaclab/sim/views/xform_prim_view.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 6ee7bd9eeee..8407fb5b3eb 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -47,13 +47,18 @@ class XformPrimView: time-sampled keyframes separately. """ - def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None): + def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None, validate_xform_ops: bool = True): """Initialize the view with matching prims. This method searches the USD stage for all prims matching the provided path pattern, validates that they are Xformable with standard transform operations, and stores references for efficient batch operations. + We generally recommend to validate the xform operations, as it ensures that the prims are in a consistent state + and have the standard transform operations (translate, orient, scale in that order). + However, if you are sure that the prims are in a consistent state, you can set this to False to improve + performance. This can save around 45-50% of the time taken to initialize the view. + Args: prim_path: USD prim path pattern to match prims. Supports wildcards (``*``) and regex patterns (e.g., ``"/World/Env_.*/Robot"``). See @@ -62,6 +67,8 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None ``"cuda:0"``. Defaults to ``"cpu"``. stage: USD stage to search for prims. If None, uses the current active stage from the simulation context. Defaults to None. + validate_xform_ops: Whether to validate that the prims have standard xform operations. + Defaults to True. Raises: ValueError: If any matched prim is not Xformable or doesn't have standardized @@ -77,13 +84,14 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) # Validate all prims have standard xform operations - for prim in self._prims: - if not sim_utils.validate_standard_xform_ops(prim): - raise ValueError( - f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" - f" operations [translate, orient, scale]. Received type: '{prim.GetTypeName()}'." - " Use sim_utils.standardize_xform_ops() to prepare the prim." - ) + if validate_xform_ops: + for prim in self._prims: + if not sim_utils.validate_standard_xform_ops(prim): + raise ValueError( + f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" + f" operations [translate, orient, scale]. Received type: '{prim.GetTypeName()}'." + " Use sim_utils.standardize_xform_ops() to prepare the prim." + ) """ Properties. From b5d1e3878123d921391cb08f487e6756fe3adbf7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:37:27 +0100 Subject: [PATCH 039/100] fixes for new xform class --- source/isaaclab/isaaclab/sensors/camera/camera.py | 6 +++--- .../isaaclab/isaaclab/sensors/camera/tiled_camera.py | 11 ++++++----- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 25a53b7f2fa..d5773bf24f9 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -423,9 +423,9 @@ def _initialize_impl(self): self._rep_registry: dict[str, list[rep.annotators.Annotator]] = {name: list() for name in self.cfg.data_types} # Convert all encapsulated prims to Camera - for cam_prim_path in self._view.prim_paths: - # Get camera prim - cam_prim = self.stage.GetPrimAtPath(cam_prim_path) + for cam_prim in self._view.prims: + # Obtain the prim path + cam_prim_path = cam_prim.GetPath().pathString # Check if prim is a camera if not cam_prim.IsA(UsdGeom.Camera): raise RuntimeError(f"Prim at path '{cam_prim_path}' is not a Camera.") diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index a4b4024750d..8a9696f4f16 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -164,19 +164,20 @@ def _initialize_impl(self): self._frame = torch.zeros(self._view.count, device=self._device, dtype=torch.long) # Convert all encapsulated prims to Camera - for cam_prim_path in self._view.prim_paths: + cam_prim_paths = [] + for cam_prim in self._view.prims: # Get camera prim - cam_prim = self.stage.GetPrimAtPath(cam_prim_path) + cam_prim_path = cam_prim.GetPath().pathString # Check if prim is a camera if not cam_prim.IsA(UsdGeom.Camera): raise RuntimeError(f"Prim at path '{cam_prim_path}' is not a Camera.") # Add to list - sensor_prim = UsdGeom.Camera(cam_prim) - self._sensor_prims.append(sensor_prim) + self._sensor_prims.append(UsdGeom.Camera(cam_prim)) + cam_prim_paths.append(cam_prim_path) # Create replicator tiled render product rp = rep.create.render_product_tiled( - cameras=self._view.prim_paths, tile_resolution=(self.cfg.width, self.cfg.height) + cameras=cam_prim_paths, tile_resolution=(self.cfg.width, self.cfg.height) ) self._render_product_paths = [rp.path] From 3145c5252e3dbca9196b8ed9a1ed2ca060d4a78e Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:53:05 +0100 Subject: [PATCH 040/100] adds support for indices --- .../isaaclab/sim/views/xform_prim_view.py | 187 ++++++++++++------ 1 file changed, 125 insertions(+), 62 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 8407fb5b3eb..284d121935d 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -65,8 +65,8 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. device: Device to place the tensors on. Can be ``"cpu"`` or CUDA devices like ``"cuda:0"``. Defaults to ``"cpu"``. - stage: USD stage to search for prims. If None, uses the current active stage - from the simulation context. Defaults to None. + stage: USD stage to search for prims. Defaults to None, in which case the current active stage + from the simulation context is used. validate_xform_ops: Whether to validate that the prims have standard xform operations. Defaults to True. @@ -83,6 +83,10 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None # Find and validate matching prims self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) + # Create indices buffer + # Since we iterate over the indices, we need to use range instead of torch tensor + self._ALL_INDICES = list(range(len(self._prims))) + # Validate all prims have standard xform operations if validate_xform_ops: for prim in self._prims: @@ -125,8 +129,8 @@ def device(self) -> str: Operations - Setters. """ - def set_world_poses(self, positions: torch.Tensor | None = None, orientations: torch.Tensor | None = None) -> None: - """Set world-space poses for all prims in the view. + def set_world_poses(self, positions: torch.Tensor | None = None, orientations: torch.Tensor | None = None, indices: torch.Tensor | None = None): + """Set world-space poses for prims in the view. This method sets the position and/or orientation of each prim in world space. The world pose is computed by considering the prim's parent transforms. If a prim has a parent, this method @@ -136,31 +140,38 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t This operation writes to USD at the default time code. Any animation data will not be affected. Args: - positions: World-space positions as a tensor of shape (N, 3) where N is the number of prims. - If None, positions are not modified. Defaults to None. - orientations: World-space orientations as quaternions (w, x, y, z) with shape (N, 4). - If None, orientations are not modified. Defaults to None. + positions: World-space positions as a tensor of shape (M, 3) where M is the number of prims + to set (either all prims if indices is None, or the number of indices provided). + Defaults to None, in which case positions are not modified. + orientations: World-space orientations as quaternions (w, x, y, z) with shape (M, 4). + Defaults to None, in which case orientations are not modified. + indices: Indices of prims to set poses for. Defaults to None, in which case poses are set + for all prims in the view. Raises: - ValueError: If positions shape is not (N, 3) or orientations shape is not (N, 4). - ValueError: If the number of poses doesn't match the number of prims in the view. + ValueError: If positions shape is not (M, 3) or orientations shape is not (M, 4). + ValueError: If the number of poses doesn't match the number of indices provided. """ + # Resolve indices + indices_list = self._ALL_INDICES if indices is None else indices.tolist() + # Validate inputs if positions is not None: - if positions.shape != (self.count, 3): + if positions.shape != (len(indices_list), 3): raise ValueError( - f"Expected positions shape ({self.count}, 3), got {positions.shape}. " + f"Expected positions shape ({len(indices_list)}, 3), got {positions.shape}. " "Number of positions must match the number of prims in the view." ) positions_array = Vt.Vec3dArray.FromNumpy(positions.cpu().numpy()) else: positions_array = None if orientations is not None: - if orientations.shape != (self.count, 4): + if orientations.shape != (len(indices_list), 4): raise ValueError( - f"Expected orientations shape ({self.count}, 4), got {orientations.shape}. " + f"Expected orientations shape ({len(indices_list)}, 4), got {orientations.shape}. " "Number of orientations must match the number of prims in the view." ) + # Vt expects quaternions in xyzw order orientations_array = Vt.QuatdArray.FromNumpy(math_utils.convert_quat(orientations, to="xyzw").cpu().numpy()) else: orientations_array = None @@ -168,7 +179,9 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t # Set poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): - for idx, prim in enumerate(self._prims): + for idx in indices_list: + # Get prim + prim = self._prims[idx] # Get parent prim for local space conversion parent_prim = prim.GetParent() @@ -215,9 +228,9 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t prim.GetAttribute("xformOp:orient").Set(local_quat) def set_local_poses( - self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None + self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None, indices: torch.Tensor | None = None ) -> None: - """Set local-space poses for all prims in the view. + """Set local-space poses for prims in the view. This method sets the position and/or orientation of each prim in local space (relative to their parent prims). This is useful when you want to directly manipulate the prim's transform @@ -227,71 +240,90 @@ def set_local_poses( This operation writes to USD at the default time code. Any animation data will not be affected. Args: - translations: Local-space translations as a tensor of shape (N, 3) where N is the number of prims. - If None, translations are not modified. Defaults to None. - orientations: Local-space orientations as quaternions (w, x, y, z) with shape (N, 4). - If None, orientations are not modified. Defaults to None. + translations: Local-space translations as a tensor of shape (M, 3) where M is the number of prims + to set (either all prims if indices is None, or the number of indices provided). + Defaults to None, in which case translations are not modified. + orientations: Local-space orientations as quaternions (w, x, y, z) with shape (M, 4). + Defaults to None, in which case orientations are not modified. + indices: Indices of prims to set poses for. Defaults to None, in which case poses are set + for all prims in the view. Raises: - ValueError: If translations shape is not (N, 3) or orientations shape is not (N, 4). - ValueError: If the number of poses doesn't match the number of prims in the view. + ValueError: If translations shape is not (M, 3) or orientations shape is not (M, 4). + ValueError: If the number of poses doesn't match the number of indices provided. """ + # Resolve indices + indices_list = self._ALL_INDICES if indices is None else indices.tolist() + # Validate inputs if translations is not None: - if translations.shape != (self.count, 3): + if translations.shape != (len(indices_list), 3): raise ValueError( - f"Expected translations shape ({self.count}, 3), got {translations.shape}. " + f"Expected translations shape ({len(indices_list)}, 3), got {translations.shape}. " "Number of translations must match the number of prims in the view." ) translations_array = Vt.Vec3dArray.FromNumpy(translations.cpu().numpy()) else: translations_array = None if orientations is not None: - if orientations.shape != (self.count, 4): + if orientations.shape != (len(indices_list), 4): raise ValueError( - f"Expected orientations shape ({self.count}, 4), got {orientations.shape}. " + f"Expected orientations shape ({len(indices_list)}, 4), got {orientations.shape}. " "Number of orientations must match the number of prims in the view." ) + # Vt expects quaternions in xyzw order orientations_array = Vt.QuatdArray.FromNumpy(math_utils.convert_quat(orientations, to="xyzw").cpu().numpy()) else: orientations_array = None # Set local poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): - for idx, prim in enumerate(self._prims): + for idx in indices_list: + # Get prim + prim = self._prims[idx] + # Set attributes if provided if translations_array is not None: - local_pos = translations_array[idx] - prim.GetAttribute("xformOp:translate").Set(local_pos) + prim.GetAttribute("xformOp:translate").Set(translations_array[idx]) if orientations_array is not None: - local_quat = orientations_array[idx] - prim.GetAttribute("xformOp:orient").Set(local_quat) + prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) - def set_scales(self, scales: torch.Tensor): - """Set scales for all prims in the view. + def set_scales(self, scales: torch.Tensor, indices: torch.Tensor | None = None): + """Set scales for prims in the view. This method sets the scale of each prim in the view. Args: - scales: Scales as a tensor of shape (N, 3) where N is the number of prims. + scales: Scales as a tensor of shape (M, 3) where M is the number of prims + to set (either all prims if indices is None, or the number of indices provided). + indices: Indices of prims to set scales for. Defaults to None, in which case scales are set + for all prims in the view. + + Raises: + ValueError: If scales shape is not (M, 3). """ + # Resolve indices + indices_list = self._ALL_INDICES if indices is None else indices.tolist() + # Validate inputs - if scales.shape != (self.count, 3): - raise ValueError(f"Expected scales shape ({self.count}, 3), got {scales.shape}.") + if scales.shape != (len(indices_list), 3): + raise ValueError(f"Expected scales shape ({len(indices_list)}, 3), got {scales.shape}.") scales_array = Vt.Vec3dArray.FromNumpy(scales.cpu().numpy()) # Set scales for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): - for idx, prim in enumerate(self._prims): - scale = scales_array[idx] - prim.GetAttribute("xformOp:scale").Set(scale) + for idx in indices_list: + # Get prim + prim = self._prims[idx] + # Set scale attribute + prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) """ Operations - Getters. """ - def get_world_poses(self) -> tuple[torch.Tensor, torch.Tensor]: - """Get world-space poses for all prims in the view. + def get_world_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get world-space poses for prims in the view. This method retrieves the position and orientation of each prim in world space by computing the full transform hierarchy from the prim to the world root. @@ -299,16 +331,26 @@ def get_world_poses(self) -> tuple[torch.Tensor, torch.Tensor]: Note: Scale and skew are ignored. The returned poses contain only translation and rotation. + Args: + indices: Indices of prims to get poses for. Defaults to None, in which case poses are retrieved + for all prims in the view. + Returns: A tuple of (positions, orientations) where: - - positions: Torch tensor of shape (N, 3) containing world-space positions (x, y, z) - - orientations: Torch tensor of shape (N, 4) containing world-space quaternions (w, x, y, z) + - positions: Torch tensor of shape (M, 3) containing world-space positions (x, y, z), + where M is the number of prims queried. + - orientations: Torch tensor of shape (M, 4) containing world-space quaternions (w, x, y, z) """ - positions = Vt.Vec3dArray(self.count) - orientations = Vt.QuatdArray(self.count) - - for idx, prim in enumerate(self._prims): + # Resolve indices + indices_list = self._ALL_INDICES if indices is None else indices.tolist() + # Create buffers + positions = Vt.Vec3dArray(len(indices_list)) + orientations = Vt.QuatdArray(len(indices_list)) + + for idx in indices_list: + # Get prim + prim = self._prims[idx] # get prim xform prim_tf = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) # sanitize quaternion @@ -321,12 +363,13 @@ def get_world_poses(self) -> tuple[torch.Tensor, torch.Tensor]: # move to torch tensors positions = torch.tensor(np.array(positions), dtype=torch.float32, device=self._device) orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) + # underlying data is in xyzw order, convert to wxyz order orientations = math_utils.convert_quat(orientations, to="wxyz") return positions, orientations # type: ignore - def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: - """Get local-space poses for all prims in the view. + def get_local_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Tensor, torch.Tensor]: + """Get local-space poses for prims in the view. This method retrieves the position and orientation of each prim in local space (relative to their parent prims). These are the raw transform values stored on each prim. @@ -334,16 +377,26 @@ def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: Note: Scale is ignored. The returned poses contain only translation and rotation. + Args: + indices: Indices of prims to get poses for. Defaults to None, in which case poses are retrieved + for all prims in the view. + Returns: A tuple of (translations, orientations) where: - - translations: Torch tensor of shape (N, 3) containing local-space translations (x, y, z) - - orientations: Torch tensor of shape (N, 4) containing local-space quaternions (w, x, y, z) + - translations: Torch tensor of shape (M, 3) containing local-space translations (x, y, z), + where M is the number of prims queried. + - orientations: Torch tensor of shape (M, 4) containing local-space quaternions (w, x, y, z) """ - translations = Vt.Vec3dArray(self.count) - orientations = Vt.QuatdArray(self.count) - - for idx, prim in enumerate(self._prims): + # Resolve indices + indices_list = self._ALL_INDICES if indices is None else indices.tolist() + # Create buffers + translations = Vt.Vec3dArray(len(indices_list)) + orientations = Vt.QuatdArray(len(indices_list)) + + for idx in indices_list: + # Get prim + prim = self._prims[idx] # get prim xform prim_tf = UsdGeom.Xformable(prim).GetLocalTransformation(Usd.TimeCode.Default()) # sanitize quaternion @@ -356,21 +409,31 @@ def get_local_poses(self) -> tuple[torch.Tensor, torch.Tensor]: # move to torch tensors translations = torch.tensor(np.array(translations), dtype=torch.float32, device=self._device) orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) + # underlying data is in xyzw order, convert to wxyz order orientations = math_utils.convert_quat(orientations, to="wxyz") return translations, orientations # type: ignore - def get_scales(self) -> torch.Tensor: - """Get scales for all prims in the view. + def get_scales(self, indices: torch.Tensor | None = None) -> torch.Tensor: + """Get scales for prims in the view. This method retrieves the scale of each prim in the view. + Args: + indices: Indices of prims to get scales for. Defaults to None, in which case scales are retrieved + for all prims in the view. + Returns: - A tensor of shape (N, 3) containing the scales of each prim. + A tensor of shape (M, 3) containing the scales of each prim, where M is the number of prims queried. """ - scales = Vt.Vec3dArray(self.count) - - for idx, prim in enumerate(self._prims): + # Resolve indices + indices_list = self._ALL_INDICES if indices is None else indices.tolist() + # Create buffers + scales = Vt.Vec3dArray(len(indices_list)) + + for idx in indices_list: + # Get prim + prim = self._prims[idx] scales[idx] = prim.GetAttribute("xformOp:scale").Get() # Convert to tensor From 82f10f4b572ebdf36f204e6f30560fc11b62d86b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 17:53:58 +0100 Subject: [PATCH 041/100] runs formatter --- .../isaaclab/sensors/camera/tiled_camera.py | 4 +--- .../isaaclab/sim/views/xform_prim_view.py | 20 ++++++++++++++----- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index 8a9696f4f16..82c43455208 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -176,9 +176,7 @@ def _initialize_impl(self): cam_prim_paths.append(cam_prim_path) # Create replicator tiled render product - rp = rep.create.render_product_tiled( - cameras=cam_prim_paths, tile_resolution=(self.cfg.width, self.cfg.height) - ) + rp = rep.create.render_product_tiled(cameras=cam_prim_paths, tile_resolution=(self.cfg.width, self.cfg.height)) self._render_product_paths = [rp.path] # Define the annotators based on requested data types diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 284d121935d..699a89ec2a3 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -47,7 +47,9 @@ class XformPrimView: time-sampled keyframes separately. """ - def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None, validate_xform_ops: bool = True): + def __init__( + self, prim_path: str, device: str = "cpu", validate_xform_ops: bool = True, stage: Usd.Stage | None = None + ): """Initialize the view with matching prims. This method searches the USD stage for all prims matching the provided path pattern, @@ -65,10 +67,10 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. device: Device to place the tensors on. Can be ``"cpu"`` or CUDA devices like ``"cuda:0"``. Defaults to ``"cpu"``. - stage: USD stage to search for prims. Defaults to None, in which case the current active stage - from the simulation context is used. validate_xform_ops: Whether to validate that the prims have standard xform operations. Defaults to True. + stage: USD stage to search for prims. Defaults to None, in which case the current active stage + from the simulation context is used. Raises: ValueError: If any matched prim is not Xformable or doesn't have standardized @@ -129,7 +131,12 @@ def device(self) -> str: Operations - Setters. """ - def set_world_poses(self, positions: torch.Tensor | None = None, orientations: torch.Tensor | None = None, indices: torch.Tensor | None = None): + def set_world_poses( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: torch.Tensor | None = None, + ): """Set world-space poses for prims in the view. This method sets the position and/or orientation of each prim in world space. The world pose @@ -228,7 +235,10 @@ def set_world_poses(self, positions: torch.Tensor | None = None, orientations: t prim.GetAttribute("xformOp:orient").Set(local_quat) def set_local_poses( - self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None, indices: torch.Tensor | None = None + self, + translations: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + indices: torch.Tensor | None = None, ) -> None: """Set local-space poses for prims in the view. From 9104778d8d893c1ead94a62d0305683dab828065 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 18:02:50 +0100 Subject: [PATCH 042/100] modifies script to benchmark different xform apis --- .../benchmarks/benchmark_xform_prim_view.py | 336 +++++++++++------- 1 file changed, 203 insertions(+), 133 deletions(-) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index e32e79a1194..416f8c5c95e 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -10,13 +10,15 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Benchmark script comparing Isaac Lab's XformPrimView against Isaac Sim's XformPrimView. +"""Benchmark script comparing XformPrimView implementations across different APIs. -This script tests the performance of batched transform operations using either -Isaac Lab's implementation or Isaac Sim's implementation. +This script tests the performance of batched transform operations using: +- Isaac Lab's XformPrimView implementation +- Isaac Sim's XformPrimView implementation (legacy) +- Isaac Sim Experimental's XformPrim implementation (latest) Usage: - # Basic benchmark + # Basic benchmark (all APIs) ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --device cuda:0 --headless # With profiling enabled (for snakeviz visualization) @@ -25,6 +27,7 @@ # Then visualize with snakeviz: snakeviz profile_results/isaaclab_XformPrimView.prof snakeviz profile_results/isaacsim_XformPrimView.prof + snakeviz profile_results/isaacsim_experimental_XformPrim.prof """ from __future__ import annotations @@ -68,11 +71,12 @@ import torch from typing import Literal +from isaacsim.core.prims import XFormPrim as IsaacSimXformPrimView from isaacsim.core.utils.extensions import enable_extension # compare against latest Isaac Sim implementation enable_extension("isaacsim.core.experimental.prims") -from isaacsim.core.experimental.prims import XformPrim as IsaacSimXformPrimView +from isaacsim.core.experimental.prims import XformPrim as IsaacSimExperimentalXformPrimView import isaaclab.sim as sim_utils from isaaclab.sim.views import XformPrimView as IsaacLabXformPrimView @@ -80,12 +84,12 @@ @torch.no_grad() def benchmark_xform_prim_view( - api: Literal["isaaclab", "isaacsim"], num_iterations: int + api: Literal["isaaclab", "isaacsim", "isaacsim-exp"], num_iterations: int ) -> tuple[dict[str, float], dict[str, torch.Tensor]]: - """Benchmark the Xform view class from either Isaac Lab or Isaac Sim. + """Benchmark the Xform view class from Isaac Lab, Isaac Sim, or Isaac Sim Experimental. Args: - api: Which API to benchmark ("isaaclab" or "isaacsim"). + api: Which API to benchmark ("isaaclab", "isaacsim", or "isaacsim-exp"). num_iterations: Number of iterations to run. Returns: @@ -123,18 +127,20 @@ def benchmark_xform_prim_view( # Create view start_time = time.perf_counter() if api == "isaaclab": - xform_view = IsaacLabXformPrimView(pattern, device=args_cli.device) + xform_view = IsaacLabXformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) elif api == "isaacsim": - xform_view = IsaacSimXformPrimView(pattern) + xform_view = IsaacSimXformPrimView(pattern, reset_xform_properties=False) + elif api == "isaacsim-exp": + xform_view = IsaacSimExperimentalXformPrimView(pattern) else: raise ValueError(f"Invalid API: {api}") timing_results["init"] = time.perf_counter() - start_time - if api == "isaaclab": + if api in ("isaaclab", "isaacsim"): num_prims = xform_view.count - elif api == "isaacsim": + elif api == "isaacsim-exp": num_prims = len(xform_view.prims) - print(f" XformPrimView managing {num_prims} prims") + print(f" XformView managing {num_prims} prims") # Benchmark get_world_poses start_time = time.perf_counter() @@ -157,9 +163,9 @@ def benchmark_xform_prim_view( new_positions[:, 2] += 0.1 start_time = time.perf_counter() for _ in range(num_iterations): - if api == "isaaclab": + if api in ("isaaclab", "isaacsim"): xform_view.set_world_poses(new_positions, orientations) - elif api == "isaacsim": + elif api == "isaacsim-exp": xform_view.set_world_poses(new_positions.cpu().numpy(), orientations.cpu().numpy()) timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations @@ -193,9 +199,9 @@ def benchmark_xform_prim_view( new_translations[:, 2] += 0.1 start_time = time.perf_counter() for _ in range(num_iterations): - if api == "isaaclab": + if api in ("isaaclab", "isaacsim"): xform_view.set_local_poses(new_translations, orientations_local) - elif api == "isaacsim": + elif api == "isaacsim-exp": xform_view.set_local_poses(new_translations.cpu().numpy(), orientations_local.cpu().numpy()) timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations @@ -224,92 +230,108 @@ def benchmark_xform_prim_view( def compare_results( - isaaclab_computed: dict[str, torch.Tensor], isaacsim_computed: dict[str, torch.Tensor], tolerance: float = 1e-4 -) -> dict[str, dict[str, float]]: - """Compare computed results between Isaac Lab and Isaac Sim implementations. + results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 +) -> dict[str, dict[str, dict[str, float]]]: + """Compare computed results across multiple implementations. Args: - isaaclab_computed: Computed values from Isaac Lab's XformPrimView. - isaacsim_computed: Computed values from Isaac Sim's XformPrimView. + results_dict: Dictionary mapping API names to their computed values. tolerance: Tolerance for numerical comparison. Returns: - Dictionary containing comparison statistics (max difference, mean difference, etc.) for each result. + Nested dictionary: {comparison_pair: {metric: {stats}}}, e.g., + {"isaaclab_vs_isaacsim": {"initial_world_positions": {"max_diff": 0.001, ...}}} """ comparison_stats = {} + api_names = list(results_dict.keys()) + + # Compare each pair of APIs + for i, api1 in enumerate(api_names): + for api2 in api_names[i + 1 :]: + pair_key = f"{api1}_vs_{api2}" + comparison_stats[pair_key] = {} - for key in isaaclab_computed.keys(): - if key not in isaacsim_computed: - print(f" Warning: Key '{key}' not found in Isaac Sim results") - continue + computed1 = results_dict[api1] + computed2 = results_dict[api2] - isaaclab_val = isaaclab_computed[key] - isaacsim_val = isaacsim_computed[key] + for key in computed1.keys(): + if key not in computed2: + print(f" Warning: Key '{key}' not found in {api2} results") + continue - # Compute differences - diff = torch.abs(isaaclab_val - isaacsim_val) - max_diff = torch.max(diff).item() - mean_diff = torch.mean(diff).item() + val1 = computed1[key] + val2 = computed2[key] - # Check if within tolerance - all_close = torch.allclose(isaaclab_val, isaacsim_val, atol=tolerance, rtol=0) + # Compute differences + diff = torch.abs(val1 - val2) + max_diff = torch.max(diff).item() + mean_diff = torch.mean(diff).item() - comparison_stats[key] = { - "max_diff": max_diff, - "mean_diff": mean_diff, - "all_close": all_close, - } + # Check if within tolerance + all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) + + comparison_stats[pair_key][key] = { + "max_diff": max_diff, + "mean_diff": mean_diff, + "all_close": all_close, + } return comparison_stats -def print_comparison_results(comparison_stats: dict[str, dict[str, float]], tolerance: float): - """Print comparison results between implementations. +def print_comparison_results(comparison_stats: dict[str, dict[str, dict[str, float]]], tolerance: float): + """Print comparison results across implementations. Args: - comparison_stats: Dictionary containing comparison statistics. + comparison_stats: Nested dictionary containing comparison statistics for each API pair. tolerance: Tolerance used for comparison. """ - # Check if all results match - all_match = all(stats["all_close"] for stats in comparison_stats.values()) - - if all_match: - # Compact output when everything matches - print("\n" + "=" * 100) - print("RESULT COMPARISON: Isaac Lab vs Isaac Sim") - print("=" * 100) - print(f"✓ All computed values match within tolerance ({tolerance})") - print("=" * 100) - print() - else: - # Detailed output when there are mismatches - print("\n" + "=" * 100) - print("RESULT COMPARISON: Isaac Lab vs Isaac Sim") - print("=" * 100) - print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") - print("-" * 100) - - for key, stats in comparison_stats.items(): - # Format the key for display - display_key = key.replace("_", " ").title() - match_str = "✓ Yes" if stats["all_close"] else "✗ No" - - print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") + for pair_key, pair_stats in comparison_stats.items(): + # Format the pair key for display (e.g., "isaaclab_vs_isaacsim" -> "Isaac Lab vs Isaac Sim") + api1, api2 = pair_key.split("_vs_") + display_api1 = api1.replace("-", " ").title() + display_api2 = api2.replace("-", " ").title() + comparison_title = f"{display_api1} vs {display_api2}" + + # Check if all results match + all_match = all(stats["all_close"] for stats in pair_stats.values()) + + if all_match: + # Compact output when everything matches + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {comparison_title}") + print("=" * 100) + print(f"✓ All computed values match within tolerance ({tolerance})") + print("=" * 100) + else: + # Detailed output when there are mismatches + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {comparison_title}") + print("=" * 100) + print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") + print("-" * 100) + + for key, stats in pair_stats.items(): + # Format the key for display + display_key = key.replace("_", " ").title() + match_str = "✓ Yes" if stats["all_close"] else "✗ No" + + print( + f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}" + ) + + print("=" * 100) + print(f"\n✗ Some results differ beyond tolerance ({tolerance})") + print(f" This may indicate implementation differences between {display_api1} and {display_api2}") - print("=" * 100) - print(f"\n✗ Some results differ beyond tolerance ({tolerance})") - print(" This may indicate implementation differences between Isaac Lab and Isaac Sim") - print() + print() -def print_results( - isaaclab_results: dict[str, float], isaacsim_results: dict[str, float], num_prims: int, num_iterations: int -): +def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num_iterations: int): """Print benchmark results in a formatted table. Args: - isaaclab_results: Results from Isaac Lab's XformPrimView benchmark. - isaacsim_results: Results from Isaac Sim's XformPrimView benchmark. + results_dict: Dictionary mapping API names to their timing results. num_prims: Number of prims tested. num_iterations: Number of iterations run. """ @@ -317,8 +339,18 @@ def print_results( print(f"BENCHMARK RESULTS: {num_prims} prims, {num_iterations} iterations") print("=" * 100) + api_names = list(results_dict.keys()) + # Format API names for display + display_names = [name.replace("-", " ").replace("_", " ").title() for name in api_names] + + # Calculate column width based on number of APIs + col_width = 20 + # Print header - print(f"{'Operation':<25} {'Isaac Lab (ms)':<25} {'Isaac Sim (ms)':<25} {'Speedup':<15}") + header = f"{'Operation':<25}" + for display_name in display_names: + header += f" {display_name + ' (ms)':<{col_width}}" + print(header) print("-" * 100) # Print each operation @@ -332,39 +364,74 @@ def print_results( ] for op_name, op_key in operations: - isaaclab_time = isaaclab_results.get(op_key, 0) * 1000 # Convert to ms - isaacsim_time = isaacsim_results.get(op_key, 0) * 1000 # Convert to ms - - if isaaclab_time > 0 and isaacsim_time > 0: - speedup = isaacsim_time / isaaclab_time - print(f"{op_name:<25} {isaaclab_time:>20.4f} {isaacsim_time:>20.4f} {speedup:>10.2f}x") - else: - print(f"{op_name:<25} {isaaclab_time:>20.4f} {'N/A':<20} {'N/A':<15}") + row = f"{op_name:<25}" + for api_name in api_names: + api_time = results_dict[api_name].get(op_key, 0) * 1000 # Convert to ms + row += f" {api_time:>{col_width-1}.4f}" + print(row) print("=" * 100) # Calculate and print total time - if isaaclab_results and isaacsim_results: - total_isaaclab = sum(isaaclab_results.values()) * 1000 - total_isaacsim = sum(isaacsim_results.values()) * 1000 - overall_speedup = total_isaacsim / total_isaaclab if total_isaaclab > 0 else 0 - print(f"\n{'Total Time':<25} {total_isaaclab:>20.4f} {total_isaacsim:>20.4f} {overall_speedup:>10.2f}x") - else: - total_isaaclab = sum(isaaclab_results.values()) * 1000 - print(f"\n{'Total Time':<25} {total_isaaclab:>20.4f} {'N/A':<20} {'N/A':<15}") + total_row = f"{'Total Time':<25}" + for api_name in api_names: + total_time = sum(results_dict[api_name].values()) * 1000 + total_row += f" {total_time:>{col_width-1}.4f}" + print(f"\n{total_row}") + + # Calculate speedups relative to Isaac Lab + if "isaaclab" in api_names: + print("\n" + "=" * 100) + print("SPEEDUP vs Isaac Lab") + print("=" * 100) + print(f"{'Operation':<25}", end="") + for display_name in display_names: + if "isaaclab" not in display_name.lower(): + print(f" {display_name + ' Speedup':<{col_width}}", end="") + print() + print("-" * 100) + + isaaclab_results = results_dict["isaaclab"] + for op_name, op_key in operations: + print(f"{op_name:<25}", end="") + isaaclab_time = isaaclab_results.get(op_key, 0) + for api_name, display_name in zip(api_names, display_names): + if api_name != "isaaclab": + api_time = results_dict[api_name].get(op_key, 0) + if isaaclab_time > 0 and api_time > 0: + speedup = api_time / isaaclab_time + print(f" {speedup:>{col_width-1}.2f}x", end="") + else: + print(f" {'N/A':>{col_width}}", end="") + print() + + # Overall speedup + print("=" * 100) + print(f"{'Overall Speedup':<25}", end="") + total_isaaclab = sum(isaaclab_results.values()) + for api_name, display_name in zip(api_names, display_names): + if api_name != "isaaclab": + total_api = sum(results_dict[api_name].values()) + if total_isaaclab > 0 and total_api > 0: + overall_speedup = total_api / total_isaaclab + print(f" {overall_speedup:>{col_width-1}.2f}x", end="") + else: + print(f" {'N/A':>{col_width}}", end="") + print() + print("\n" + "=" * 100) print("\nNotes:") print(" - Times are averaged over all iterations") - print(" - Speedup = (Isaac Sim time) / (Isaac Lab time)") + print(" - Speedup = (Other API time) / (Isaac Lab time)") print(" - Speedup > 1.0 means Isaac Lab is faster") - print(" - Speedup < 1.0 means Isaac Sim is faster") + print(" - Speedup < 1.0 means the other API is faster") print() def main(): """Main benchmark function.""" print("=" * 100) - print("XformPrimView Benchmark") + print("XformPrimView Benchmark - Comparing Multiple APIs") print("=" * 100) print("Configuration:") print(f" Number of environments: {args_cli.num_envs}") @@ -381,50 +448,51 @@ def main(): os.makedirs(args_cli.profile_dir, exist_ok=True) - # Benchmark Isaac Lab XformPrimView - print("Benchmarking XformPrimView from Isaac Lab...") - if args_cli.profile: - profiler_isaaclab = cProfile.Profile() - profiler_isaaclab.enable() + # Dictionary to store all results + all_timing_results = {} + all_computed_results = {} + profile_files = {} - isaaclab_timing, isaaclab_computed = benchmark_xform_prim_view( - api="isaaclab", num_iterations=args_cli.num_iterations - ) + # APIs to benchmark + apis_to_test = [ + ("isaaclab", "Isaac Lab XformPrimView"), + ("isaacsim", "Isaac Sim XformPrimView (Legacy)"), + ("isaacsim-exp", "Isaac Sim Experimental XformPrim"), + ] - if args_cli.profile: - profiler_isaaclab.disable() - profile_file_isaaclab = f"{args_cli.profile_dir}/isaaclab_XformPrimView.prof" - profiler_isaaclab.dump_stats(profile_file_isaaclab) - print(f" Profile saved to: {profile_file_isaaclab}") + # Benchmark each API + for api_key, api_name in apis_to_test: + print(f"Benchmarking {api_name}...") - print(" Done!") - print() + if args_cli.profile: + profiler = cProfile.Profile() + profiler.enable() - # Benchmark Isaac Sim XformPrimView - print("Benchmarking Isaac Sim XformPrimView...") - if args_cli.profile: - profiler_isaacsim = cProfile.Profile() - profiler_isaacsim.enable() + # Cast api_key to Literal type for type checker + timing, computed = benchmark_xform_prim_view( + api=api_key, # type: ignore[arg-type] + num_iterations=args_cli.num_iterations, + ) - isaacsim_timing, isaacsim_computed = benchmark_xform_prim_view( - api="isaacsim", num_iterations=args_cli.num_iterations - ) + if args_cli.profile: + profiler.disable() + profile_file = f"{args_cli.profile_dir}/{api_key.replace('-', '_')}_benchmark.prof" + profiler.dump_stats(profile_file) + profile_files[api_key] = profile_file + print(f" Profile saved to: {profile_file}") - if args_cli.profile: - profiler_isaacsim.disable() - profile_file_isaacsim = f"{args_cli.profile_dir}/isaacsim_XformPrimView.prof" - profiler_isaacsim.dump_stats(profile_file_isaacsim) - print(f" Profile saved to: {profile_file_isaacsim}") + all_timing_results[api_key] = timing + all_computed_results[api_key] = computed - print(" Done!") - print() + print(" Done!") + print() # Print timing results - print_results(isaaclab_timing, isaacsim_timing, args_cli.num_envs, args_cli.num_iterations) + print_results(all_timing_results, args_cli.num_envs, args_cli.num_iterations) # Compare computed results - print("\nComparing computed results...") - comparison_stats = compare_results(isaaclab_computed, isaacsim_computed, tolerance=1e-6) + print("\nComparing computed results across APIs...") + comparison_stats = compare_results(all_computed_results, tolerance=1e-6) print_comparison_results(comparison_stats, tolerance=1e-4) # Print profiling instructions if enabled @@ -433,10 +501,12 @@ def main(): print("PROFILING RESULTS") print("=" * 100) print("Profile files have been saved. To visualize with snakeviz, run:") - print(f" snakeviz {profile_file_isaaclab}") - print(f" snakeviz {profile_file_isaacsim}") + for api_key, profile_file in profile_files.items(): + api_display = api_key.replace("-", " ").title() + print(f" # {api_display}") + print(f" snakeviz {profile_file}") print("\nAlternatively, use pstats to analyze in terminal:") - print(f" python -m pstats {profile_file_isaaclab}") + print(" python -m pstats ") print("=" * 100) print() From 8527b3b4ce5393d191a2809111b34a63a12d09b3 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 18:23:02 +0100 Subject: [PATCH 043/100] uses xform cache --- .../isaaclab/sim/views/xform_prim_view.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 699a89ec2a3..174c2bc95c5 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -183,6 +183,9 @@ def set_world_poses( else: orientations_array = None + # Create xform cache instance + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + # Set poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): @@ -201,7 +204,7 @@ def set_world_poses( # Get current world pose if we're only setting one component if positions_array is None or orientations_array is None: # get prim xform - prim_tf = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + prim_tf = xform_cache.GetLocalToWorldTransform(prim) # sanitize quaternion # this is needed, otherwise the quaternion might be non-normalized prim_tf.Orthonormalize() @@ -217,9 +220,7 @@ def set_world_poses( prim_tf.SetRotateOnly(world_quat) # Convert to local space - parent_world_tf = UsdGeom.Xformable(parent_prim).ComputeLocalToWorldTransform( - Usd.TimeCode.Default() - ) + parent_world_tf = xform_cache.GetLocalToWorldTransform(parent_prim) local_tf = prim_tf * parent_world_tf.GetInverse() local_pos = local_tf.ExtractTranslation() local_quat = local_tf.ExtractRotationQuat() @@ -357,12 +358,14 @@ def get_world_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te # Create buffers positions = Vt.Vec3dArray(len(indices_list)) orientations = Vt.QuatdArray(len(indices_list)) + # Create xform cache instance + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) for idx in indices_list: # Get prim prim = self._prims[idx] # get prim xform - prim_tf = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + prim_tf = xform_cache.GetLocalToWorldTransform(prim) # sanitize quaternion # this is needed, otherwise the quaternion might be non-normalized prim_tf.Orthonormalize() @@ -403,12 +406,14 @@ def get_local_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te # Create buffers translations = Vt.Vec3dArray(len(indices_list)) orientations = Vt.QuatdArray(len(indices_list)) + # Create xform cache instance + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) for idx in indices_list: # Get prim prim = self._prims[idx] # get prim xform - prim_tf = UsdGeom.Xformable(prim).GetLocalTransformation(Usd.TimeCode.Default()) + prim_tf = xform_cache.GetLocalTransformation(prim)[0] # sanitize quaternion # this is needed, otherwise the quaternion might be non-normalized prim_tf.Orthonormalize() From 6834e09fefe5a0edbe88b6b5e3327402a7b644b9 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 18:31:32 +0100 Subject: [PATCH 044/100] runs formatter --- scripts/benchmarks/benchmark_xform_prim_view.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 416f8c5c95e..8a2791b9d95 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -316,9 +316,7 @@ def print_comparison_results(comparison_stats: dict[str, dict[str, dict[str, flo display_key = key.replace("_", " ").title() match_str = "✓ Yes" if stats["all_close"] else "✗ No" - print( - f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}" - ) + print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") print("=" * 100) print(f"\n✗ Some results differ beyond tolerance ({tolerance})") From 9faf081115cd585b926dc19256ae3167ed2cc990 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 18:36:49 +0100 Subject: [PATCH 045/100] adds note for why we don't use the utils functions --- source/isaaclab/isaaclab/sim/views/xform_prim_view.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 174c2bc95c5..134b702c495 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -200,6 +200,8 @@ def set_world_poses( world_quat = orientations_array[idx] if orientations_array is not None else None # Convert world pose to local if we have a valid parent + # Note: We don't use :func:`isaaclab.sim.utils.transforms.convert_world_pose_to_local` + # here since it isn't optimized for batch operations. if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: # Get current world pose if we're only setting one component if positions_array is None or orientations_array is None: @@ -361,6 +363,8 @@ def get_world_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te # Create xform cache instance xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + # Note: We don't use :func:`isaaclab.sim.utils.transforms.resolve_prim_pose` + # here since it isn't optimized for batch operations. for idx in indices_list: # Get prim prim = self._prims[idx] @@ -409,6 +413,8 @@ def get_local_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te # Create xform cache instance xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + # Note: We don't use :func:`isaaclab.sim.utils.transforms.resolve_prim_pose` + # here since it isn't optimized for batch operations. for idx in indices_list: # Get prim prim = self._prims[idx] From dad21afec289e013e2dcf265a9499d8d0ff07b3b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 18:45:57 +0100 Subject: [PATCH 046/100] fixes typehints --- source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py | 2 +- .../isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py | 4 ++-- .../isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py b/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py index 511a660b3d9..94b402d41a3 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/__init__.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Sub-module for rigid contact sensor based on :class:`isaacsim.core.prims.RigidContactView`.""" +"""Sub-module for rigid contact sensor.""" from .contact_sensor import ContactSensor from .contact_sensor_cfg import ContactSensorCfg diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py index b60722f9587..ae97386f3ce 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py @@ -15,9 +15,9 @@ import omni.physics.tensors.impl.api as physx import warp as wp -from isaacsim.core.prims import XFormPrim import isaaclab.sim as sim_utils +from isaaclab.sim.views import XformPrimView from isaaclab.utils.math import matrix_from_quat, quat_mul from isaaclab.utils.mesh import PRIMITIVE_MESH_TYPES, create_trimesh_from_geom_mesh, create_trimesh_from_geom_shape from isaaclab.utils.warp import convert_to_warp_mesh, raycast_dynamic_meshes @@ -78,7 +78,7 @@ class MultiMeshRayCaster(RayCaster): mesh_offsets: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} - mesh_views: ClassVar[dict[str, XFormPrim | physx.ArticulationView | physx.RigidBodyView]] = {} + mesh_views: ClassVar[dict[str, XformPrimView | physx.ArticulationView | physx.RigidBodyView]] = {} """A dictionary to store mesh views for raycasting, shared across all instances. The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py index a5a62fea183..543276e8ea2 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py @@ -10,13 +10,13 @@ import torch import omni.physics.tensors.impl.api as physx -from isaacsim.core.prims import XFormPrim +from isaaclab.sim.views import XformPrimView from isaaclab.utils.math import convert_quat def obtain_world_pose_from_view( - physx_view: XFormPrim | physx.ArticulationView | physx.RigidBodyView, + physx_view: XformPrimView | physx.ArticulationView | physx.RigidBodyView, env_ids: torch.Tensor, clone: bool = False, ) -> tuple[torch.Tensor, torch.Tensor]: @@ -34,7 +34,7 @@ def obtain_world_pose_from_view( Raises: NotImplementedError: If the prim view is not of the supported type. """ - if isinstance(physx_view, XFormPrim): + if isinstance(physx_view, XformPrimView): pos_w, quat_w = physx_view.get_world_poses(env_ids) elif isinstance(physx_view, physx.ArticulationView): pos_w, quat_w = physx_view.get_root_transforms()[env_ids].split([3, 4], dim=-1) From 67faf0a82e0dba67f37d7e032b876837f67c3ed5 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 19:05:08 +0100 Subject: [PATCH 047/100] use inplace operation for ortho --- source/isaaclab/isaaclab/sim/utils/transforms.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/transforms.py b/source/isaaclab/isaaclab/sim/utils/transforms.py index 65aecaee951..b7da662fd77 100644 --- a/source/isaaclab/isaaclab/sim/utils/transforms.py +++ b/source/isaaclab/isaaclab/sim/utils/transforms.py @@ -300,19 +300,16 @@ def resolve_prim_pose( prim_tf = xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) # sanitize quaternion # this is needed, otherwise the quaternion might be non-normalized - prim_tf = prim_tf.GetOrthonormalized() + prim_tf.Orthonormalize() if ref_prim is not None: - # check if ref prim is valid - if not ref_prim.IsValid(): - raise ValueError(f"Ref prim at path '{ref_prim.GetPath().pathString}' is not valid.") # if reference prim is the root, we can skip the computation if ref_prim.GetPath() != Sdf.Path.absoluteRootPath: # get ref prim xform ref_xform = UsdGeom.Xformable(ref_prim) ref_tf = ref_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) # make sure ref tf is orthonormal - ref_tf = ref_tf.GetOrthonormalized() + ref_tf.Orthonormalize() # compute relative transform to get prim in ref frame prim_tf = prim_tf * ref_tf.GetInverse() From 2bc8c1ef9dbd8a81aa3893124ffd361f6616c1a7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 19:15:14 +0100 Subject: [PATCH 048/100] ensures stage is cached for deletion --- source/isaaclab/isaaclab/sim/utils/prims.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index feef485414a..020aa5d4d6f 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -21,7 +21,7 @@ import usdrt # noqa: F401 from isaacsim.core.cloner import Cloner from omni.usd.commands import DeletePrimsCommand, MovePrimCommand -from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade, UsdUtils from isaaclab.utils.string import to_camel_case from isaaclab.utils.version import get_isaac_sim_version @@ -197,6 +197,12 @@ def delete_prim(prim_path: str | Sequence[str], stage: Usd.Stage | None = None) prim_path = [prim_path] # get stage handle stage = get_current_stage() if stage is None else stage + # the prim command looks for the stage ID in the stage cache + # so we need to ensure the stage is cached + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(stage).ToLongInt() + if stage_id < 0: + stage_id = stage_cache.Insert(stage).ToLongInt() # delete prims DeletePrimsCommand(prim_path, stage=stage).do() From ac1619b4a43c90a0d087b26ee2d94a1df06ea461 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 22:46:57 +0100 Subject: [PATCH 049/100] updates changelog --- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 66246612534..81047f7cadb 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.51.1" +version = "0.52.0" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index ae136334a93..09aa6d708e7 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,23 @@ Changelog --------- +0.52.0 (2026-01-02) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :mod:`~isaaclab.sim.utils.transforms` module to handle USD Xform operations. +* Added passing of ``stage`` to the :func:`~isaaclab.sim.utils.prims.create_prim` function + inside spawning functions to allow for the creation of prims in a specific stage. + +Changed +^^^^^^^ + +* Changed :func:`~isaaclab.sim.utils.prims.create_prim` function to use the :mod:`~isaaclab.sim.utils.transforms` + module for USD Xform operations. It removes the usage of Isaac Sim's XformPrim class. + + 0.51.2 (2025-12-30) ~~~~~~~~~~~~~~~~~~~ From 28f134844da1e9c32026b2750299673eb23f0bf7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 22:53:42 +0100 Subject: [PATCH 050/100] removes exe from top --- scripts/benchmarks/benchmark_xform_prim_view.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 8a2791b9d95..694bd359bf2 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -1,15 +1,8 @@ -#!/usr/bin/env python3 - # 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 -# Copyright (c) 2022-2026, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - """Benchmark script comparing XformPrimView implementations across different APIs. This script tests the performance of batched transform operations using: From 582d2818d2dc1d82ef53f9b60cf0ac67cb6f2b3d Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 22:56:22 +0100 Subject: [PATCH 051/100] fixes for extras --- source/isaaclab/isaaclab/scene/interactive_scene.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 3a884f3e6be..33abcaed7f9 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -409,8 +409,8 @@ def surface_grippers(self) -> dict[str, SurfaceGripper]: def extras(self) -> dict[str, XformPrimView]: """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. - The keys are the names of the miscellaneous objects, and the values are the :class:`~isaaclab.sim.utils.views.XformPrimView` - of the corresponding prims. + The keys are the names of the miscellaneous objects, and the values are the + :class:`~isaaclab.sim.views.XformPrimView` instances of the corresponding prims. As an example, lights or other props in the scene that do not have any attributes or properties that you want to alter at runtime can be added to this dictionary. @@ -777,7 +777,7 @@ def _add_entities_from_cfg(self): ) # store xform prim view corresponding to this asset # all prims in the scene are Xform prims (i.e. have a transform component) - self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, reset_xform_properties=False) + self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, device=self.device, stage=self.stage) else: raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") # store global collision paths From 1b3f82a60a04e08624954bd5da116e7614d53f87 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Fri, 2 Jan 2026 23:06:05 +0100 Subject: [PATCH 052/100] hopes --- scripts/benchmarks/benchmark_xform_prim_view.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 694bd359bf2..e4fd4c95d5b 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -358,7 +358,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num row = f"{op_name:<25}" for api_name in api_names: api_time = results_dict[api_name].get(op_key, 0) * 1000 # Convert to ms - row += f" {api_time:>{col_width-1}.4f}" + row += f" {api_time:>{col_width - 1}.4f}" print(row) print("=" * 100) @@ -367,7 +367,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num total_row = f"{'Total Time':<25}" for api_name in api_names: total_time = sum(results_dict[api_name].values()) * 1000 - total_row += f" {total_time:>{col_width-1}.4f}" + total_row += f" {total_time:>{col_width - 1}.4f}" print(f"\n{total_row}") # Calculate speedups relative to Isaac Lab @@ -391,7 +391,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num api_time = results_dict[api_name].get(op_key, 0) if isaaclab_time > 0 and api_time > 0: speedup = api_time / isaaclab_time - print(f" {speedup:>{col_width-1}.2f}x", end="") + print(f" {speedup:>{col_width - 1}.2f}x", end="") else: print(f" {'N/A':>{col_width}}", end="") print() @@ -405,7 +405,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num total_api = sum(results_dict[api_name].values()) if total_isaaclab > 0 and total_api > 0: overall_speedup = total_api / total_isaaclab - print(f" {overall_speedup:>{col_width-1}.2f}x", end="") + print(f" {overall_speedup:>{col_width - 1}.2f}x", end="") else: print(f" {'N/A':>{col_width}}", end="") print() From 34a07704b467f57fe6246ede9830f38794d261e0 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 03:34:57 +0100 Subject: [PATCH 053/100] moves to follow ordering --- .../isaaclab/isaaclab/sim/simulation_cfg.py | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index 9f74b5ba016..ac19ca22b32 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -43,6 +43,26 @@ class PhysxCfg: * :obj:`1`: TGS (Temporal Gauss-Seidel) """ + solve_articulation_contact_last: bool = False + """Changes the ordering inside the articulation solver. Default is False. + + PhysX employs a strict ordering for handling constraints in an articulation. The outcome of + each constraint resolution modifies the joint and associated link speeds. However, the default + ordering may not be ideal for gripping scenarios because the solver favours the constraint + types that are resolved last. This is particularly true of stiff constraint systems that are hard + to resolve without resorting to vanishingly small simulation timesteps. + + With dynamic contact resolution being such an important part of gripping, it may make + more sense to solve dynamic contact towards the end of the solver rather than at the + beginning. This parameter modifies the default ordering to enable this change. + + For more information, please check `here `__. + + .. versionadded:: v2.3 + This parameter is only available with Isaac Sim 5.1. + + """ + min_position_iteration_count: int = 1 """Minimum number of solver position iterations (rigid bodies, cloth, particles etc.). Default is 1. @@ -173,26 +193,6 @@ class PhysxCfg: gpu_max_particle_contacts: int = 2**20 """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" - solve_articulation_contact_last: bool = False - """Changes the ordering inside the articulation solver. Default is False. - - PhysX employs a strict ordering for handling constraints in an articulation. The outcome of - each constraint resolution modifies the joint and associated link speeds. However, the default - ordering may not be ideal for gripping scenarios because the solver favours the constraint - types that are resolved last. This is particularly true of stiff constraint systems that are hard - to resolve without resorting to vanishingly small simulation timesteps. - - With dynamic contact resolution being such an important part of gripping, it may make - more sense to solve dynamic contact towards the end of the solver rather than at the - beginning. This parameter modifies the default ordering to enable this change. - - For more information, please check `here `__. - - .. versionadded:: v2.3 - This parameter is only available with Isaac Sim 5.1. - - """ - @configclass class RenderCfg: From bf1083733e806d7d5eaec77df952c537c6cb245e Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 03:36:59 +0100 Subject: [PATCH 054/100] removes unused clear callbacks --- .../isaaclab/isaaclab/envs/direct_marl_env.py | 1 - .../isaaclab/isaaclab/envs/direct_rl_env.py | 1 - .../isaaclab/envs/manager_based_env.py | 1 - .../isaaclab/sim/simulation_context.py | 581 ++++++++++++------ 4 files changed, 387 insertions(+), 197 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 97fb3ae3da5..a37112a28bb 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -549,7 +549,6 @@ def close(self): self.sim.stop() self.sim.clear() - self.sim.clear_all_callbacks() self.sim.clear_instance() # destroy the window diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 22b8f3217c5..728edc73247 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -518,7 +518,6 @@ def close(self): self.sim.stop() self.sim.clear() - self.sim.clear_all_callbacks() self.sim.clear_instance() # destroy the window diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 564668d55f3..8626dd7587a 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -536,7 +536,6 @@ def close(self): self.sim.stop() self.sim.clear() - self.sim.clear_all_callbacks() self.sim.clear_instance() # destroy the window diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 9b23b29708c..c6eee377923 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -3,6 +3,8 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import builtins import enum import glob @@ -18,16 +20,18 @@ from collections.abc import Iterator from contextlib import contextmanager from datetime import datetime -from typing import Any +from typing import Any, ClassVar import carb import flatdict +import omni.kit.app +import omni.physics.tensors import omni.physx +import omni.timeline import omni.usd -from isaacsim.core.api.simulation_context import SimulationContext as _SimulationContext -from isaacsim.core.simulation_manager import SimulationManager +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager from isaacsim.core.utils.viewports import set_camera_view -from pxr import Gf, PhysxSchema, Sdf, Usd, UsdPhysics +from pxr import Gf, PhysxSchema, Usd, UsdGeom, UsdPhysics, UsdUtils import isaaclab.sim as sim_utils from isaaclab.utils.logger import configure_logging @@ -35,13 +39,12 @@ from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg -from .utils import bind_physics_material # import logger logger = logging.getLogger(__name__) -class SimulationContext(_SimulationContext): +class SimulationContext: """A class to control simulation-related events such as physics stepping and rendering. The simulation context helps control various simulation aspects. This includes: @@ -82,6 +85,12 @@ class SimulationContext(_SimulationContext): the non-``_async`` functions are used in the standalone python script mode. """ + _instance: SimulationContext | None = None + """The singleton instance of the simulation context.""" + + _is_initialized: ClassVar[bool] = False + """Whether the simulation context is initialized.""" + class RenderMode(enum.IntEnum): """Different rendering modes for the simulation. @@ -124,15 +133,18 @@ def __init__(self, cfg: SimulationCfg | None = None): cfg: The configuration of the simulation. Defaults to None, in which case the default configuration is used. """ + # skip if already initialized + if SimulationContext._is_initialized: + return + SimulationContext._is_initialized = True + # store input if cfg is None: cfg = SimulationCfg() # check that the config is valid - cfg.validate() + cfg.validate() # type: ignore + # store configuration self.cfg = cfg - # check that simulation is running - if sim_utils.get_current_stage() is None: - raise RuntimeError("The stage has not been created. Did you run the simulator?") # setup logger self.logger = configure_logging( @@ -146,13 +158,19 @@ def __init__(self, cfg: SimulationCfg | None = None): self._initial_stage = sim_utils.create_new_stage_in_memory() else: self._initial_stage = omni.usd.get_context().get_stage() + # add stage to USD cache + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(self._initial_stage).ToLongInt() + if stage_id < 0: + stage_id = stage_cache.Insert(self._initial_stage).ToLongInt() + self._initial_stage_id = stage_id + # check that stage is created + if self._initial_stage is None: + raise RuntimeError("Failed to create a new stage. Please check if the USD context is valid.") # acquire settings interface self.carb_settings = carb.settings.get_settings() - # apply carb physics settings - self._apply_physics_settings() - # note: we read this once since it is not expected to change during runtime # read flag for whether a local GUI is enabled self._local_gui = self.carb_settings.get("/app/window/enabled") @@ -222,11 +240,6 @@ def __init__(self, cfg: SimulationCfg | None = None): # this is needed for some GUI features if self._has_gui: self.cfg.enable_scene_query_support = True - # set up flatcache/fabric interface (default is None) - # this is needed to flush the flatcache data into Hydra manually when calling `render()` - # ref: https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_physics.html - # note: need to do this here because super().__init__ calls render and this variable is needed - self._fabric_iface = None # create a tensor for gravity # note: this line is needed to create a "tensor" in the device to avoid issues with torch 2.1 onwards. @@ -250,62 +263,99 @@ def __init__(self, cfg: SimulationCfg | None = None): self._app_control_on_stop_handle = None self._disable_app_control_on_stop_handle = False - # flatten out the simulation dictionary - sim_params = self.cfg.to_dict() - if sim_params is not None: - if "physx" in sim_params: - physx_params = sim_params.pop("physx") - sim_params.update(physx_params) + # obtain interfaces for simulation + self._app_iface = omni.kit.app.get_app_interface() + self._framework = carb.get_framework() + self._physx_iface = omni.physx.get_physx_interface() + self._physx_sim_iface = omni.physx.get_physx_simulation_interface() + self._timeline_iface = omni.timeline.get_timeline_interface() + + # set stage properties + with sim_utils.use_stage(self._initial_stage): + # correct conventions for metric units + UsdGeom.SetStageUpAxis(self._initial_stage, "Z") + UsdGeom.SetStageMetersPerUnit(self._initial_stage, 1.0) + UsdPhysics.SetStageKilogramsPerUnit(self._initial_stage, 1.0) + + # find if any physics prim already exists and delete it + for prim in self._initial_stage.Traverse(): + if prim.HasAPI(PhysxSchema.PhysxSceneAPI): + sim_utils.delete_prim(prim.GetPath().pathString) + # create a new physics scene + if self._initial_stage.GetPrimAtPath(self.cfg.physics_prim_path).IsValid(): + raise RuntimeError(f"A prim already exists at path '{self.cfg.physics_prim_path}'.") + self._physics_scene = UsdPhysics.Scene.Define(self._initial_stage, self.cfg.physics_prim_path) + self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene) + + # set time codes per second + self._configure_simulation_dt() + # apply physics settings (carb and physx) + self._apply_physics_settings() - # add warning about enabling stabilization for large step sizes - if not self.cfg.physx.enable_stabilization and (self.cfg.dt > 0.0333): - self.logger.warning( - "Large simulation step size (> 0.0333 seconds) is not recommended without enabling stabilization." - " Consider setting the `enable_stabilization` flag to True in the PhysxCfg, or reducing the" - " simulation step size if you run into physics issues." - ) + # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes + # when in headless mode + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) + # load flatcache/fabric interface + self._load_fabric_interface() - # set simulation device - # note: Although Isaac Sim sets the physics device in the init function, - # it does a render call which gets the wrong device. - SimulationManager.set_physics_sim_device(self.cfg.device) - - # obtain the parsed device - # This device should be the same as "self.cfg.device". However, for cases, where users specify the device - # as "cuda" and not "cuda:X", then it fetches the current device from SimulationManager. - # Note: Since we fix the device from the configuration and don't expect users to change it at runtime, - # we can obtain the device once from the SimulationManager.get_physics_sim_device() function. - # This reduces the overhead of calling the function. - self._physics_device = SimulationManager.get_physics_sim_device() - - # create a simulation context to control the simulator - if get_isaac_sim_version().major < 5: - # stage arg is not supported before isaac sim 5.0 - super().__init__( - stage_units_in_meters=1.0, - physics_dt=self.cfg.dt, - rendering_dt=self.cfg.dt * self.cfg.render_interval, - backend="torch", - sim_params=sim_params, - physics_prim_path=self.cfg.physics_prim_path, - device=self.cfg.device, - ) + def __new__(cls, *args, **kwargs) -> SimulationContext: + """Returns the instance of the simulation context. + + This function is used to create a singleton instance of the simulation context. + If the instance already exists, it returns the previously defined one. Otherwise, + it creates a new instance and returns it + """ + if cls._instance is None: + cls._instance = super().__new__(cls) else: - super().__init__( - stage_units_in_meters=1.0, - physics_dt=self.cfg.dt, - rendering_dt=self.cfg.dt * self.cfg.render_interval, - backend="torch", - sim_params=sim_params, - physics_prim_path=self.cfg.physics_prim_path, - device=self.cfg.device, - stage=self._initial_stage, + logger.info("Returning the previously defined instance of Simulation Context.") + return cls._instance # type: ignore + + """ + Operations - Singleton. + """ + + @classmethod + def instance(cls) -> SimulationContext: + """Returns the instance of the simulation context. + + Returns: + SimulationContext: The instance of the simulation context. + """ + if cls._instance is None: + raise RuntimeError( + "Simulation context is not initialized. Please create a new instance using the constructor." ) + return cls._instance + + @classmethod + def clear_instance(cls) -> None: + """Delete the simulation context singleton instance.""" + if cls._instance is not None: + # clear the callback + if cls._instance._app_control_on_stop_handle is not None: + cls._instance._app_control_on_stop_handle.unsubscribe() + cls._instance._app_control_on_stop_handle = None + # clear the instance and the flag + cls._instance = None + cls._is_initialized = False """ - Properties - Override. + Properties. """ + @property + def app(self) -> omni.kit.app.IApp: + """Omniverse Kit Application interface.""" + return self._app_iface + + @property + def stage(self) -> Usd.Stage: + """USD Stage.""" + return self._initial_stage + @property def device(self) -> str: """Device used by the simulation. @@ -316,8 +366,13 @@ def device(self) -> str: """ return self._physics_device + @property + def physics_sim_view(self) -> omni.physics.tensors.SimulationView: + """Physics simulation view with torch backend.""" + return self._physics_sim_view + """ - Operations - New. + Operations - Simulation Information. """ def has_gui(self) -> bool: @@ -379,8 +434,16 @@ def get_version(self) -> tuple[int, int, int]: """ return get_isaac_sim_version().major, get_isaac_sim_version().minor, get_isaac_sim_version().micro + def get_initial_stage(self) -> Usd.Stage: + """Returns stage handle used during scene creation. + + Returns: + The stage used during scene creation. + """ + return self._initial_stage + """ - Operations - New utilities. + Operations - Utilities. """ def set_camera_view( @@ -416,8 +479,8 @@ def set_render_mode(self, mode: RenderMode): change the render mode. Args: - mode (RenderMode): The rendering mode. If different than SimulationContext's rendering mode, - SimulationContext's mode is changed to the new mode. + mode: The rendering mode. If different than SimulationContext's rendering mode, + SimulationContext's mode is changed to the new mode. Raises: ValueError: If the input mode is not supported. @@ -491,37 +554,120 @@ def get_setting(self, name: str) -> Any: """ return self.carb_settings.get(name) - def get_initial_stage(self) -> Usd.Stage: - """Returns stage handle used during scene creation. + """ + Operations- Timeline. + """ + + def is_playing(self) -> bool: + """Check whether the simulation is playing. Returns: - The stage used during scene creation. + True if the simulator is playing. """ - return self._initial_stage + return self._timeline.is_playing() + + def is_stopped(self) -> bool: + """Check whether the simulation is stopped. + + Returns: + True if the simulator is stopped. + """ + return self._timeline.is_stopped() + + def play(self) -> None: + """Start playing the simulation.""" + # play the simulation + self._timeline.play() + self._timeline.commit() + # perform one step to propagate all physics handles properly + if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) + + def pause(self) -> None: + """Pause the simulation.""" + # pause the simulation + self._timeline.pause() + # set the play simulations setting + if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) + + def stop(self) -> None: + """Stop the simulation. + + Note: + Stopping the simulation will lead to the simulation state being lost. + """ + # stop the simulation + self._timeline.stop() + # set the play simulations setting + if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) """ Operations - Override (standalone) """ def reset(self, soft: bool = False): + """Reset the simulation. + + .. warning:: + + This method is not intended to be used in the Isaac Sim's Extensions workflow since the Kit application + has the control over the rendering steps. For the Extensions workflow use the ``reset_async`` method instead + + Args: + soft (bool, optional): if set to True simulation won't be stopped and start again. It only calls the reset on the scene objects. + + """ + # disable simulation stopping control so that we can crash the program + # if an exception is raised in a callback during initialization. self._disable_app_control_on_stop_handle = True # check if we need to raise an exception that was raised in a callback if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION builtins.ISAACLAB_CALLBACK_EXCEPTION = None raise exception_to_raise - super().reset(soft=soft) + + # reset the simulation + if not soft: + if not self.is_stopped(): + self.stop() + self.play() + # initialize the physics simulation + self._physx_iface.force_load_physics_from_usd() + self._physx_iface.start_simulation() + self._physx_iface.update_simulation(self.cfg.dt, 0.0) + self._physx_sim_iface.fetch_results() + SimulationManager._message_bus.dispatch_event(IsaacEvents.PHYSICS_WARMUP.value, payload={}) + # app.update() may be changing the cuda device in reset, so we force it back to our desired device here if "cuda" in self.device: torch.cuda.set_device(self.device) + + # create the simulation view + self._physics_sim_view = omni.physics.tensors.create_simulation_view("torch", stage_id=self._initial_stage_id) + self._physics_sim_view.set_subspace_roots("/") + self._physx_iface.update_simulation(self.cfg.dt, 0.0) + SimulationManager._message_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) + SimulationManager._simulation_view_created = True + SimulationManager._physics_sim_view = self._physics_sim_view + SimulationManager._message_bus.dispatch_event(IsaacEvents.PHYSICS_READY.value, payload={}) + # enable kinematic rendering with fabric - if self.physics_sim_view: - self.physics_sim_view._backend.initialize_kinematic_bodies() + if self._physics_sim_view: + self._physics_sim_view._backend.initialize_kinematic_bodies() # perform additional rendering steps to warm up replicator buffers # this is only needed for the first time we set the simulation if not soft: for _ in range(2): self.render() + # re-enable simulation stopping control self._disable_app_control_on_stop_handle = False def forward(self) -> None: @@ -570,7 +716,11 @@ def step(self, render: bool = True): self.app.update() # step the simulation - super().step(render=render) + if render: + self._app_iface.update() + else: + self._physics_sim_view.simulate(self.cfg.dt, 0.0) + self._physics_sim_view.fetch_results() # app.update() may be changing the cuda device in step, so we force it back to our desired device here if "cuda" in self.device: @@ -606,7 +756,6 @@ def render(self, mode: RenderMode | None = None): if self._render_throttle_counter % self._render_throttle_period == 0: self._render_throttle_counter = 0 # here we don't render viewport so don't need to flush fabric data - # note: we don't call super().render() anymore because they do flush the fabric data self.set_setting("/app/player/playSimulations", False) self._app.update() self.set_setting("/app/player/playSimulations", True) @@ -640,61 +789,58 @@ def _predicate(prim: Usd.Prim) -> bool: sim_utils.clear_stage(predicate=_predicate) - """ - Operations - Override (extension) - """ - - async def reset_async(self, soft: bool = False): - # need to load all "physics" information from the USD file - if not soft: - omni.physx.acquire_physx_interface().force_load_physics_from_usd() - # play the simulation - await super().reset_async(soft=soft) - - """ - Initialization/Destruction - Override. - """ - - def _init_stage(self, *args, **kwargs) -> Usd.Stage: - _ = super()._init_stage(*args, **kwargs) - with sim_utils.use_stage(self.get_initial_stage()): - # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes - # when in headless mode - self.set_setting("/app/player/playSimulations", False) - self._app.update() - self.set_setting("/app/player/playSimulations", True) - # set additional physx parameters and bind material - self._set_additional_physx_params() - # load flatcache/fabric interface - self._load_fabric_interface() - # return the stage - return self.stage - - async def _initialize_stage_async(self, *args, **kwargs) -> Usd.Stage: - await super()._initialize_stage_async(*args, **kwargs) - # set additional physx parameters and bind material - self._set_additional_physx_params() - # load flatcache/fabric interface - self._load_fabric_interface() - # return the stage - return self.stage - - @classmethod - def clear_instance(cls): - # clear the callback - if cls._instance is not None: - if cls._instance._app_control_on_stop_handle is not None: - cls._instance._app_control_on_stop_handle.unsubscribe() - cls._instance._app_control_on_stop_handle = None - # call parent to clear the instance - super().clear_instance() - """ Helper Functions """ + def _configure_simulation_dt(self): + """Configures the simulation step size based on the physics and rendering step sizes.""" + # if rendering is called the substeps term is used to determine + # how many physics steps to perform per rendering step. + # it is not used if step(render=False). + render_interval = max(self.cfg.render_interval, 1) + + # set simulation step per second + steps_per_second = int(1.0 / self.cfg.dt) + self._physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) + # set minimum number of steps per frame + min_steps = int(steps_per_second / render_interval) + self.carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) + + # compute rendering frequency + rendering_hz = int(1.0 / (self.cfg.dt * render_interval)) + + # If rate limiting is enabled, set the rendering rate to the specified value + # Otherwise run the app as fast as possible and do not specify the target rate + if self.carb_settings.get("/app/runLoops/main/rateLimitEnabled"): + self.carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) + with Usd.EditContext(self._initial_stage, self._initial_stage.GetRootLayer()): + self._initial_stage.SetTimeCodesPerSecond(rendering_hz) + self._timeline_iface.set_time_codes_per_second(rendering_hz) + # The isaac sim loop runner is enabled by default in isaac sim apps, + # but in case we are in an app with the kit loop runner, protect against this + try: + import omni.kit.loop._loop as omni_loop + + _loop_runner = omni_loop.acquire_loop_interface() + _loop_runner.set_manual_step_size(self.cfg.dt * render_interval) + _loop_runner.set_manual_mode(True) + except Exception: + self.logger.warning( + "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" + ) + self.carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) + self.carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) + def _apply_physics_settings(self): """Sets various carb physics settings.""" + + # -------------------------- + # Carb Physics API settings + # -------------------------- + # enable hydra scene-graph instancing # note: this allows rendering of instanceable assets on the GUI self.carb_settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) @@ -721,6 +867,99 @@ def _apply_physics_settings(self): # hide the Simulation Settings window self.carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) + # handle device settings + if "cuda" in self.cfg.device: + parsed_device = self.cfg.device.split(":") + if len(parsed_device) == 1: + # if users only specified "cuda", we check if carb settings provide a valid device id + # otherwise, we default to device id 0 + device_id = self.carb_settings.get_as_int("/physics/cudaDevice") + if device_id < 0: + self.carb_settings.set_int("/physics/cudaDevice", 0) + device_id = 0 + else: + # if users specified "cuda:N", we use the provided device id + self.carb_settings.set_int("/physics/cudaDevice", int(parsed_device[1])) + # suppress readback for GPU physics + self.carb_settings.set_bool("/physics/suppressReadback", True) + # save the device + self._physics_device = f"cuda:{device_id}" + else: + # enable USD read/write operations for CPU physics + self.carb_settings.set_int("/physics/cudaDevice", -1) + self.carb_settings.set_bool("/physics/suppressReadback", False) + # save the device + self._physics_device = "cpu" + + # -------------------------- + # USDPhysics API settings + # -------------------------- + + # set gravity + gravity = np.asarray(self.cfg.gravity) + gravity_magnitude = np.linalg.norm(gravity) + # avoid division by zero + if gravity_magnitude != 0.0: + gravity_direction = gravity / gravity_magnitude + else: + gravity_direction = gravity + + self._physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) + self._physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) + + # create the default physics material + # this material is used when no material is specified for a primitive + material_path = f"{self.cfg.physics_prim_path}/defaultMaterial" + self.cfg.physics_material.func(material_path, self.cfg.physics_material) + # bind the physics material to the scene + sim_utils.bind_physics_material(self.cfg.physics_prim_path, material_path) + + # -------------------------- + # PhysX API settings + # -------------------------- + + # set broadphase type + broadphase_type = "GPU" if "cuda" in self.cfg.device else "MBP" + self._physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) + # set gpu dynamics + enable_gpu_dynamics = "cuda" in self.cfg.device + self._physx_scene_api.CreateEnableGPUDynamicsAttr(enable_gpu_dynamics) + + # GPU-dynamics does not support CCD, so we disable it if it is enabled. + if enable_gpu_dynamics and self.cfg.physx.enable_ccd: + self.cfg.physx.enable_ccd = False + self.logger.warning( + "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxCfg config to avoid this" + " warning." + ) + self._physx_scene_api.CreateEnableCCDAttr(self.cfg.physx.enable_ccd) + + # set solver type + solver_type = "PGS" if self.cfg.physx.solver_type == 0 else "TGS" + self._physx_scene_api.CreateSolverTypeAttr(solver_type) + + # iterate over all the settings and set them + for key, value in self.cfg.physx.to_dict().items(): # type: ignore + if key in ["solver_type", "enable_ccd"]: + continue + sim_utils.safe_set_attribute_on_usd_schema(self._physx_scene_api, key, value, camel_case=True) + + # throw warnings for helpful guidance + if self.cfg.physx.solver_type == 1 and not self.cfg.physx.enable_external_forces_every_iteration: + logger.warning( + "The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are" + " experiencing noisy velocities, consider enabling this flag. You may need to slightly increase the" + " number of velocity iterations (setting it to 1 or 2 rather than 0), together with this flag, to" + " improve the accuracy of velocity updates." + ) + + if not self.cfg.physx.enable_stabilization and self.cfg.dt > 0.0333: + self.logger.warning( + "Large simulation step size (> 0.0333 seconds) is not recommended without enabling stabilization." + " Consider setting the `enable_stabilization` flag to True in the PhysxCfg, or reducing the" + " simulation step size if you run into physics issues." + ) + def _apply_render_settings_from_cfg(self): # noqa: C901 """Sets rtx settings specified in the RenderCfg.""" @@ -815,74 +1054,16 @@ def _apply_render_settings_from_cfg(self): # noqa: C901 if self.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": self.carb_settings.set_string("/rtx/rendermode", "RaytracedLighting") - def _set_additional_physx_params(self): - """Sets additional PhysX parameters that are not directly supported by the parent class.""" - # obtain the physics scene api - physics_scene: UsdPhysics.Scene = self._physics_context._physics_scene - physx_scene_api: PhysxSchema.PhysxSceneAPI = self._physics_context._physx_scene_api - # assert that scene api is not None - if physx_scene_api is None: - raise RuntimeError("Physics scene API is None! Please create the scene first.") - # set parameters not directly supported by the constructor - # -- Continuous Collision Detection (CCD) - # ref: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/AdvancedCollisionDetection.html?highlight=ccd#continuous-collision-detection - self._physics_context.enable_ccd(self.cfg.physx.enable_ccd) - # -- GPU collision stack size - physx_scene_api.CreateGpuCollisionStackSizeAttr(self.cfg.physx.gpu_collision_stack_size) - # -- Improved determinism by PhysX - physx_scene_api.CreateEnableEnhancedDeterminismAttr(self.cfg.physx.enable_enhanced_determinism) - # -- Set solve_articulation_contact_last by add attribute to the PhysxScene prim, and add attribute there. - physx_prim = physx_scene_api.GetPrim() - physx_prim.CreateAttribute("physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool).Set( - self.cfg.physx.solve_articulation_contact_last - ) - # -- Enable external forces every iteration, helps improve the accuracy of velocity updates. - - if self.cfg.physx.solver_type == 1: - if not self.cfg.physx.enable_external_forces_every_iteration: - logger.warning( - "The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are" - " experiencing noisy velocities, consider enabling this flag. You may need to slightly increase the" - " number of velocity iterations (setting it to 1 or 2 rather than 0), together with this flag, to" - " improve the accuracy of velocity updates." - ) - physx_scene_api.CreateEnableExternalForcesEveryIterationAttr( - self.cfg.physx.enable_external_forces_every_iteration - ) - - # -- Gravity - # note: Isaac sim only takes the "up-axis" as the gravity direction. But physics allows any direction so we - # need to convert the gravity vector to a direction and magnitude pair explicitly. - gravity = np.asarray(self.cfg.gravity) - gravity_magnitude = np.linalg.norm(gravity) - - # Avoid division by zero - if gravity_magnitude != 0.0: - gravity_direction = gravity / gravity_magnitude - else: - gravity_direction = gravity - - physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) - physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) - - # position iteration count - physx_scene_api.CreateMinPositionIterationCountAttr(self.cfg.physx.min_position_iteration_count) - physx_scene_api.CreateMaxPositionIterationCountAttr(self.cfg.physx.max_position_iteration_count) - # velocity iteration count - physx_scene_api.CreateMinVelocityIterationCountAttr(self.cfg.physx.min_velocity_iteration_count) - physx_scene_api.CreateMaxVelocityIterationCountAttr(self.cfg.physx.max_velocity_iteration_count) - - # create the default physics material - # this material is used when no material is specified for a primitive - # check: https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.sim.html#isaaclab.sim.SimulationCfg.physics_material - material_path = f"{self.cfg.physics_prim_path}/defaultMaterial" - self.cfg.physics_material.func(material_path, self.cfg.physics_material) - # bind the physics material to the scene - bind_physics_material(self.cfg.physics_prim_path, material_path) - def _load_fabric_interface(self): """Loads the fabric interface if enabled.""" + extension_manager = omni.kit.app.get_app().get_extension_manager() + fabric_enabled = extension_manager.is_extension_enabled("omni.physx.fabric") + if self.cfg.use_fabric: + if not fabric_enabled: + extension_manager.set_extension_enabled_immediate("omni.physx.fabric", True) + + # load fabric interface from omni.physxfabric import get_physx_fabric_interface # acquire fabric interface @@ -896,6 +1077,19 @@ def _load_fabric_interface(self): else: # Needed for backward compatibility with older Isaac Sim versions self._update_fabric = self._fabric_iface.update + else: + if fabric_enabled: + extension_manager.set_extension_enabled_immediate("omni.physx.fabric", False) + + # set carb settings for fabric + self.carb_settings.set_bool("/physics/updateToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateParticlesToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateVelocitiesToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateForceSensorsToUsd", not self.cfg.use_fabric) + + """ + Helper functions - Animation Recording. + """ def _update_anim_recording(self): """Tracks anim recording timestamps and triggers finish animation recording if the total time has elapsed.""" @@ -1131,7 +1325,6 @@ def build_simulation_context( sim.stop() # Clear the stage - sim.clear_all_callbacks() sim.clear_instance() # check if we need to raise an exception that was raised in a callback if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: From b77a28cb0f2ac8f0378bae21f67f6109a9d22e4e Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 03:44:25 +0100 Subject: [PATCH 055/100] fixes run instructions --- .../isaaclab/sim/simulation_context.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index c6eee377923..a23aa1322b4 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -295,7 +295,7 @@ def __init__(self, cfg: SimulationCfg | None = None): # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes # when in headless mode self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) # load flatcache/fabric interface self._load_fabric_interface() @@ -564,7 +564,7 @@ def is_playing(self) -> bool: Returns: True if the simulator is playing. """ - return self._timeline.is_playing() + return self._timeline_iface.is_playing() def is_stopped(self) -> bool: """Check whether the simulation is stopped. @@ -572,27 +572,27 @@ def is_stopped(self) -> bool: Returns: True if the simulator is stopped. """ - return self._timeline.is_stopped() + return self._timeline_iface.is_stopped() def play(self) -> None: """Start playing the simulation.""" # play the simulation - self._timeline.play() - self._timeline.commit() + self._timeline_iface.play() + self._timeline_iface.commit() # perform one step to propagate all physics handles properly if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) def pause(self) -> None: """Pause the simulation.""" # pause the simulation - self._timeline.pause() + self._timeline_iface.pause() # set the play simulations setting if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) def stop(self) -> None: @@ -602,11 +602,11 @@ def stop(self) -> None: Stopping the simulation will lead to the simulation state being lost. """ # stop the simulation - self._timeline.stop() + self._timeline_iface.stop() # set the play simulations setting if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) """ @@ -699,7 +699,7 @@ def step(self, render: bool = True): is_anim_recording_finished = self._update_anim_recording() if is_anim_recording_finished: logger.warning("[INFO][SimulationContext]: Animation recording finished. Closing app.") - self._app.shutdown() + self._app_iface.shutdown() # check if the simulation timeline is paused. in that case keep stepping until it is playing if not self.is_playing(): @@ -757,7 +757,7 @@ def render(self, mode: RenderMode | None = None): self._render_throttle_counter = 0 # here we don't render viewport so don't need to flush fabric data self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) else: # manually flush the fabric data to update Hydra textures @@ -766,7 +766,7 @@ def render(self, mode: RenderMode | None = None): # note: we don't call super().render() anymore because they do above operation inside # and we don't want to do it twice. We may remove it once we drop support for Isaac Sim 2022.2. self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) # app.update() may be changing the cuda device, so we force it back to our desired device here From 865502d4823e82173d44e605650b4bd73a03d651 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 03:46:19 +0100 Subject: [PATCH 056/100] reuse gravity --- source/isaaclab/isaaclab/sim/simulation_context.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index a23aa1322b4..951fd9caad1 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -896,13 +896,11 @@ def _apply_physics_settings(self): # -------------------------- # set gravity - gravity = np.asarray(self.cfg.gravity) - gravity_magnitude = np.linalg.norm(gravity) + gravity = self._gravity_tensor + gravity_magnitude = torch.norm(gravity).item() # avoid division by zero - if gravity_magnitude != 0.0: - gravity_direction = gravity / gravity_magnitude - else: - gravity_direction = gravity + gravity_direction = gravity / (gravity_magnitude + 1e-6) + gravity_direction = gravity_direction.cpu().numpy() self._physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) self._physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) From 8e5d0802df0cfc4c27a74e20b6cfa1a969df1f11 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 03:58:36 +0100 Subject: [PATCH 057/100] fixes some bugs --- .../isaaclab/sim/simulation_context.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 951fd9caad1..ebdf5097b7d 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -9,7 +9,6 @@ import enum import glob import logging -import numpy as np import os import re import time @@ -31,7 +30,7 @@ import omni.usd from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager from isaacsim.core.utils.viewports import set_camera_view -from pxr import Gf, PhysxSchema, Usd, UsdGeom, UsdPhysics, UsdUtils +from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils import isaaclab.sim as sim_utils from isaaclab.utils.logger import configure_logging @@ -285,7 +284,7 @@ def __init__(self, cfg: SimulationCfg | None = None): if self._initial_stage.GetPrimAtPath(self.cfg.physics_prim_path).IsValid(): raise RuntimeError(f"A prim already exists at path '{self.cfg.physics_prim_path}'.") self._physics_scene = UsdPhysics.Scene.Define(self._initial_stage, self.cfg.physics_prim_path) - self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene) + self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene.GetPrim()) # set time codes per second self._configure_simulation_dt() @@ -879,7 +878,8 @@ def _apply_physics_settings(self): device_id = 0 else: # if users specified "cuda:N", we use the provided device id - self.carb_settings.set_int("/physics/cudaDevice", int(parsed_device[1])) + device_id = int(parsed_device[1]) + self.carb_settings.set_int("/physics/cudaDevice", device_id) # suppress readback for GPU physics self.carb_settings.set_bool("/physics/suppressReadback", True) # save the device @@ -900,7 +900,7 @@ def _apply_physics_settings(self): gravity_magnitude = torch.norm(gravity).item() # avoid division by zero gravity_direction = gravity / (gravity_magnitude + 1e-6) - gravity_direction = gravity_direction.cpu().numpy() + gravity_direction = gravity_direction.tolist() self._physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) self._physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) @@ -936,10 +936,16 @@ def _apply_physics_settings(self): solver_type = "PGS" if self.cfg.physx.solver_type == 0 else "TGS" self._physx_scene_api.CreateSolverTypeAttr(solver_type) + # set solve articulation contact last + attr = self._physx_scene_api.GetPrim().CreateAttribute("physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool) + attr.Set(self.cfg.physx.solve_articulation_contact_last) + # iterate over all the settings and set them for key, value in self.cfg.physx.to_dict().items(): # type: ignore - if key in ["solver_type", "enable_ccd"]: + if key in ["solver_type", "enable_ccd", "solve_articulation_contact_last"]: continue + if key == "bounce_threshold_velocity": + key = "bounce_threshold" sim_utils.safe_set_attribute_on_usd_schema(self._physx_scene_api, key, value, camel_case=True) # throw warnings for helpful guidance From bddd0c2216a94669f856e706b425fb52bde6678f Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 04:05:28 +0100 Subject: [PATCH 058/100] removes other locations of clear_all_callbacks --- .../benchmarks/benchmark_xform_prim_view.py | 1 - .../isaaclab/sim/simulation_context.py | 8 +++-- .../test/controllers/test_differential_ik.py | 1 - .../controllers/test_operational_space.py | 1 - .../test/scene/test_interactive_scene.py | 1 - source/isaaclab/test/sensors/test_camera.py | 3 +- .../test/sensors/test_frame_transformer.py | 1 - source/isaaclab/test/sensors/test_imu.py | 1 - .../test_multi_mesh_ray_caster_camera.py | 3 +- .../test/sensors/test_multi_tiled_camera.py | 3 +- .../test/sensors/test_ray_caster_camera.py | 3 +- .../isaaclab/test/sensors/test_sensor_base.py | 3 +- .../test/sensors/test_tiled_camera.py | 3 +- .../isaaclab/test/sim/test_mesh_converter.py | 1 - .../isaaclab/test/sim/test_mjcf_converter.py | 1 - source/isaaclab/test/sim/test_schemas.py | 1 - .../test/sim/test_simulation_context.py | 31 ++----------------- .../sim/test_simulation_stage_in_memory.py | 1 - .../test/sim/test_spawn_from_files.py | 1 - source/isaaclab/test/sim/test_spawn_lights.py | 1 - .../isaaclab/test/sim/test_spawn_materials.py | 1 - source/isaaclab/test/sim/test_spawn_meshes.py | 1 - .../isaaclab/test/sim/test_spawn_sensors.py | 1 - source/isaaclab/test/sim/test_spawn_shapes.py | 1 - .../isaaclab/test/sim/test_spawn_wrappers.py | 1 - .../isaaclab/test/sim/test_urdf_converter.py | 1 - 26 files changed, 13 insertions(+), 62 deletions(-) diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 99adf7cea2a..d00dff6514d 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -203,7 +203,6 @@ def benchmark_xform_prim_view( # close simulation sim.clear() - sim.clear_all_callbacks() sim.clear_instance() return timing_results, computed_results diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index ebdf5097b7d..25c23950cba 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -324,10 +324,10 @@ def instance(cls) -> SimulationContext: SimulationContext: The instance of the simulation context. """ if cls._instance is None: - raise RuntimeError( + logging.error( "Simulation context is not initialized. Please create a new instance using the constructor." ) - return cls._instance + return cls._instance # type: ignore @classmethod def clear_instance(cls) -> None: @@ -937,7 +937,9 @@ def _apply_physics_settings(self): self._physx_scene_api.CreateSolverTypeAttr(solver_type) # set solve articulation contact last - attr = self._physx_scene_api.GetPrim().CreateAttribute("physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool) + attr = self._physx_scene_api.GetPrim().CreateAttribute( + "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool + ) attr.Set(self.cfg.physx.solve_articulation_contact_last) # iterate over all the settings and set them diff --git a/source/isaaclab/test/controllers/test_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py index f7c588aad81..60e48027588 100644 --- a/source/isaaclab/test/controllers/test_differential_ik.py +++ b/source/isaaclab/test/controllers/test_differential_ik.py @@ -78,7 +78,6 @@ def sim(): # Cleanup sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 3da3e627bdb..03464ff9465 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -203,7 +203,6 @@ def sim(): # Cleanup sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/scene/test_interactive_scene.py b/source/isaaclab/test/scene/test_interactive_scene.py index 8cda88089a8..80a8c892d5a 100644 --- a/source/isaaclab/test/scene/test_interactive_scene.py +++ b/source/isaaclab/test/scene/test_interactive_scene.py @@ -69,7 +69,6 @@ def make_scene(num_envs: int, env_spacing: float = 1.0): yield make_scene, sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index dd492c1a2c4..4d557c547df 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -77,9 +77,8 @@ def teardown(sim: sim_utils.SimulationContext): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 546a9c06fa3..d84bd046afa 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -82,7 +82,6 @@ def sim(): sim.set_camera_view(eye=(5.0, 5.0, 5.0), target=(0.0, 0.0, 0.0)) yield sim # Cleanup - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_imu.py b/source/isaaclab/test/sensors/test_imu.py index 0c5a3219cea..73e2f2bec21 100644 --- a/source/isaaclab/test/sensors/test_imu.py +++ b/source/isaaclab/test/sensors/test_imu.py @@ -214,7 +214,6 @@ def setup_sim(): sim.reset() yield sim, scene # Cleanup - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py index 03f32345ff8..385b0aac460 100644 --- a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py @@ -81,9 +81,8 @@ def setup_simulation(): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_multi_tiled_camera.py b/source/isaaclab/test/sensors/test_multi_tiled_camera.py index 41b5ad019c7..e480f113bc0 100644 --- a/source/isaaclab/test/sensors/test_multi_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_multi_tiled_camera.py @@ -60,9 +60,8 @@ def setup_camera(): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index c441d0dca70..7375a8c22fc 100644 --- a/source/isaaclab/test/sensors/test_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_ray_caster_camera.py @@ -84,9 +84,8 @@ def teardown(sim: sim_utils.SimulationContext): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_sensor_base.py b/source/isaaclab/test/sensors/test_sensor_base.py index eee39ac22ff..b61c7144f93 100644 --- a/source/isaaclab/test/sensors/test_sensor_base.py +++ b/source/isaaclab/test/sensors/test_sensor_base.py @@ -109,9 +109,8 @@ def create_dummy_sensor(request, device): # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_tiled_camera.py b/source/isaaclab/test/sensors/test_tiled_camera.py index 038646392e5..cc4950f2723 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_tiled_camera.py @@ -59,8 +59,7 @@ def setup_camera(device) -> tuple[sim_utils.SimulationContext, TiledCameraCfg, f yield sim, camera_cfg, dt # Teardown rep.vp_manager.destroy_hydra_textures("Replicator") - sim._timeline.stop() - sim.clear_all_callbacks() + sim.stop() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index 8a14cd53bb7..838f1171b31 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -71,7 +71,6 @@ def sim(): sim.stop() # cleanup stage and context sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_mjcf_converter.py b/source/isaaclab/test/sim/test_mjcf_converter.py index ed061f73be8..a10650f39b8 100644 --- a/source/isaaclab/test/sim/test_mjcf_converter.py +++ b/source/isaaclab/test/sim/test_mjcf_converter.py @@ -48,7 +48,6 @@ def test_setup_teardown(): # Teardown: Cleanup simulation sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 0895f49080a..3e88e856cbe 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -75,7 +75,6 @@ def setup_simulation(): # Teardown sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 180fb9e7941..fb810f88123 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -13,13 +13,9 @@ """Rest everything follows.""" import numpy as np -from collections.abc import Generator -import omni.physx import pytest -from isaacsim.core.api.simulation_context import SimulationContext as IsaacSimulationContext -import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -36,33 +32,12 @@ def test_setup_teardown(): SimulationContext.clear_instance() -@pytest.fixture -def sim_with_stage_in_memory() -> Generator[SimulationContext, None, None]: - """Create a simulation context with stage in memory.""" - # create stage in memory - cfg = SimulationCfg(create_stage_in_memory=True) - sim = SimulationContext(cfg=cfg) - # update stage - sim_utils.update_stage() - # yield simulation context - yield sim - # stop simulation - omni.physx.get_physx_simulation_interface().detach_stage() - sim.stop() - # clear simulation context - sim.clear() - sim.clear_all_callbacks() - sim.clear_instance() - - @pytest.mark.isaacsim_ci def test_singleton(): """Tests that the singleton is working.""" sim1 = SimulationContext() sim2 = SimulationContext() - sim3 = IsaacSimulationContext() assert sim1 is sim2 - assert sim1 is sim3 # try to delete the singleton sim2.clear_instance() @@ -70,11 +45,9 @@ def test_singleton(): # create new instance sim4 = SimulationContext() assert sim1 is not sim4 - assert sim3 is not sim4 assert sim1.instance() is sim4.instance() - assert sim3.instance() is sim4.instance() # clear instance - sim3.clear_instance() + sim4.clear_instance() @pytest.mark.isaacsim_ci @@ -138,7 +111,7 @@ def test_headless_mode(): # """ # sim = SimulationContext() # # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. -# sim.clear_all_callbacks() +# # sim._stage_open_callback = None # sim._physics_timer_callback = None # sim._event_timer_callback = None diff --git a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py index fdb5f333813..eaf95b5f011 100644 --- a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py @@ -39,7 +39,6 @@ def sim(): omni.physx.get_physx_simulation_interface().detach_stage() sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index 199d69a731e..94fa9b0fde4 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -37,7 +37,6 @@ def sim(): # cleanup after test sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_lights.py b/source/isaaclab/test/sim/test_spawn_lights.py index ea8cbea7f94..b4d04697b82 100644 --- a/source/isaaclab/test/sim/test_spawn_lights.py +++ b/source/isaaclab/test/sim/test_spawn_lights.py @@ -39,7 +39,6 @@ def sim(): # Teardown: Stop simulation sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_materials.py b/source/isaaclab/test/sim/test_spawn_materials.py index af02dadc3dc..04a47e72271 100644 --- a/source/isaaclab/test/sim/test_spawn_materials.py +++ b/source/isaaclab/test/sim/test_spawn_materials.py @@ -31,7 +31,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index b5fbd89bf41..7a16fb434af 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -34,7 +34,6 @@ def sim(): # Cleanup sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_sensors.py b/source/isaaclab/test/sim/test_spawn_sensors.py index 6b4e9cc6d40..8647cb3f6e4 100644 --- a/source/isaaclab/test/sim/test_spawn_sensors.py +++ b/source/isaaclab/test/sim/test_spawn_sensors.py @@ -32,7 +32,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index ec1a12bc044..3979a99d4b1 100644 --- a/source/isaaclab/test/sim/test_spawn_shapes.py +++ b/source/isaaclab/test/sim/test_spawn_shapes.py @@ -28,7 +28,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index 812c6c2a8d7..9049082d1a8 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -30,7 +30,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_urdf_converter.py b/source/isaaclab/test/sim/test_urdf_converter.py index 68d3f58be07..c93e66823d4 100644 --- a/source/isaaclab/test/sim/test_urdf_converter.py +++ b/source/isaaclab/test/sim/test_urdf_converter.py @@ -48,7 +48,6 @@ def sim_config(): # Teardown sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() From 8da331f282a953411e1082f5601c1adbd656a6b2 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 04:18:04 +0100 Subject: [PATCH 059/100] hacks in some fixes --- .../isaaclab/sim/simulation_context.py | 10 ++++---- .../test/sim/test_simulation_context.py | 25 +++++++++++++------ 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 25c23950cba..632deaff6b0 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -324,9 +324,7 @@ def instance(cls) -> SimulationContext: SimulationContext: The instance of the simulation context. """ if cls._instance is None: - logging.error( - "Simulation context is not initialized. Please create a new instance using the constructor." - ) + logging.error("Simulation context is not initialized. Please create a new instance using the constructor.") return cls._instance # type: ignore @classmethod @@ -394,7 +392,7 @@ def has_rtx_sensors(self) -> bool: .. _NVIDIA RTX documentation: https://developer.nvidia.com/rendering-technologies """ - return self._settings.get_as_bool("/isaaclab/render/rtx_sensors") + return self.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") def is_fabric_enabled(self) -> bool: """Returns whether the fabric interface is enabled. @@ -899,7 +897,9 @@ def _apply_physics_settings(self): gravity = self._gravity_tensor gravity_magnitude = torch.norm(gravity).item() # avoid division by zero - gravity_direction = gravity / (gravity_magnitude + 1e-6) + if gravity_magnitude == 0.0: + gravity_magnitude = 1.0 + gravity_direction = gravity / gravity_magnitude gravity_direction = gravity_direction.tolist() self._physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index fb810f88123..7411abe7166 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -43,11 +43,11 @@ def test_singleton(): sim2.clear_instance() assert sim1.instance() is None # create new instance - sim4 = SimulationContext() - assert sim1 is not sim4 - assert sim1.instance() is sim4.instance() + sim3 = SimulationContext() + assert sim1 is not sim3 + assert sim1.instance() is sim3.instance() # clear instance - sim4.clear_instance() + sim3.clear_instance() @pytest.mark.isaacsim_ci @@ -60,14 +60,20 @@ def test_initialization(): # sim = SimulationContext(cfg=cfg) # check valid settings - assert sim.get_physics_dt() == cfg.dt - assert sim.get_rendering_dt() == cfg.dt * cfg.render_interval + physics_hz = sim._physx_scene_api.GetTimeStepsPerSecondAttr().Get() + physics_dt = 1.0 / physics_hz + rendering_dt = cfg.dt * cfg.render_interval + assert physics_dt == cfg.dt + assert rendering_dt == cfg.dt * cfg.render_interval assert not sim.has_rtx_sensors() # check valid paths assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() assert sim.stage.GetPrimAtPath("/Physics/PhysX/defaultMaterial").IsValid() # check valid gravity - gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() + gravity_dir, gravity_mag = ( + sim._physics_scene.GetGravityDirectionAttr().Get(), + sim._physics_scene.GetGravityMagnitudeAttr().Get(), + ) gravity = np.array(gravity_dir) * gravity_mag np.testing.assert_almost_equal(gravity, cfg.gravity) @@ -137,6 +143,9 @@ def test_zero_gravity(): sim = SimulationContext(cfg) - gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() + gravity_dir, gravity_mag = ( + sim._physics_scene.GetGravityDirectionAttr().Get(), + sim._physics_scene.GetGravityMagnitudeAttr().Get(), + ) gravity = np.array(gravity_dir) * gravity_mag np.testing.assert_almost_equal(gravity, cfg.gravity) From a253344d31a95d70e1acd004f627b114728294f2 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 13:37:47 +0100 Subject: [PATCH 060/100] handles different indexing schemes --- .../isaaclab/sim/views/xform_prim_view.py | 52 ++++++++++++++----- 1 file changed, 40 insertions(+), 12 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 134b702c495..aff12bb57ee 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -7,6 +7,7 @@ import numpy as np import torch +from collections.abc import Sequence from pxr import Gf, Sdf, Usd, UsdGeom, Vt @@ -135,7 +136,7 @@ def set_world_poses( self, positions: torch.Tensor | None = None, orientations: torch.Tensor | None = None, - indices: torch.Tensor | None = None, + indices: Sequence[int] | None = None, ): """Set world-space poses for prims in the view. @@ -160,7 +161,11 @@ def set_world_poses( ValueError: If the number of poses doesn't match the number of indices provided. """ # Resolve indices - indices_list = self._ALL_INDICES if indices is None else indices.tolist() + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) # Validate inputs if positions is not None: @@ -241,7 +246,7 @@ def set_local_poses( self, translations: torch.Tensor | None = None, orientations: torch.Tensor | None = None, - indices: torch.Tensor | None = None, + indices: Sequence[int] | None = None, ) -> None: """Set local-space poses for prims in the view. @@ -266,7 +271,11 @@ def set_local_poses( ValueError: If the number of poses doesn't match the number of indices provided. """ # Resolve indices - indices_list = self._ALL_INDICES if indices is None else indices.tolist() + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) # Validate inputs if translations is not None: @@ -300,7 +309,7 @@ def set_local_poses( if orientations_array is not None: prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) - def set_scales(self, scales: torch.Tensor, indices: torch.Tensor | None = None): + def set_scales(self, scales: torch.Tensor, indices: Sequence[int] | None = None): """Set scales for prims in the view. This method sets the scale of each prim in the view. @@ -315,7 +324,11 @@ def set_scales(self, scales: torch.Tensor, indices: torch.Tensor | None = None): ValueError: If scales shape is not (M, 3). """ # Resolve indices - indices_list = self._ALL_INDICES if indices is None else indices.tolist() + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) # Validate inputs if scales.shape != (len(indices_list), 3): @@ -335,7 +348,7 @@ def set_scales(self, scales: torch.Tensor, indices: torch.Tensor | None = None): Operations - Getters. """ - def get_world_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Tensor, torch.Tensor]: + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: """Get world-space poses for prims in the view. This method retrieves the position and orientation of each prim in world space by computing @@ -356,7 +369,12 @@ def get_world_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te - orientations: Torch tensor of shape (M, 4) containing world-space quaternions (w, x, y, z) """ # Resolve indices - indices_list = self._ALL_INDICES if indices is None else indices.tolist() + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + # Create buffers positions = Vt.Vec3dArray(len(indices_list)) orientations = Vt.QuatdArray(len(indices_list)) @@ -385,7 +403,7 @@ def get_world_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te return positions, orientations # type: ignore - def get_local_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Tensor, torch.Tensor]: + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: """Get local-space poses for prims in the view. This method retrieves the position and orientation of each prim in local space (relative to @@ -406,7 +424,12 @@ def get_local_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te - orientations: Torch tensor of shape (M, 4) containing local-space quaternions (w, x, y, z) """ # Resolve indices - indices_list = self._ALL_INDICES if indices is None else indices.tolist() + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + # Create buffers translations = Vt.Vec3dArray(len(indices_list)) orientations = Vt.QuatdArray(len(indices_list)) @@ -435,7 +458,7 @@ def get_local_poses(self, indices: torch.Tensor | None = None) -> tuple[torch.Te return translations, orientations # type: ignore - def get_scales(self, indices: torch.Tensor | None = None) -> torch.Tensor: + def get_scales(self, indices: Sequence[int] | None = None) -> torch.Tensor: """Get scales for prims in the view. This method retrieves the scale of each prim in the view. @@ -448,7 +471,12 @@ def get_scales(self, indices: torch.Tensor | None = None) -> torch.Tensor: A tensor of shape (M, 3) containing the scales of each prim, where M is the number of prims queried. """ # Resolve indices - indices_list = self._ALL_INDICES if indices is None else indices.tolist() + if indices is None or indices == slice(None): + indices_list = self._ALL_INDICES + else: + # Convert to list if it is a tensor array + indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + # Create buffers scales = Vt.Vec3dArray(len(indices_list)) From 58d031b4154073004da2afed37f09a78efd4ce68 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 13:49:31 +0100 Subject: [PATCH 061/100] fixes bug with indexing --- .../isaaclab/sim/views/xform_prim_view.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index aff12bb57ee..fae14356b8a 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -194,9 +194,9 @@ def set_world_poses( # Set poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): - for idx in indices_list: + for idx, prim_idx in enumerate(indices_list): # Get prim - prim = self._prims[idx] + prim = self._prims[prim_idx] # Get parent prim for local space conversion parent_prim = prim.GetParent() @@ -300,9 +300,9 @@ def set_local_poses( # Set local poses for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): - for idx in indices_list: + for idx, prim_idx in enumerate(indices_list): # Get prim - prim = self._prims[idx] + prim = self._prims[prim_idx] # Set attributes if provided if translations_array is not None: prim.GetAttribute("xformOp:translate").Set(translations_array[idx]) @@ -338,9 +338,9 @@ def set_scales(self, scales: torch.Tensor, indices: Sequence[int] | None = None) # Set scales for each prim # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): - for idx in indices_list: + for idx, prim_idx in enumerate(indices_list): # Get prim - prim = self._prims[idx] + prim = self._prims[prim_idx] # Set scale attribute prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) @@ -383,9 +383,9 @@ def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.T # Note: We don't use :func:`isaaclab.sim.utils.transforms.resolve_prim_pose` # here since it isn't optimized for batch operations. - for idx in indices_list: + for idx, prim_idx in enumerate(indices_list): # Get prim - prim = self._prims[idx] + prim = self._prims[prim_idx] # get prim xform prim_tf = xform_cache.GetLocalToWorldTransform(prim) # sanitize quaternion @@ -438,9 +438,9 @@ def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.T # Note: We don't use :func:`isaaclab.sim.utils.transforms.resolve_prim_pose` # here since it isn't optimized for batch operations. - for idx in indices_list: + for idx, prim_idx in enumerate(indices_list): # Get prim - prim = self._prims[idx] + prim = self._prims[prim_idx] # get prim xform prim_tf = xform_cache.GetLocalTransformation(prim)[0] # sanitize quaternion @@ -480,9 +480,9 @@ def get_scales(self, indices: Sequence[int] | None = None) -> torch.Tensor: # Create buffers scales = Vt.Vec3dArray(len(indices_list)) - for idx in indices_list: + for idx, prim_idx in enumerate(indices_list): # Get prim - prim = self._prims[idx] + prim = self._prims[prim_idx] scales[idx] = prim.GetAttribute("xformOp:scale").Get() # Convert to tensor From 29285295459d232337fc16366e4a5986119f2fef Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 13:49:40 +0100 Subject: [PATCH 062/100] adds test for indexing --- .../isaaclab/test/sim/test_xform_prim_view.py | 502 +++++++++++++++--- 1 file changed, 425 insertions(+), 77 deletions(-) diff --git a/source/isaaclab/test/sim/test_xform_prim_view.py b/source/isaaclab/test/sim/test_xform_prim_view.py index d3407611e51..456cf2e3e50 100644 --- a/source/isaaclab/test/sim/test_xform_prim_view.py +++ b/source/isaaclab/test/sim/test_xform_prim_view.py @@ -40,6 +40,23 @@ def test_setup_teardown(): sim_utils.clear_stage() +""" +Helper functions. +""" + + +def _prepare_indices(index_type, target_indices, num_prims, device): + """Helper function to prepare indices based on type.""" + if index_type == "list": + return target_indices, target_indices + elif index_type == "torch_tensor": + return torch.tensor(target_indices, dtype=torch.int64, device=device), target_indices + elif index_type == "slice_none": + return slice(None), list(range(num_prims)) + else: + raise ValueError(f"Unknown index type: {index_type}") + + """ Tests - Initialization. """ @@ -123,7 +140,7 @@ def test_xform_prim_view_initialization_empty_pattern(device): """ -Tests - Get/Set World Poses. +Tests - Getters. """ @@ -166,6 +183,83 @@ def test_get_world_poses(device): torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_local_poses(device): + """Test getting local poses from XformPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create parent and child prims + sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) + + # Children with different local poses + expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] + expected_local_orientations = [ + (1.0, 0.0, 0.0, 0.0), + (0.7071068, 0.0, 0.0, 0.7071068), + (0.7071068, 0.7071068, 0.0, 0.0), + ] + + for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=pos, orientation=quat, stage=stage) + + # Create view + view = XformPrimView("/World/Parent/Child_.*", device=device) + + # Get local poses + translations, orientations = view.get_local_poses() + + # Verify shapes + assert translations.shape == (3, 3) + assert orientations.shape == (3, 4) + + # Convert expected values to tensors + expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) + + # Verify translations + torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) + + # Verify orientations (allow for quaternion sign ambiguity) + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_scales(device): + """Test getting scales from XformPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims with different scales + expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] + + for i, scale in enumerate(expected_scales): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Get scales + scales = view.get_scales() + + # Verify shape and values + assert scales.shape == (3, 3) + expected_scales_tensor = torch.tensor(expected_scales, dtype=torch.float32, device=device) + torch.testing.assert_close(scales, expected_scales_tensor, atol=1e-5, rtol=0) + + +""" +Tests - Setters. +""" + + @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_set_world_poses(device): """Test setting world poses in XformPrimView.""" @@ -327,57 +421,6 @@ def test_set_world_poses_with_hierarchy(device): torch.testing.assert_close(retrieved_orientations, -desired_world_orientations, atol=1e-4, rtol=0) -""" -Tests - Get/Set Local Poses. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_get_local_poses(device): - """Test getting local poses from XformPrimView.""" - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - stage = sim_utils.get_current_stage() - - # Create parent and child prims - sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) - - # Children with different local poses - expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] - expected_local_orientations = [ - (1.0, 0.0, 0.0, 0.0), - (0.7071068, 0.0, 0.0, 0.7071068), - (0.7071068, 0.7071068, 0.0, 0.0), - ] - - for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): - sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=pos, orientation=quat, stage=stage) - - # Create view - view = XformPrimView("/World/Parent/Child_.*", device=device) - - # Get local poses - translations, orientations = view.get_local_poses() - - # Verify shapes - assert translations.shape == (3, 3) - assert orientations.shape == (3, 4) - - # Convert expected values to tensors - expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) - expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) - - # Verify translations - torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) - - # Verify orientations (allow for quaternion sign ambiguity) - try: - torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) - - @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_set_local_poses(device): """Test setting local poses in XformPrimView.""" @@ -462,65 +505,370 @@ def test_set_local_poses_only_translations(device): torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_scales(device): + """Test setting scales in XformPrimView.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=(1.0, 1.0, 1.0), stage=stage) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Set new scales + new_scales = torch.tensor( + [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device + ) + + view.set_scales(new_scales) + + # Get scales back + retrieved_scales = view.get_scales() + + # Verify they match + torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) + + """ -Tests - Get/Set Scales. +Tests - Index Handling. """ @pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_get_scales(device): - """Test getting scales from XformPrimView.""" +@pytest.mark.parametrize("index_type", ["list", "torch_tensor", "slice_none"]) +@pytest.mark.parametrize("method", ["world_poses", "local_poses", "scales"]) +def test_index_types_get_methods(device, index_type, method): + """Test that getter methods work with different index types.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") stage = sim_utils.get_current_stage() - # Create prims with different scales - expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] + # Create prims based on method type + num_prims = 10 + if method == "local_poses": + # Create parent and children for local poses + sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Parent/Child_{i}", "Xform", translation=(float(i), float(i) * 0.5, 0.0), stage=stage + ) + view = XformPrimView("/World/Parent/Child_.*", device=device) + elif method == "scales": + # Create prims with different scales + for i in range(num_prims): + scale = (1.0 + i * 0.5, 1.0 + i * 0.3, 1.0 + i * 0.2) + sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) + view = XformPrimView("/World/Object_.*", device=device) + else: # world_poses + # Create prims with different positions + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + view = XformPrimView("/World/Object_.*", device=device) + + # Get all data as reference + if method == "world_poses": + all_data1, all_data2 = view.get_world_poses() + elif method == "local_poses": + all_data1, all_data2 = view.get_local_poses() + else: # scales + all_data1 = view.get_scales() + all_data2 = None + + # Prepare indices + target_indices_base = [2, 5, 7] + indices, target_indices = _prepare_indices(index_type, target_indices_base, num_prims, device) + + # Get subset + if method == "world_poses": + subset_data1, subset_data2 = view.get_world_poses(indices=indices) # type: ignore[arg-type] + elif method == "local_poses": + subset_data1, subset_data2 = view.get_local_poses(indices=indices) # type: ignore[arg-type] + else: # scales + subset_data1 = view.get_scales(indices=indices) # type: ignore[arg-type] + subset_data2 = None - for i, scale in enumerate(expected_scales): - sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) + # Verify shapes + expected_count = len(target_indices) + assert subset_data1.shape == (expected_count, 3) + if subset_data2 is not None: + assert subset_data2.shape == (expected_count, 4) + + # Verify values + target_indices_tensor = torch.tensor(target_indices, dtype=torch.int64, device=device) + torch.testing.assert_close(subset_data1, all_data1[target_indices_tensor], atol=1e-5, rtol=0) + if subset_data2 is not None and all_data2 is not None: + torch.testing.assert_close(subset_data2, all_data2[target_indices_tensor], atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +@pytest.mark.parametrize("index_type", ["list", "torch_tensor", "slice_none"]) +@pytest.mark.parametrize("method", ["world_poses", "local_poses", "scales"]) +def test_index_types_set_methods(device, index_type, method): + """Test that setter methods work with different index types.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims based on method type + num_prims = 10 + if method == "local_poses": + # Create parent and children for local poses + sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 0.0), stage=stage) + for i in range(num_prims): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + view = XformPrimView("/World/Parent/Child_.*", device=device) + else: # world_poses or scales + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + view = XformPrimView("/World/Object_.*", device=device) + + # Get initial data + if method == "world_poses": + initial_data1, initial_data2 = view.get_world_poses() + elif method == "local_poses": + initial_data1, initial_data2 = view.get_local_poses() + else: # scales + initial_data1 = view.get_scales() + initial_data2 = None + + # Prepare indices + target_indices_base = [2, 5, 7] + indices, target_indices = _prepare_indices(index_type, target_indices_base, num_prims, device) + + # Prepare new data + num_to_set = len(target_indices) + if method in ["world_poses", "local_poses"]: + new_data1 = torch.randn(num_to_set, 3, device=device) * 10.0 + new_data2 = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_to_set, dtype=torch.float32, device=device) + else: # scales + new_data1 = torch.rand(num_to_set, 3, device=device) * 2.0 + 0.5 + new_data2 = None + + # Set data + if method == "world_poses": + view.set_world_poses(positions=new_data1, orientations=new_data2, indices=indices) # type: ignore[arg-type] + elif method == "local_poses": + view.set_local_poses(translations=new_data1, orientations=new_data2, indices=indices) # type: ignore[arg-type] + else: # scales + view.set_scales(scales=new_data1, indices=indices) # type: ignore[arg-type] + + # Get all data after update + if method == "world_poses": + updated_data1, updated_data2 = view.get_world_poses() + elif method == "local_poses": + updated_data1, updated_data2 = view.get_local_poses() + else: # scales + updated_data1 = view.get_scales() + updated_data2 = None + + # Verify that specified indices were updated + for i, target_idx in enumerate(target_indices): + torch.testing.assert_close(updated_data1[target_idx], new_data1[i], atol=1e-5, rtol=0) + if new_data2 is not None and updated_data2 is not None: + try: + torch.testing.assert_close(updated_data2[target_idx], new_data2[i], atol=1e-5, rtol=0) + except AssertionError: + # Account for quaternion sign ambiguity + torch.testing.assert_close(updated_data2[target_idx], -new_data2[i], atol=1e-5, rtol=0) + + # Verify that other indices were NOT updated (only for non-slice(None) cases) + if index_type != "slice_none": + for i in range(num_prims): + if i not in target_indices: + torch.testing.assert_close(updated_data1[i], initial_data1[i], atol=1e-5, rtol=0) + if initial_data2 is not None and updated_data2 is not None: + try: + torch.testing.assert_close(updated_data2[i], initial_data2[i], atol=1e-5, rtol=0) + except AssertionError: + # Account for quaternion sign ambiguity + torch.testing.assert_close(updated_data2[i], -initial_data2[i], atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_indices_single_element(device): + """Test with a single index.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) # Create view view = XformPrimView("/World/Object_.*", device=device) - # Get scales - scales = view.get_scales() + # Test with single index + indices = [3] + positions, orientations = view.get_world_poses(indices=indices) - # Verify shape and values - assert scales.shape == (3, 3) - expected_scales_tensor = torch.tensor(expected_scales, dtype=torch.float32, device=device) - torch.testing.assert_close(scales, expected_scales_tensor, atol=1e-5, rtol=0) + # Verify shapes + assert positions.shape == (1, 3) + assert orientations.shape == (1, 4) + + # Set pose for single index + new_position = torch.tensor([[100.0, 200.0, 300.0]], device=device) + view.set_world_poses(positions=new_position, indices=indices) + + # Verify it was set + retrieved_positions, _ = view.get_world_poses(indices=indices) + torch.testing.assert_close(retrieved_positions, new_position, atol=1e-5, rtol=0) @pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_scales(device): - """Test setting scales in XformPrimView.""" +def test_indices_out_of_order(device): + """Test with indices provided in non-sequential order.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") stage = sim_utils.get_current_stage() # Create prims - num_prims = 5 + num_prims = 10 for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=(1.0, 1.0, 1.0), stage=stage) + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) # Create view view = XformPrimView("/World/Object_.*", device=device) - # Set new scales - new_scales = torch.tensor( - [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device + # Use out-of-order indices + indices = [7, 2, 9, 0, 5] + new_positions = torch.tensor( + [[7.0, 0.0, 0.0], [2.0, 0.0, 0.0], [9.0, 0.0, 0.0], [0.0, 0.0, 0.0], [5.0, 0.0, 0.0]], device=device ) - view.set_scales(new_scales) + # Set poses with out-of-order indices + view.set_world_poses(positions=new_positions, indices=indices) - # Get scales back - retrieved_scales = view.get_scales() + # Get all poses + all_positions, _ = view.get_world_poses() - # Verify they match - torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) + # Verify each index got the correct value + expected_x_values = [0.0, 0.0, 2.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 9.0] + for i in range(num_prims): + assert abs(all_positions[i, 0].item() - expected_x_values[i]) < 1e-5 + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_indices_with_only_positions_or_orientations(device): + """Test indices work correctly when setting only positions or only orientations.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), orientation=(1.0, 0.0, 0.0, 0.0), stage=stage + ) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Get initial poses + initial_positions, initial_orientations = view.get_world_poses() + + # Set only positions for specific indices + indices = [1, 3] + new_positions = torch.tensor([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]], device=device) + view.set_world_poses(positions=new_positions, orientations=None, indices=indices) + + # Get updated poses + updated_positions, updated_orientations = view.get_world_poses() + + # Verify positions updated for indices 1 and 3, others unchanged + torch.testing.assert_close(updated_positions[1], new_positions[0], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_positions[3], new_positions[1], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_positions[0], initial_positions[0], atol=1e-5, rtol=0) + + # Verify all orientations unchanged + try: + torch.testing.assert_close(updated_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(updated_orientations, -initial_orientations, atol=1e-5, rtol=0) + + # Now set only orientations for different indices + indices2 = [0, 4] + new_orientations = torch.tensor([[0.7071068, 0.0, 0.0, 0.7071068], [0.7071068, 0.7071068, 0.0, 0.0]], device=device) + view.set_world_poses(positions=None, orientations=new_orientations, indices=indices2) + + # Get final poses + final_positions, final_orientations = view.get_world_poses() + + # Verify positions unchanged from previous step + torch.testing.assert_close(final_positions, updated_positions, atol=1e-5, rtol=0) + + # Verify orientations updated for indices 0 and 4 + try: + torch.testing.assert_close(final_orientations[0], new_orientations[0], atol=1e-5, rtol=0) + torch.testing.assert_close(final_orientations[4], new_orientations[1], atol=1e-5, rtol=0) + except AssertionError: + # Account for quaternion sign ambiguity + torch.testing.assert_close(final_orientations[0], -new_orientations[0], atol=1e-5, rtol=0) + torch.testing.assert_close(final_orientations[4], -new_orientations[1], atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_index_type_none_equivalent_to_all(device): + """Test that indices=None is equivalent to getting/setting all prims.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + + stage = sim_utils.get_current_stage() + + # Create prims + num_prims = 6 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) + + # Create view + view = XformPrimView("/World/Object_.*", device=device) + + # Get poses with indices=None + pos_none, quat_none = view.get_world_poses(indices=None) + + # Get poses with no argument (default) + pos_default, quat_default = view.get_world_poses() + + # Get poses with slice(None) + pos_slice, quat_slice = view.get_world_poses(indices=slice(None)) # type: ignore[arg-type] + + # All should be equivalent + torch.testing.assert_close(pos_none, pos_default, atol=1e-10, rtol=0) + torch.testing.assert_close(quat_none, quat_default, atol=1e-10, rtol=0) + torch.testing.assert_close(pos_none, pos_slice, atol=1e-10, rtol=0) + torch.testing.assert_close(quat_none, quat_slice, atol=1e-10, rtol=0) + + # Test the same for set operations + new_positions = torch.randn(num_prims, 3, device=device) * 10.0 + new_orientations = torch.tensor([[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32, device=device) + + # Set with indices=None + view.set_world_poses(positions=new_positions, orientations=new_orientations, indices=None) + pos_after_none, quat_after_none = view.get_world_poses() + + # Reset + view.set_world_poses(positions=torch.zeros(num_prims, 3, device=device), indices=None) + + # Set with slice(None) + view.set_world_poses(positions=new_positions, orientations=new_orientations, indices=slice(None)) # type: ignore[arg-type] + pos_after_slice, quat_after_slice = view.get_world_poses() + + # Should be equivalent + torch.testing.assert_close(pos_after_none, pos_after_slice, atol=1e-5, rtol=0) + torch.testing.assert_close(quat_after_none, quat_after_slice, atol=1e-5, rtol=0) """ From 25a313e248db22eec12728ba078d048ebd11168c Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 13:59:50 +0100 Subject: [PATCH 063/100] moves file for naming --- .../sim/{test_xform_prim_view.py => test_views_xform_prim.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename source/isaaclab/test/sim/{test_xform_prim_view.py => test_views_xform_prim.py} (100%) diff --git a/source/isaaclab/test/sim/test_xform_prim_view.py b/source/isaaclab/test/sim/test_views_xform_prim.py similarity index 100% rename from source/isaaclab/test/sim/test_xform_prim_view.py rename to source/isaaclab/test/sim/test_views_xform_prim.py From bcee3578c0825008e653bfb7421dcea98ca4ff70 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 14:01:55 +0100 Subject: [PATCH 064/100] fixes doc --- source/isaaclab/isaaclab/sim/views/xform_prim_view.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index fae14356b8a..d0c74de0f99 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -40,7 +40,8 @@ class XformPrimView: All prims in the view must be Xformable and have standardized transform operations: ``[translate, orient, scale]``. Non-standard prims will raise a ValueError during - initialization. Use :func:`isaaclab.sim.utils.standardize_xform_ops` to prepare prims. + initialization if :attr:`validate_xform_ops` is True. Please use the function + :func:`isaaclab.sim.utils.standardize_xform_ops` to prepare prims before using this view. .. warning:: This class operates at the USD default time code. Any animation or time-sampled data From 51d68c0e8d4df86523c7d731aad91163dd82c413 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 14:57:46 +0100 Subject: [PATCH 065/100] format fixes --- .../test/sim/test_simulation_stage_in_memory.py | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py index eaf95b5f011..ee3db266fb8 100644 --- a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py @@ -66,16 +66,9 @@ def test_stage_in_memory_with_shapes(sim): cfg = sim_utils.MultiAssetSpawnerCfg( assets_cfg=[ - sim_utils.ConeCfg( - radius=0.3, - height=0.6, - ), - sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - ), - sim_utils.SphereCfg( - radius=0.3, - ), + sim_utils.ConeCfg(radius=0.3, height=0.6), + sim_utils.CuboidCfg(size=(0.3, 0.3, 0.3)), + sim_utils.SphereCfg(radius=0.3), ], random_choice=True, rigid_props=sim_utils.RigidBodyPropertiesCfg( From c81a2d9b2757ea809e3ef12164e97df9de2292d3 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 14:57:59 +0100 Subject: [PATCH 066/100] fixes initialization --- .../isaaclab/sim/simulation_context.py | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 632deaff6b0..5c2087711ea 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -157,15 +157,17 @@ def __init__(self, cfg: SimulationCfg | None = None): self._initial_stage = sim_utils.create_new_stage_in_memory() else: self._initial_stage = omni.usd.get_context().get_stage() + if not self._initial_stage: + self._initial_stage = sim_utils.create_new_stage() + # check that stage is created + if self._initial_stage is None: + raise RuntimeError("Failed to create a new stage. Please check if the USD context is valid.") # add stage to USD cache stage_cache = UsdUtils.StageCache.Get() stage_id = stage_cache.GetId(self._initial_stage).ToLongInt() if stage_id < 0: stage_id = stage_cache.Insert(self._initial_stage).ToLongInt() self._initial_stage_id = stage_id - # check that stage is created - if self._initial_stage is None: - raise RuntimeError("Failed to create a new stage. Please check if the USD context is valid.") # acquire settings interface self.carb_settings = carb.settings.get_settings() @@ -269,6 +271,9 @@ def __init__(self, cfg: SimulationCfg | None = None): self._physx_sim_iface = omni.physx.get_physx_simulation_interface() self._timeline_iface = omni.timeline.get_timeline_interface() + # set timeline auto update to True + self._timeline_iface.set_auto_update(True) + # set stage properties with sim_utils.use_stage(self._initial_stage): # correct conventions for metric units @@ -278,8 +283,8 @@ def __init__(self, cfg: SimulationCfg | None = None): # find if any physics prim already exists and delete it for prim in self._initial_stage.Traverse(): - if prim.HasAPI(PhysxSchema.PhysxSceneAPI): - sim_utils.delete_prim(prim.GetPath().pathString) + if prim.HasAPI(PhysxSchema.PhysxSceneAPI) or prim.GetTypeName() == "PhysicsScene": + sim_utils.delete_prim(prim.GetPath().pathString, stage=self._initial_stage) # create a new physics scene if self._initial_stage.GetPrimAtPath(self.cfg.physics_prim_path).IsValid(): raise RuntimeError(f"A prim already exists at path '{self.cfg.physics_prim_path}'.") @@ -331,10 +336,19 @@ def instance(cls) -> SimulationContext: def clear_instance(cls) -> None: """Delete the simulation context singleton instance.""" if cls._instance is not None: + # check if the instance is initialized + if not cls._is_initialized: + logger.warning("Simulation context is not initialized. Unable to clear the instance.") + return # clear the callback if cls._instance._app_control_on_stop_handle is not None: cls._instance._app_control_on_stop_handle.unsubscribe() cls._instance._app_control_on_stop_handle = None + # detach the stage from the USD stage cache + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() + if stage_id > 0: + stage_cache.Erase(cls._instance._initial_stage) # clear the instance and the flag cls._instance = None cls._is_initialized = False From a2f6548cab4e5996e6e1d20e3a040539c76e4f48 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 15:16:27 +0100 Subject: [PATCH 067/100] adds todos --- source/isaaclab/isaaclab/sim/simulation_context.py | 1 + 1 file changed, 1 insertion(+) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 5c2087711ea..bfdd89a4cf2 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -665,6 +665,7 @@ def reset(self, soft: bool = False): self._physics_sim_view = omni.physics.tensors.create_simulation_view("torch", stage_id=self._initial_stage_id) self._physics_sim_view.set_subspace_roots("/") self._physx_iface.update_simulation(self.cfg.dt, 0.0) + # TODO: Remove these once we don't rely on Isaac Sim internals. SimulationManager._message_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) SimulationManager._simulation_view_created = True SimulationManager._physics_sim_view = self._physics_sim_view From 1d8d50a2e440ed139fa193d3adfb15ace834b851 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 15:43:34 +0100 Subject: [PATCH 068/100] adds more tests for sim context operations --- .../isaaclab/sim/simulation_context.py | 23 +- .../test/sim/test_simulation_context.py | 397 +++++++++++++++--- 2 files changed, 346 insertions(+), 74 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index bfdd89a4cf2..c1def749869 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -270,6 +270,8 @@ def __init__(self, cfg: SimulationCfg | None = None): self._physx_iface = omni.physx.get_physx_interface() self._physx_sim_iface = omni.physx.get_physx_simulation_interface() self._timeline_iface = omni.timeline.get_timeline_interface() + # create interfaces that are initialized on 'reset + self._physics_sim_view = None # set timeline auto update to True self._timeline_iface.set_auto_update(True) @@ -314,7 +316,7 @@ def __new__(cls, *args, **kwargs) -> SimulationContext: if cls._instance is None: cls._instance = super().__new__(cls) else: - logger.info("Returning the previously defined instance of Simulation Context.") + logger.debug("Returning the previously defined instance of Simulation Context.") return cls._instance # type: ignore """ @@ -700,17 +702,11 @@ def step(self, render: bool = True): render: Whether to render the scene after stepping the physics simulation. If set to False, the scene is not rendered and only the physics simulation is stepped. """ - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise - # update anim recording if needed if self._anim_recording_enabled: is_anim_recording_finished = self._update_anim_recording() if is_anim_recording_finished: - logger.warning("[INFO][SimulationContext]: Animation recording finished. Closing app.") + logger.warning("Animation recording finished. Closing app.") self._app_iface.shutdown() # check if the simulation timeline is paused. in that case keep stepping until it is playing @@ -731,8 +727,8 @@ def step(self, render: bool = True): if render: self._app_iface.update() else: - self._physics_sim_view.simulate(self.cfg.dt, 0.0) - self._physics_sim_view.fetch_results() + self._physx_sim_iface.simulate(self.cfg.dt, 0.0) + self._physx_sim_iface.fetch_results() # app.update() may be changing the cuda device in step, so we force it back to our desired device here if "cuda" in self.device: @@ -750,11 +746,6 @@ def render(self, mode: RenderMode | None = None): Args: mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. """ - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise # check if we need to change the render mode if mode is not None: self.set_render_mode(mode) @@ -1101,6 +1092,8 @@ def _load_fabric_interface(self): else: if fabric_enabled: extension_manager.set_extension_enabled_immediate("omni.physx.fabric", False) + # set fabric interface to None + self._fabric_iface = None # set carb settings for fabric self.carb_settings.set_bool("/physics/updateToUsd", not self.cfg.use_fabric) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 7411abe7166..b2e13e872a2 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -13,9 +13,11 @@ """Rest everything follows.""" import numpy as np +import torch import pytest +import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -32,6 +34,61 @@ def test_setup_teardown(): SimulationContext.clear_instance() +""" +Basic Configuration Tests +""" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_init(device): + """Test the simulation context initialization.""" + cfg = SimulationCfg(physics_prim_path="/Physics/PhysX", render_interval=5, gravity=(0.0, -0.5, -0.5), device=device) + # sim = SimulationContext(cfg) + # TODO: Figure out why keyword argument doesn't work. + # note: added a fix in Isaac Sim 2023.1 for this. + sim = SimulationContext(cfg=cfg) + + # verify app interface is valid + assert sim.app is not None + # verify stage is valid + assert sim.stage is not None + # verify device property + assert sim.device == device + # verify no RTX sensors are available + assert not sim.has_rtx_sensors() + + # obtain physics scene api + physx_scene_api = sim._physx_scene_api # type: ignore + physics_scene = sim._physics_scene # type: ignore + + # check valid settings + physics_hz = physx_scene_api.GetTimeStepsPerSecondAttr().Get() + physics_dt = 1.0 / physics_hz + assert physics_dt == cfg.dt + + # check valid paths + assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() + assert sim.stage.GetPrimAtPath("/Physics/PhysX/defaultMaterial").IsValid() + # check valid gravity + gravity_dir, gravity_mag = ( + physics_scene.GetGravityDirectionAttr().Get(), + physics_scene.GetGravityMagnitudeAttr().Get(), + ) + gravity = np.array(gravity_dir) * gravity_mag + np.testing.assert_almost_equal(gravity, cfg.gravity) + + +@pytest.mark.isaacsim_ci +def test_instance_before_creation(): + """Test accessing instance before creating returns None.""" + # clear any existing instance + SimulationContext.clear_instance() + + # accessing instance before creation should return None + assert SimulationContext.instance() is None + + @pytest.mark.isaacsim_ci def test_singleton(): """Tests that the singleton is working.""" @@ -50,32 +107,9 @@ def test_singleton(): sim3.clear_instance() -@pytest.mark.isaacsim_ci -def test_initialization(): - """Test the simulation config.""" - cfg = SimulationCfg(physics_prim_path="/Physics/PhysX", render_interval=5, gravity=(0.0, -0.5, -0.5)) - sim = SimulationContext(cfg) - # TODO: Figure out why keyword argument doesn't work. - # note: added a fix in Isaac Sim 2023.1 for this. - # sim = SimulationContext(cfg=cfg) - - # check valid settings - physics_hz = sim._physx_scene_api.GetTimeStepsPerSecondAttr().Get() - physics_dt = 1.0 / physics_hz - rendering_dt = cfg.dt * cfg.render_interval - assert physics_dt == cfg.dt - assert rendering_dt == cfg.dt * cfg.render_interval - assert not sim.has_rtx_sensors() - # check valid paths - assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() - assert sim.stage.GetPrimAtPath("/Physics/PhysX/defaultMaterial").IsValid() - # check valid gravity - gravity_dir, gravity_mag = ( - sim._physics_scene.GetGravityDirectionAttr().Get(), - sim._physics_scene.GetGravityMagnitudeAttr().Get(), - ) - gravity = np.array(gravity_dir) * gravity_mag - np.testing.assert_almost_equal(gravity, cfg.gravity) +""" +Property Tests. +""" @pytest.mark.isaacsim_ci @@ -105,47 +139,292 @@ def test_headless_mode(): sim = SimulationContext() # check default render mode assert sim.render_mode == sim.RenderMode.NO_GUI_OR_RENDERING + assert not sim.has_gui() -# def test_boundedness(): -# """Test that the boundedness of the simulation context remains constant. -# -# Note: This test fails right now because Isaac Sim does not handle boundedness correctly. On creation, -# it is registering itself to various callbacks and hence the boundedness is more than 1. This may not be -# critical for the simulation context since we usually call various clear functions before deleting the -# simulation context. -# """ -# sim = SimulationContext() -# # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. -# -# sim._stage_open_callback = None -# sim._physics_timer_callback = None -# sim._event_timer_callback = None -# -# # check that boundedness of simulation context is correct -# sim_ref_count = ctypes.c_long.from_address(id(sim)).value -# # reset the simulation -# sim.reset() -# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count -# # step the simulation -# for _ in range(10): -# sim.step() -# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count -# # clear the simulation -# sim.clear_instance() -# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - 1 +""" +Timeline Operations Tests. +""" @pytest.mark.isaacsim_ci -def test_zero_gravity(): - """Test that gravity can be properly disabled.""" - cfg = SimulationCfg(gravity=(0.0, 0.0, 0.0)) +def test_timeline_play_stop(): + """Test timeline play and stop operations.""" + sim = SimulationContext() + + # initially simulation should be stopped + assert sim.is_stopped() + assert not sim.is_playing() + + # start the simulation + sim.play() + assert sim.is_playing() + assert not sim.is_stopped() + + # disable callback to prevent app from continuing + sim._disable_app_control_on_stop_handle = True # type: ignore + # stop the simulation + sim.stop() + assert sim.is_stopped() + assert not sim.is_playing() + + +@pytest.mark.isaacsim_ci +def test_timeline_pause(): + """Test timeline pause operation.""" + sim = SimulationContext() + # start the simulation + sim.play() + assert sim.is_playing() + + # pause the simulation + sim.pause() + assert not sim.is_playing() + assert not sim.is_stopped() # paused is different from stopped + + +""" +Reset and Step Tests +""" + + +@pytest.mark.isaacsim_ci +def test_reset(): + """Test simulation reset.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create a simple cube to test with + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # reset the simulation + sim.reset() + + # check that simulation is playing after reset + assert sim.is_playing() + + # check that physics sim view is created + assert sim.physics_sim_view is not None + + +@pytest.mark.isaacsim_ci +def test_reset_soft(): + """Test soft reset (without stopping simulation).""" + cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) + # create a simple cube + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # perform initial reset + sim.reset() + assert sim.is_playing() + + # perform soft reset + sim.reset(soft=True) + + # simulation should still be playing + assert sim.is_playing() + + +@pytest.mark.isaacsim_ci +def test_forward(): + """Test forward propagation for fabric updates.""" + cfg = SimulationCfg(dt=0.01, use_fabric=True) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # call forward + sim.forward() + + # should not raise any errors + assert sim.is_playing() + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("render", [True, False]) +def test_step(render): + """Test stepping simulation with and without rendering.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # step with rendering + for _ in range(10): + sim.step(render=render) + + # simulation should still be playing + assert sim.is_playing() + + +@pytest.mark.isaacsim_ci +def test_render(): + """Test rendering simulation.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # render + for _ in range(10): + sim.render() + + # simulation should still be playing + assert sim.is_playing() + + +""" +Stage Operations Tests +""" + + +@pytest.mark.isaacsim_ci +def test_get_initial_stage(): + """Test getting the initial stage.""" + sim = SimulationContext() + + # get initial stage + stage = sim.get_initial_stage() + + # verify stage is valid + assert stage is not None + assert stage == sim.stage + + +@pytest.mark.isaacsim_ci +def test_clear_stage(): + """Test clearing the stage.""" + sim = SimulationContext() + + # create some objects + cube_cfg1 = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg1.func("/World/Cube1", cube_cfg1) + cube_cfg2 = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg2.func("/World/Cube2", cube_cfg2) + + # verify objects exist + assert sim.stage.GetPrimAtPath("/World/Cube1").IsValid() + assert sim.stage.GetPrimAtPath("/World/Cube2").IsValid() + + # clear the stage + sim.clear() + + # verify objects are removed but World and Physics remain + assert not sim.stage.GetPrimAtPath("/World/Cube1").IsValid() + assert not sim.stage.GetPrimAtPath("/World/Cube2").IsValid() + assert sim.stage.GetPrimAtPath("/World").IsValid() + assert sim.stage.GetPrimAtPath(sim.cfg.physics_prim_path).IsValid() + + +""" +Physics Configuration Tests +""" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("solver_type", [0, 1]) # 0=PGS, 1=TGS +def test_solver_type(solver_type): + """Test different solver types.""" + from isaaclab.sim.simulation_cfg import PhysxCfg + + cfg = SimulationCfg(physx=PhysxCfg(solver_type=solver_type)) + sim = SimulationContext(cfg) + + # obtain physics scene api + physx_scene_api = sim._physx_scene_api # type: ignore + # check solver type is set + solver_type_str = "PGS" if solver_type == 0 else "TGS" + assert physx_scene_api.GetSolverTypeAttr().Get() == solver_type_str + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("use_fabric", [True, False]) +def test_fabric_setting(use_fabric): + """Test that fabric setting is properly set.""" + cfg = SimulationCfg(use_fabric=use_fabric) + sim = SimulationContext(cfg) + + # check fabric is enabled + assert sim.is_fabric_enabled() == use_fabric + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("dt", [0.01, 0.02, 0.005]) +def test_physics_dt(dt): + """Test that physics time step is properly configured.""" + cfg = SimulationCfg(dt=dt) + sim = SimulationContext(cfg) + + # obtain physics scene api + physx_scene_api = sim._physx_scene_api # type: ignore + # check physics dt + physics_hz = physx_scene_api.GetTimeStepsPerSecondAttr().Get() + physics_dt = 1.0 / physics_hz + assert abs(physics_dt - dt) < 1e-6 + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("gravity", [(0.0, 0.0, 0.0), (0.0, 0.0, -9.81), (0.5, 0.5, 0.5)]) +def test_custom_gravity(gravity): + """Test that gravity can be properly set.""" + cfg = SimulationCfg(gravity=gravity) + sim = SimulationContext(cfg) + + # obtain physics scene api + physics_scene = sim._physics_scene # type: ignore + gravity_dir, gravity_mag = ( - sim._physics_scene.GetGravityDirectionAttr().Get(), - sim._physics_scene.GetGravityMagnitudeAttr().Get(), + physics_scene.GetGravityDirectionAttr().Get(), + physics_scene.GetGravityMagnitudeAttr().Get(), ) gravity = np.array(gravity_dir) * gravity_mag - np.testing.assert_almost_equal(gravity, cfg.gravity) + np.testing.assert_almost_equal(gravity, cfg.gravity, decimal=6) + + +""" +Edge Cases and Error Handling +""" + + +def test_boundedness(): + """Test that the boundedness of the simulation context remains constant. + + Note: This test fails right now because Isaac Sim does not handle boundedness correctly. On creation, + it is registering itself to various callbacks and hence the boundedness is more than 1. This may not be + critical for the simulation context since we usually call various clear functions before deleting the + simulation context. + """ + import ctypes + + sim = SimulationContext() + # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. + + # check that boundedness of simulation context is correct + sim_ref_count = ctypes.c_long.from_address(id(sim)).value + # reset the simulation + sim.reset() + assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count + # step the simulation + for _ in range(10): + sim.step() + assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count + # clear the simulation + sim.clear_instance() + assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - 1 From 832db86a14ce83af728269ed5daad43e8814e960 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 15:51:56 +0100 Subject: [PATCH 069/100] remove boundedness as that test is wrong --- .../test/sim/test_simulation_context.py | 33 ------------------- 1 file changed, 33 deletions(-) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index b2e13e872a2..614c1d2dcb2 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -13,7 +13,6 @@ """Rest everything follows.""" import numpy as np -import torch import pytest @@ -396,35 +395,3 @@ def test_custom_gravity(gravity): ) gravity = np.array(gravity_dir) * gravity_mag np.testing.assert_almost_equal(gravity, cfg.gravity, decimal=6) - - -""" -Edge Cases and Error Handling -""" - - -def test_boundedness(): - """Test that the boundedness of the simulation context remains constant. - - Note: This test fails right now because Isaac Sim does not handle boundedness correctly. On creation, - it is registering itself to various callbacks and hence the boundedness is more than 1. This may not be - critical for the simulation context since we usually call various clear functions before deleting the - simulation context. - """ - import ctypes - - sim = SimulationContext() - # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. - - # check that boundedness of simulation context is correct - sim_ref_count = ctypes.c_long.from_address(id(sim)).value - # reset the simulation - sim.reset() - assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - # step the simulation - for _ in range(10): - sim.step() - assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - # clear the simulation - sim.clear_instance() - assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - 1 From fae3e0c03b2ff511d5255b391023f10acba9bd96 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 16:13:38 +0100 Subject: [PATCH 070/100] adds get physics dt for funsies --- source/isaaclab/isaaclab/sim/simulation_context.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index c1def749869..07e22f31d07 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -455,6 +455,14 @@ def get_initial_stage(self) -> Usd.Stage: """ return self._initial_stage + def get_physics_dt(self) -> float: + """Returns the physics time step of the simulation. + + Returns: + The physics time step of the simulation. + """ + return self.cfg.dt + """ Operations - Utilities. """ From 3b6fb8aee0eee4d3ea50a040c56585170660c7b2 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 16:26:25 +0100 Subject: [PATCH 071/100] adds callback tests --- .../test/sim/test_simulation_context.py | 347 ++++++++++++++++++ 1 file changed, 347 insertions(+) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 614c1d2dcb2..dea8e21dc06 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -395,3 +395,350 @@ def test_custom_gravity(gravity): ) gravity = np.array(gravity_dir) * gravity_mag np.testing.assert_almost_equal(gravity, cfg.gravity, decimal=6) + + +""" +Callback Tests. +""" + + +@pytest.mark.isaacsim_ci +def test_timeline_callbacks_on_play(): + """Test that timeline callbacks are triggered on play event.""" + import omni.timeline + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create a simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a flag to track callback execution + callback_state = {"play_called": False, "stop_called": False} + + # define callback functions + def on_play_callback(event): + callback_state["play_called"] = True + + def on_stop_callback(event): + callback_state["stop_called"] = True + + # register callbacks + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event: on_play_callback(event), + order=20, + ) + stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event: on_stop_callback(event), + order=20, + ) + + try: + # ensure callbacks haven't been called yet + assert not callback_state["play_called"] + assert not callback_state["stop_called"] + + # play the simulation - this should trigger play callback + sim.play() + assert callback_state["play_called"] + assert not callback_state["stop_called"] + + # reset flags + callback_state["play_called"] = False + + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # stop the simulation - this should trigger stop callback + sim.stop() + assert callback_state["stop_called"] + + finally: + # cleanup callbacks + if play_handle is not None: + play_handle.unsubscribe() + if stop_handle is not None: + stop_handle.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_timeline_callbacks_with_weakref(): + """Test that timeline callbacks work correctly with weak references (similar to asset_base.py).""" + import weakref + + import omni.timeline + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create a simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a test object that will be weakly referenced + class CallbackTracker: + def __init__(self): + self.play_count = 0 + self.stop_count = 0 + + def on_play(self, event): + self.play_count += 1 + + def on_stop(self, event): + self.stop_count += 1 + + # create an instance of the callback tracker + tracker = CallbackTracker() + + # define safe callback wrapper (similar to asset_base.py pattern) + def safe_callback(callback_name, event, obj_ref): + """Safely invoke a callback on a weakly-referenced object.""" + try: + obj = obj_ref() # Dereference the weakref + if obj is not None: + getattr(obj, callback_name)(event) + except ReferenceError: + # Object has been deleted; ignore + pass + + # register callbacks with weakref + obj_ref = weakref.ref(tracker) + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj_ref=obj_ref: safe_callback("on_play", event, obj_ref), + order=20, + ) + stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj_ref=obj_ref: safe_callback("on_stop", event, obj_ref), + order=20, + ) + + try: + # verify callbacks haven't been called + assert tracker.play_count == 0 + assert tracker.stop_count == 0 + + # trigger play event + sim.play() + assert tracker.play_count == 1 + assert tracker.stop_count == 0 + + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # trigger stop event + sim.stop() + assert tracker.play_count == 1 + assert tracker.stop_count == 1 + + # delete the tracker object + del tracker + + # trigger events again - callbacks should handle the deleted object gracefully + sim.play() + # disable app control again + sim._disable_app_control_on_stop_handle = True # type: ignore + sim.stop() + # should not raise any errors + + finally: + # cleanup callbacks + if play_handle is not None: + play_handle.unsubscribe() + if stop_handle is not None: + stop_handle.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_multiple_callbacks_on_same_event(): + """Test that multiple callbacks can be registered for the same event.""" + import omni.timeline + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create tracking for multiple callbacks + callback_counts = {"callback1": 0, "callback2": 0, "callback3": 0} + + def callback1(event): + callback_counts["callback1"] += 1 + + def callback2(event): + callback_counts["callback2"] += 1 + + def callback3(event): + callback_counts["callback3"] += 1 + + # register multiple callbacks for play event + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + handle1 = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback1(event), order=20 + ) + handle2 = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback2(event), order=21 + ) + handle3 = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback3(event), order=22 + ) + + try: + # verify none have been called + assert all(count == 0 for count in callback_counts.values()) + + # trigger play event + sim.play() + + # all callbacks should have been called + assert callback_counts["callback1"] == 1 + assert callback_counts["callback2"] == 1 + assert callback_counts["callback3"] == 1 + + finally: + # cleanup all callbacks + if handle1 is not None: + handle1.unsubscribe() + if handle2 is not None: + handle2.unsubscribe() + if handle3 is not None: + handle3.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_callback_execution_order(): + """Test that callbacks are executed in the correct order based on priority.""" + import omni.timeline + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # track execution order + execution_order = [] + + def callback_low_priority(event): + execution_order.append("low") + + def callback_medium_priority(event): + execution_order.append("medium") + + def callback_high_priority(event): + execution_order.append("high") + + # register callbacks with different priorities (lower order = higher priority) + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + handle_high = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback_high_priority(event), order=5 + ) + handle_medium = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback_medium_priority(event), order=10 + ) + handle_low = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback_low_priority(event), order=15 + ) + + try: + # trigger play event + sim.play() + + # verify callbacks were executed in correct order + assert len(execution_order) == 3 + assert execution_order[0] == "high" + assert execution_order[1] == "medium" + assert execution_order[2] == "low" + + finally: + # cleanup callbacks + if handle_high is not None: + handle_high.unsubscribe() + if handle_medium is not None: + handle_medium.unsubscribe() + if handle_low is not None: + handle_low.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_callback_unsubscribe(): + """Test that unsubscribing callbacks works correctly.""" + import omni.timeline + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback counter + callback_count = {"count": 0} + + def on_play_callback(event): + callback_count["count"] += 1 + + # register callback + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: on_play_callback(event), order=20 + ) + + try: + # trigger play event + sim.play() + assert callback_count["count"] == 1 + + # stop simulation + sim._disable_app_control_on_stop_handle = True # type: ignore + sim.stop() + + # unsubscribe the callback + play_handle.unsubscribe() + play_handle = None + + # trigger play event again + sim.play() + + # callback should not have been called again (still 1) + assert callback_count["count"] == 1 + + finally: + # cleanup if needed + if play_handle is not None: + play_handle.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_pause_event_callback(): + """Test that pause event callbacks are triggered correctly.""" + import omni.timeline + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback tracker + callback_state = {"pause_called": False} + + def on_pause_callback(event): + callback_state["pause_called"] = True + + # register pause callback + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + pause_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PAUSE), lambda event: on_pause_callback(event), order=20 + ) + + try: + # play the simulation first + sim.play() + assert not callback_state["pause_called"] + + # pause the simulation + sim.pause() + + # callback should have been triggered + assert callback_state["pause_called"] + + finally: + # cleanup + if pause_handle is not None: + pause_handle.unsubscribe() From e77de12c829a562a763318d2d828d0e43f000ffb Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 16:27:52 +0100 Subject: [PATCH 072/100] fixes imports --- .../isaaclab/test/sim/test_simulation_context.py | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index dea8e21dc06..ed07ae0cc75 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -13,7 +13,9 @@ """Rest everything follows.""" import numpy as np +import weakref +import omni.timeline import pytest import isaaclab.sim as sim_utils @@ -405,8 +407,6 @@ def test_custom_gravity(gravity): @pytest.mark.isaacsim_ci def test_timeline_callbacks_on_play(): """Test that timeline callbacks are triggered on play event.""" - import omni.timeline - cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) @@ -468,9 +468,6 @@ def on_stop_callback(event): @pytest.mark.isaacsim_ci def test_timeline_callbacks_with_weakref(): """Test that timeline callbacks work correctly with weak references (similar to asset_base.py).""" - import weakref - - import omni.timeline cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) @@ -559,8 +556,6 @@ def safe_callback(callback_name, event, obj_ref): @pytest.mark.isaacsim_ci def test_multiple_callbacks_on_same_event(): """Test that multiple callbacks can be registered for the same event.""" - import omni.timeline - cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) @@ -613,8 +608,6 @@ def callback3(event): @pytest.mark.isaacsim_ci def test_callback_execution_order(): """Test that callbacks are executed in the correct order based on priority.""" - import omni.timeline - cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) @@ -665,8 +658,6 @@ def callback_high_priority(event): @pytest.mark.isaacsim_ci def test_callback_unsubscribe(): """Test that unsubscribing callbacks works correctly.""" - import omni.timeline - cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) @@ -710,8 +701,6 @@ def on_play_callback(event): @pytest.mark.isaacsim_ci def test_pause_event_callback(): """Test that pause event callbacks are triggered correctly.""" - import omni.timeline - cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) From f1afe763f053fc5c3838526f06db219f79d21ecb Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 16:59:19 +0100 Subject: [PATCH 073/100] fixes reset pausing --- source/isaaclab/isaaclab/sim/simulation_context.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 07e22f31d07..df9bd267094 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -306,6 +306,11 @@ def __init__(self, cfg: SimulationCfg | None = None): # load flatcache/fabric interface self._load_fabric_interface() + # attach stage to physx + current_attached_stage = self._physx_sim_iface.get_attached_stage() + if current_attached_stage != self._initial_stage_id: + self._physx_sim_iface.attach_stage(self._initial_stage_id) + def __new__(cls, *args, **kwargs) -> SimulationContext: """Returns the instance of the simulation context. @@ -346,6 +351,9 @@ def clear_instance(cls) -> None: if cls._instance._app_control_on_stop_handle is not None: cls._instance._app_control_on_stop_handle.unsubscribe() cls._instance._app_control_on_stop_handle = None + # detach the stage from physx + if cls._instance._physx_sim_iface is not None: + cls._instance._physx_sim_iface.detach_stage() # detach the stage from the USD stage cache stage_cache = UsdUtils.StageCache.Get() stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() @@ -657,8 +665,12 @@ def reset(self, soft: bool = False): # reset the simulation if not soft: + # disable app control on stop handle + self._disable_app_control_on_stop_handle = True if not self.is_stopped(): self.stop() + self._disable_app_control_on_stop_handle = False + # play the simulation self.play() # initialize the physics simulation self._physx_iface.force_load_physics_from_usd() From b0a2949ba468a72ed84f75a337c21e6725de4281 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 17:00:33 +0100 Subject: [PATCH 074/100] adds test for isaac sim events --- .../isaaclab/sim/simulation_context.py | 39 ++- .../test/sim/test_simulation_context.py | 327 ++++++++++++++++++ 2 files changed, 348 insertions(+), 18 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index df9bd267094..8edefd0090b 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -306,11 +306,6 @@ def __init__(self, cfg: SimulationCfg | None = None): # load flatcache/fabric interface self._load_fabric_interface() - # attach stage to physx - current_attached_stage = self._physx_sim_iface.get_attached_stage() - if current_attached_stage != self._initial_stage_id: - self._physx_sim_iface.attach_stage(self._initial_stage_id) - def __new__(cls, *args, **kwargs) -> SimulationContext: """Returns the instance of the simulation context. @@ -673,25 +668,33 @@ def reset(self, soft: bool = False): # play the simulation self.play() # initialize the physics simulation - self._physx_iface.force_load_physics_from_usd() - self._physx_iface.start_simulation() - self._physx_iface.update_simulation(self.cfg.dt, 0.0) - self._physx_sim_iface.fetch_results() - SimulationManager._message_bus.dispatch_event(IsaacEvents.PHYSICS_WARMUP.value, payload={}) + SimulationManager.initialize_physics() + # self._physx_iface.force_load_physics_from_usd() + # self._physx_iface.start_simulation() + # self._physx_iface.update_simulation(self.cfg.dt, 0.0) + # # attach stage to physx + # current_attached_stage = self._physx_sim_iface.get_attached_stage() + # if current_attached_stage != self._initial_stage_id: + # self._physx_sim_iface.attach_stage(self._initial_stage_id) + # # fetch results + # self._physx_sim_iface.fetch_results() + # self._message_bus.dispatch_event(IsaacEvents.PHYSICS_WARMUP.value, payload={}) # app.update() may be changing the cuda device in reset, so we force it back to our desired device here if "cuda" in self.device: torch.cuda.set_device(self.device) # create the simulation view - self._physics_sim_view = omni.physics.tensors.create_simulation_view("torch", stage_id=self._initial_stage_id) - self._physics_sim_view.set_subspace_roots("/") - self._physx_iface.update_simulation(self.cfg.dt, 0.0) - # TODO: Remove these once we don't rely on Isaac Sim internals. - SimulationManager._message_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) - SimulationManager._simulation_view_created = True - SimulationManager._physics_sim_view = self._physics_sim_view - SimulationManager._message_bus.dispatch_event(IsaacEvents.PHYSICS_READY.value, payload={}) + # SimulationManager._create_simulation_view(None) + # self._physics_sim_view = omni.physics.tensors.create_simulation_view("torch", stage_id=self._initial_stage_id) + # self._physics_sim_view.set_subspace_roots("/") + # self._physx_iface.update_simulation(self.cfg.dt, 0.0) + # # TODO: Remove these once we don't rely on Isaac Sim internals. + # self._message_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) + # SimulationManager._simulation_view_created = True + # SimulationManager._physics_sim_view = self._physics_sim_view + # self._message_bus.dispatch_event(IsaacEvents.PHYSICS_READY.value, payload={}) + self._physics_sim_view = SimulationManager.get_physics_sim_view() # enable kinematic rendering with fabric if self._physics_sim_view: diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index ed07ae0cc75..916ebc89066 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -17,6 +17,7 @@ import omni.timeline import pytest +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -731,3 +732,329 @@ def on_pause_callback(event): # cleanup if pause_handle is not None: pause_handle.unsubscribe() + + +""" +Isaac Events Callback Tests. +""" + + +@pytest.mark.isaacsim_ci +def test_isaac_event_physics_warmup(): + """Test that PHYSICS_WARMUP Isaac event is triggered during reset.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create callback tracker + callback_state = {"warmup_called": False} + + def on_physics_warmup(event): + callback_state["warmup_called"] = True + + # register callback for PHYSICS_WARMUP event + callback_id = SimulationManager.register_callback( + lambda event: on_physics_warmup(event), event=IsaacEvents.PHYSICS_WARMUP + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["warmup_called"] + + # reset the simulation - should trigger PHYSICS_WARMUP + sim.reset() + + # verify callback was triggered + assert callback_state["warmup_called"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_simulation_view_created(): + """Test that SIMULATION_VIEW_CREATED Isaac event is triggered during reset.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create callback tracker + callback_state = {"view_created_called": False} + + def on_simulation_view_created(event): + callback_state["view_created_called"] = True + + # register callback for SIMULATION_VIEW_CREATED event + callback_id = SimulationManager.register_callback( + lambda event: on_simulation_view_created(event), event=IsaacEvents.SIMULATION_VIEW_CREATED + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["view_created_called"] + + # reset the simulation - should trigger SIMULATION_VIEW_CREATED + sim.reset() + + # verify callback was triggered + assert callback_state["view_created_called"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_physics_ready(): + """Test that PHYSICS_READY Isaac event is triggered during reset.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create callback tracker + callback_state = {"physics_ready_called": False} + + def on_physics_ready(event): + callback_state["physics_ready_called"] = True + + # register callback for PHYSICS_READY event + callback_id = SimulationManager.register_callback( + lambda event: on_physics_ready(event), event=IsaacEvents.PHYSICS_READY + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["physics_ready_called"] + + # reset the simulation - should trigger PHYSICS_READY + sim.reset() + + # verify callback was triggered + assert callback_state["physics_ready_called"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_prim_deletion(): + """Test that PRIM_DELETION Isaac event is triggered when a prim is deleted.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # create callback tracker + callback_state = {"prim_deleted": False, "deleted_path": None} + + def on_prim_deletion(event): + callback_state["prim_deleted"] = True + # event payload should contain the deleted prim path + if hasattr(event, "payload") and event.payload: + callback_state["deleted_path"] = event.payload.get("prim_path", None) + + # register callback for PRIM_DELETION event + callback_id = SimulationManager.register_callback( + lambda event: on_prim_deletion(event), event=IsaacEvents.PRIM_DELETION + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["prim_deleted"] + + # delete the cube prim + sim_utils.delete_prim("/World/Cube") + + # trigger the event by dispatching it manually (since deletion might be handled differently) + SimulationManager._message_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": "/World/Cube"}) # type: ignore + + # verify callback was triggered + assert callback_state["prim_deleted"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_timeline_stop(): + """Test that TIMELINE_STOP Isaac event can be registered and triggered.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback tracker + callback_state = {"timeline_stop_called": False} + + def on_timeline_stop(event): + callback_state["timeline_stop_called"] = True + + # register callback for TIMELINE_STOP event + callback_id = SimulationManager.register_callback( + lambda event: on_timeline_stop(event), event=IsaacEvents.TIMELINE_STOP + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["timeline_stop_called"] + + # play and stop the simulation + sim.play() + + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # stop the simulation + sim.stop() + + # dispatch the event manually + SimulationManager._message_bus.dispatch_event(IsaacEvents.TIMELINE_STOP.value, payload={}) # type: ignore + + # verify callback was triggered + assert callback_state["timeline_stop_called"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_callbacks_with_weakref(): + """Test Isaac event callbacks with weak references (similar to asset_base.py pattern).""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a test object that will be weakly referenced + class PhysicsTracker: + def __init__(self): + self.warmup_count = 0 + self.ready_count = 0 + + def on_warmup(self, event): + self.warmup_count += 1 + + def on_ready(self, event): + self.ready_count += 1 + + tracker = PhysicsTracker() + + # define safe callback wrapper (same pattern as asset_base.py) + def safe_callback(callback_name, event, obj_ref): + """Safely invoke a callback on a weakly-referenced object.""" + try: + obj = obj_ref() + if obj is not None: + getattr(obj, callback_name)(event) + except ReferenceError: + # Object has been deleted; ignore + pass + + # register callbacks with weakref + obj_ref = weakref.ref(tracker) + + warmup_id = SimulationManager.register_callback( + lambda event, obj_ref=obj_ref: safe_callback("on_warmup", event, obj_ref), + event=IsaacEvents.PHYSICS_WARMUP, + ) + ready_id = SimulationManager.register_callback( + lambda event, obj_ref=obj_ref: safe_callback("on_ready", event, obj_ref), event=IsaacEvents.PHYSICS_READY + ) + + try: + # verify callbacks haven't been called + assert tracker.warmup_count == 0 + assert tracker.ready_count == 0 + + # reset simulation - triggers WARMUP and READY events + sim.reset() + + # verify callbacks were triggered + assert tracker.warmup_count == 1 + assert tracker.ready_count == 1 + + # delete the tracker object + del tracker + + # reset again - callbacks should handle the deleted object gracefully + sim.reset(soft=True) + + # should not raise any errors even though tracker is deleted + + finally: + # cleanup callbacks + if warmup_id is not None: + SimulationManager.deregister_callback(warmup_id) + if ready_id is not None: + SimulationManager.deregister_callback(ready_id) + + +@pytest.mark.isaacsim_ci +def test_multiple_isaac_event_callbacks(): + """Test that multiple callbacks can be registered for the same Isaac event.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create tracking for multiple callbacks + callback_counts = {"callback1": 0, "callback2": 0, "callback3": 0} + + def callback1(event): + callback_counts["callback1"] += 1 + + def callback2(event): + callback_counts["callback2"] += 1 + + def callback3(event): + callback_counts["callback3"] += 1 + + # register multiple callbacks for PHYSICS_READY event + id1 = SimulationManager.register_callback(lambda event: callback1(event), event=IsaacEvents.PHYSICS_READY) + id2 = SimulationManager.register_callback(lambda event: callback2(event), event=IsaacEvents.PHYSICS_READY) + id3 = SimulationManager.register_callback(lambda event: callback3(event), event=IsaacEvents.PHYSICS_READY) + + try: + # verify none have been called + assert all(count == 0 for count in callback_counts.values()) + + # reset simulation - triggers PHYSICS_READY event + sim.reset() + + # all callbacks should have been called + assert callback_counts["callback1"] == 1 + assert callback_counts["callback2"] == 1 + assert callback_counts["callback3"] == 1 + + finally: + # cleanup all callbacks + if id1 is not None: + SimulationManager.deregister_callback(id1) + if id2 is not None: + SimulationManager.deregister_callback(id2) + if id3 is not None: + SimulationManager.deregister_callback(id3) From 567c10d453c89d6298e12cfb20b21a2858f37eac Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 17:37:59 +0100 Subject: [PATCH 075/100] makes logger file more visible --- source/isaaclab/isaaclab/utils/logger.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/utils/logger.py b/source/isaaclab/isaaclab/utils/logger.py index 27ce59f808a..db93f026290 100644 --- a/source/isaaclab/isaaclab/utils/logger.py +++ b/source/isaaclab/isaaclab/utils/logger.py @@ -94,8 +94,14 @@ def configure_logging( file_handler.setFormatter(file_formatter) root_logger.addHandler(file_handler) - # print the log file path once at startup - print(f"[INFO] [IsaacLab] Logging to file: {log_file_path}") + # print the log file path once at startup with nice formatting + border = "=" * 85 + cyan = "\033[36m" # cyan color + bold = "\033[1m" # bold text + reset = "\033[0m" # reset formatting + print(f"\n{cyan}{border}{reset}") + print(f"{cyan}{bold}[INFO][IsaacLab]: Logging to file: {log_file_path}{reset}") + print(f"{cyan}{border}{reset}\n") # return the root logger return root_logger From 2a4298507e3be619726b33ccaaa343ee793f1ec6 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 17:38:34 +0100 Subject: [PATCH 076/100] typo --- source/isaaclab/isaaclab/utils/logger.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/utils/logger.py b/source/isaaclab/isaaclab/utils/logger.py index db93f026290..a3dfd7019ee 100644 --- a/source/isaaclab/isaaclab/utils/logger.py +++ b/source/isaaclab/isaaclab/utils/logger.py @@ -95,7 +95,7 @@ def configure_logging( root_logger.addHandler(file_handler) # print the log file path once at startup with nice formatting - border = "=" * 85 + border = "=" * 86 cyan = "\033[36m" # cyan color bold = "\033[1m" # bold text reset = "\033[0m" # reset formatting From 68bb03056b802f81c48812ade1b7f893cdd9d33c Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 17:59:59 +0100 Subject: [PATCH 077/100] fixes info not logging to logger --- source/isaaclab/isaaclab/utils/logger.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/utils/logger.py b/source/isaaclab/isaaclab/utils/logger.py index a3dfd7019ee..c0ea26b6074 100644 --- a/source/isaaclab/isaaclab/utils/logger.py +++ b/source/isaaclab/isaaclab/utils/logger.py @@ -58,7 +58,8 @@ def configure_logging( The root logger. """ root_logger = logging.getLogger() - root_logger.setLevel(logging_level) + # the root logger must be the lowest level to ensure that all messages are logged + root_logger.setLevel(logging.DEBUG) # remove existing handlers # Note: iterate over a copy [:] to avoid modifying list during iteration From eed13055368dd11a1c0ec798d86c7eb08e0b2a9a Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:00:35 +0100 Subject: [PATCH 078/100] makes formatter nicer --- .../assets/articulation/articulation.py | 101 +++++++++--------- 1 file changed, 50 insertions(+), 51 deletions(-) diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index 03c1bf9dd3a..c19ab6e3424 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -1926,6 +1926,22 @@ def _log_articulation_info(self): Note: We purposefully read the values from the simulator to ensure that the values are configured as expected. """ + + # define custom formatters for large numbers and limit ranges + def format_large_number(_, v: float) -> str: + """Format large numbers using scientific notation.""" + if abs(v) >= 1e3: + return f"{v:.1e}" + else: + return f"{v:.3f}" + + def format_limits(_, v: tuple[float, float]) -> str: + """Format limit ranges using scientific notation.""" + if abs(v[0]) >= 1e3 or abs(v[1]) >= 1e3: + return f"[{v[0]:.1e}, {v[1]:.1e}]" + else: + return f"[{v[0]:.3f}, {v[1]:.3f}]" + # read out all joint parameters from simulation # -- gains stiffnesses = self.root_physx_view.get_dof_stiffnesses()[0].tolist() @@ -1946,64 +1962,39 @@ def _log_articulation_info(self): # create table for term information joint_table = PrettyTable() joint_table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})" + # build field names based on Isaac Sim version + joint_table.field_names = ["Index", "Name", "Stiffness", "Damping", "Armature"] if get_isaac_sim_version().major < 5: - joint_table.field_names = [ - "Index", - "Name", - "Stiffness", - "Damping", - "Armature", - "Static Friction", - "Position Limits", - "Velocity Limits", - "Effort Limits", - ] + joint_table.field_names.extend(["Static Friction"]) else: - joint_table.field_names = [ - "Index", - "Name", - "Stiffness", - "Damping", - "Armature", - "Static Friction", - "Dynamic Friction", - "Viscous Friction", - "Position Limits", - "Velocity Limits", - "Effort Limits", - ] - joint_table.float_format = ".3" - joint_table.custom_format["Position Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]" + joint_table.field_names.extend(["Static Friction", "Dynamic Friction", "Viscous Friction"]) + joint_table.field_names.extend(["Position Limits", "Velocity Limits", "Effort Limits"]) + + # apply custom formatters to numeric columns + joint_table.custom_format["Stiffness"] = format_large_number + joint_table.custom_format["Damping"] = format_large_number + joint_table.custom_format["Armature"] = format_large_number + joint_table.custom_format["Static Friction"] = format_large_number + if get_isaac_sim_version().major >= 5: + joint_table.custom_format["Dynamic Friction"] = format_large_number + joint_table.custom_format["Viscous Friction"] = format_large_number + joint_table.custom_format["Position Limits"] = format_limits + joint_table.custom_format["Velocity Limits"] = format_large_number + joint_table.custom_format["Effort Limits"] = format_large_number + # set alignment of table columns joint_table.align["Name"] = "l" # add info on each term for index, name in enumerate(self.joint_names): + # build row data based on Isaac Sim version + row_data = [index, name, stiffnesses[index], dampings[index], armatures[index]] if get_isaac_sim_version().major < 5: - joint_table.add_row([ - index, - name, - stiffnesses[index], - dampings[index], - armatures[index], - static_frictions[index], - position_limits[index], - velocity_limits[index], - effort_limits[index], - ]) + row_data.append(static_frictions[index]) else: - joint_table.add_row([ - index, - name, - stiffnesses[index], - dampings[index], - armatures[index], - static_frictions[index], - dynamic_frictions[index], - viscous_frictions[index], - position_limits[index], - velocity_limits[index], - effort_limits[index], - ]) + row_data.extend([static_frictions[index], dynamic_frictions[index], viscous_frictions[index]]) + row_data.extend([position_limits[index], velocity_limits[index], effort_limits[index]]) + # add row to table + joint_table.add_row(row_data) # convert table to string logger.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string()) @@ -2030,7 +2021,15 @@ def _log_articulation_info(self): "Offset", ] tendon_table.float_format = ".3" - joint_table.custom_format["Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]" + + # apply custom formatters to tendon table columns + tendon_table.custom_format["Stiffness"] = format_large_number + tendon_table.custom_format["Damping"] = format_large_number + tendon_table.custom_format["Limit Stiffness"] = format_large_number + tendon_table.custom_format["Limits"] = format_limits + tendon_table.custom_format["Rest Length"] = format_large_number + tendon_table.custom_format["Offset"] = format_large_number + # add info on each term for index in range(self.num_fixed_tendons): tendon_table.add_row([ From 11625d7c152276503a934a7b2c168eb1783e05a6 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:17:58 +0100 Subject: [PATCH 079/100] makes error bold red --- source/isaaclab/isaaclab/utils/logger.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/utils/logger.py b/source/isaaclab/isaaclab/utils/logger.py index c0ea26b6074..67fd9fd4f88 100644 --- a/source/isaaclab/isaaclab/utils/logger.py +++ b/source/isaaclab/isaaclab/utils/logger.py @@ -116,8 +116,8 @@ class ColoredFormatter(logging.Formatter): COLORS = { "WARNING": "\033[33m", # orange/yellow - "ERROR": "\033[31m", # red - "CRITICAL": "\033[31m", # red + "ERROR": "\033[1;31m", # bold red + "CRITICAL": "\033[1;31m", # bold red "INFO": "\033[0m", # reset "DEBUG": "\033[0m", } From d94635de4fdb2088b598b98100d0faf22c9e4924 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:21:26 +0100 Subject: [PATCH 080/100] fixes error from some code --- .../isaaclab/isaaclab/sim/simulation_context.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 8edefd0090b..1262346d8d9 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -324,15 +324,13 @@ def __new__(cls, *args, **kwargs) -> SimulationContext: """ @classmethod - def instance(cls) -> SimulationContext: + def instance(cls) -> SimulationContext | None: """Returns the instance of the simulation context. Returns: - SimulationContext: The instance of the simulation context. + The instance of the simulation context. None if the instance is not initialized. """ - if cls._instance is None: - logging.error("Simulation context is not initialized. Please create a new instance using the constructor.") - return cls._instance # type: ignore + return cls._instance @classmethod def clear_instance(cls) -> None: @@ -466,6 +464,14 @@ def get_physics_dt(self) -> float: """ return self.cfg.dt + def get_rendering_dt(self) -> float: + """Returns the rendering time step of the simulation. + + Returns: + The rendering time step of the simulation. + """ + return self.cfg.dt * self.cfg.render_interval + """ Operations - Utilities. """ From ceeaf5e034b880f4f8feaa7cff81c05245661d12 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:22:57 +0100 Subject: [PATCH 081/100] makes border length match --- source/isaaclab/isaaclab/utils/logger.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/utils/logger.py b/source/isaaclab/isaaclab/utils/logger.py index 67fd9fd4f88..23c61c45387 100644 --- a/source/isaaclab/isaaclab/utils/logger.py +++ b/source/isaaclab/isaaclab/utils/logger.py @@ -96,12 +96,13 @@ def configure_logging( root_logger.addHandler(file_handler) # print the log file path once at startup with nice formatting - border = "=" * 86 cyan = "\033[36m" # cyan color bold = "\033[1m" # bold text reset = "\033[0m" # reset formatting + message = f"[INFO][IsaacLab]: Logging to file: {log_file_path}" + border = "=" * len(message) print(f"\n{cyan}{border}{reset}") - print(f"{cyan}{bold}[INFO][IsaacLab]: Logging to file: {log_file_path}{reset}") + print(f"{cyan}{bold}{message}{reset}") print(f"{cyan}{border}{reset}\n") # return the root logger From c7b843792bf18eb20246c194fd0d0bdfa55a0960 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:30:04 +0100 Subject: [PATCH 082/100] fixes printing of table in articulation --- .../isaaclab/assets/articulation/articulation.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index c19ab6e3424..552e9b2d92e 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -1963,12 +1963,13 @@ def format_limits(_, v: tuple[float, float]) -> str: joint_table = PrettyTable() joint_table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})" # build field names based on Isaac Sim version - joint_table.field_names = ["Index", "Name", "Stiffness", "Damping", "Armature"] + field_names = ["Index", "Name", "Stiffness", "Damping", "Armature"] if get_isaac_sim_version().major < 5: - joint_table.field_names.extend(["Static Friction"]) + field_names.append("Static Friction") else: - joint_table.field_names.extend(["Static Friction", "Dynamic Friction", "Viscous Friction"]) - joint_table.field_names.extend(["Position Limits", "Velocity Limits", "Effort Limits"]) + field_names.extend(["Static Friction", "Dynamic Friction", "Viscous Friction"]) + field_names.extend(["Position Limits", "Velocity Limits", "Effort Limits"]) + joint_table.field_names = field_names # apply custom formatters to numeric columns joint_table.custom_format["Stiffness"] = format_large_number From 682ddfdaeaae9da2bf2d8d7b7e01446b57f112e7 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:40:23 +0100 Subject: [PATCH 083/100] fies how check for callback is done --- .../isaaclab/sim/simulation_context.py | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 1262346d8d9..657460a4968 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -609,6 +609,8 @@ def play(self) -> None: # play the simulation self._timeline_iface.play() self._timeline_iface.commit() + # check for callback exceptions + self._check_for_callback_exceptions() # perform one step to propagate all physics handles properly if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: self.set_setting("/app/player/playSimulations", False) @@ -619,6 +621,8 @@ def pause(self) -> None: """Pause the simulation.""" # pause the simulation self._timeline_iface.pause() + # check for callback exceptions + self._check_for_callback_exceptions() # set the play simulations setting if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: self.set_setting("/app/player/playSimulations", False) @@ -633,6 +637,8 @@ def stop(self) -> None: """ # stop the simulation self._timeline_iface.stop() + # check for callback exceptions + self._check_for_callback_exceptions() # set the play simulations setting if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: self.set_setting("/app/player/playSimulations", False) @@ -655,15 +661,6 @@ def reset(self, soft: bool = False): soft (bool, optional): if set to True simulation won't be stopped and start again. It only calls the reset on the scene objects. """ - # disable simulation stopping control so that we can crash the program - # if an exception is raised in a callback during initialization. - self._disable_app_control_on_stop_handle = True - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise - # reset the simulation if not soft: # disable app control on stop handle @@ -674,7 +671,8 @@ def reset(self, soft: bool = False): # play the simulation self.play() # initialize the physics simulation - SimulationManager.initialize_physics() + if SimulationManager.get_physics_sim_view() is None: + SimulationManager.initialize_physics() # self._physx_iface.force_load_physics_from_usd() # self._physx_iface.start_simulation() # self._physx_iface.update_simulation(self.cfg.dt, 0.0) @@ -686,6 +684,9 @@ def reset(self, soft: bool = False): # self._physx_sim_iface.fetch_results() # self._message_bus.dispatch_event(IsaacEvents.PHYSICS_WARMUP.value, payload={}) + # check for callback exceptions + self._check_for_callback_exceptions() + # app.update() may be changing the cuda device in reset, so we force it back to our desired device here if "cuda" in self.device: torch.cuda.set_device(self.device) @@ -710,8 +711,6 @@ def reset(self, soft: bool = False): if not soft: for _ in range(2): self.render() - # re-enable simulation stopping control - self._disable_app_control_on_stop_handle = False def forward(self) -> None: """Updates articulation kinematics and fabric for rendering.""" @@ -1130,6 +1129,16 @@ def _load_fabric_interface(self): self.carb_settings.set_bool("/physics/updateVelocitiesToUsd", not self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateForceSensorsToUsd", not self.cfg.use_fabric) + def _check_for_callback_exceptions(self): + """Checks for callback exceptions and raises them if found.""" + # disable simulation stopping control so that we can crash the program + # if an exception is raised in a callback. + self._disable_app_control_on_stop_handle = True + # check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # type: ignore + raise builtins.ISAACLAB_CALLBACK_EXCEPTION # type: ignore + self._disable_app_control_on_stop_handle = False + """ Helper functions - Animation Recording. """ From 5b97b782081d6e051c17a86b59d174620f1ba001 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:45:50 +0100 Subject: [PATCH 084/100] fixes init errors --- source/isaaclab/isaaclab/assets/asset_base.py | 15 +++++++++------ source/isaaclab/isaaclab/sensors/sensor_base.py | 5 ++--- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index 3c78c079377..6dc450baca1 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -308,15 +308,18 @@ def _initialize_callback(self, event): called whenever the simulator "plays" from a "stop" state. """ if not self._is_initialized: - # obtain simulation related information - self._backend = SimulationManager.get_backend() - self._device = SimulationManager.get_physics_sim_device() - # initialize the asset try: + # Obtain Simulation Context + sim = sim_utils.SimulationContext.instance() + if sim is None: + raise RuntimeError("Simulation Context is not initialized!") + # Obtain device and backend + self._device = sim.device + # initialize the asset self._initialize_impl() except Exception as e: - if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: - builtins.ISAACLAB_CALLBACK_EXCEPTION = e + if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: # type: ignore + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore # set flag self._is_initialized = True diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 7de0c82541a..7d6bdf88183 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -203,7 +203,6 @@ def _initialize_impl(self): raise RuntimeError("Simulation Context is not initialized!") # Obtain device and backend self._device = sim.device - self._backend = sim.backend self._sim_physics_dt = sim.get_physics_dt() # Count number of environments env_prim_path_expr = self.cfg.prim_path.rsplit("/", 1)[0] @@ -301,8 +300,8 @@ def _initialize_callback(self, event): try: self._initialize_impl() except Exception as e: - if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: - builtins.ISAACLAB_CALLBACK_EXCEPTION = e + if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: # type: ignore + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore self._is_initialized = True def _invalidate_initialize_callback(self, event): From ffcf7702d410c7d43405c03af5b1d0143b041114 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 18:46:01 +0100 Subject: [PATCH 085/100] comments out detach stage for now --- source/isaaclab/isaaclab/sim/simulation_context.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 657460a4968..c7f990e15e7 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -345,13 +345,13 @@ def clear_instance(cls) -> None: cls._instance._app_control_on_stop_handle.unsubscribe() cls._instance._app_control_on_stop_handle = None # detach the stage from physx - if cls._instance._physx_sim_iface is not None: - cls._instance._physx_sim_iface.detach_stage() + # if cls._instance._physx_sim_iface is not None: + # cls._instance._physx_sim_iface.detach_stage() # detach the stage from the USD stage cache - stage_cache = UsdUtils.StageCache.Get() - stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() - if stage_id > 0: - stage_cache.Erase(cls._instance._initial_stage) + # stage_cache = UsdUtils.StageCache.Get() + # stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() + # if stage_id > 0: + # stage_cache.Erase(cls._instance._initial_stage) # clear the instance and the flag cls._instance = None cls._is_initialized = False From 32f4165b95548f3a6352d8537e8ad230f7f4c22d Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:02:46 +0100 Subject: [PATCH 086/100] uses simulation manager for context ops --- .../isaaclab/sim/simulation_context.py | 41 ++++--------------- 1 file changed, 7 insertions(+), 34 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index c7f990e15e7..259db045097 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -270,8 +270,6 @@ def __init__(self, cfg: SimulationCfg | None = None): self._physx_iface = omni.physx.get_physx_interface() self._physx_sim_iface = omni.physx.get_physx_simulation_interface() self._timeline_iface = omni.timeline.get_timeline_interface() - # create interfaces that are initialized on 'reset - self._physics_sim_view = None # set timeline auto update to True self._timeline_iface.set_auto_update(True) @@ -383,7 +381,7 @@ def device(self) -> str: @property def physics_sim_view(self) -> omni.physics.tensors.SimulationView: """Physics simulation view with torch backend.""" - return self._physics_sim_view + return SimulationManager.get_physics_sim_view() """ Operations - Simulation Information. @@ -673,17 +671,6 @@ def reset(self, soft: bool = False): # initialize the physics simulation if SimulationManager.get_physics_sim_view() is None: SimulationManager.initialize_physics() - # self._physx_iface.force_load_physics_from_usd() - # self._physx_iface.start_simulation() - # self._physx_iface.update_simulation(self.cfg.dt, 0.0) - # # attach stage to physx - # current_attached_stage = self._physx_sim_iface.get_attached_stage() - # if current_attached_stage != self._initial_stage_id: - # self._physx_sim_iface.attach_stage(self._initial_stage_id) - # # fetch results - # self._physx_sim_iface.fetch_results() - # self._message_bus.dispatch_event(IsaacEvents.PHYSICS_WARMUP.value, payload={}) - # check for callback exceptions self._check_for_callback_exceptions() @@ -691,21 +678,9 @@ def reset(self, soft: bool = False): if "cuda" in self.device: torch.cuda.set_device(self.device) - # create the simulation view - # SimulationManager._create_simulation_view(None) - # self._physics_sim_view = omni.physics.tensors.create_simulation_view("torch", stage_id=self._initial_stage_id) - # self._physics_sim_view.set_subspace_roots("/") - # self._physx_iface.update_simulation(self.cfg.dt, 0.0) - # # TODO: Remove these once we don't rely on Isaac Sim internals. - # self._message_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) - # SimulationManager._simulation_view_created = True - # SimulationManager._physics_sim_view = self._physics_sim_view - # self._message_bus.dispatch_event(IsaacEvents.PHYSICS_READY.value, payload={}) - self._physics_sim_view = SimulationManager.get_physics_sim_view() - # enable kinematic rendering with fabric - if self._physics_sim_view: - self._physics_sim_view._backend.initialize_kinematic_bodies() + if self.physics_sim_view is not None: + self.physics_sim_view._backend.initialize_kinematic_bodies() # perform additional rendering steps to warm up replicator buffers # this is only needed for the first time we set the simulation if not soft: @@ -749,7 +724,7 @@ def step(self, render: bool = True): # reason: physics has to parse the scene again and inform other extensions like hydra-delegate. # without this the app becomes unresponsive. # FIXME: This steps physics as well, which we is not good in general. - self.app.update() + self._app_iface.update() # step the simulation if render: @@ -1378,8 +1353,6 @@ def build_simulation_context( # Clear the stage sim.clear_instance() - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise + # Check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # type: ignore + raise builtins.ISAACLAB_CALLBACK_EXCEPTION # type: ignore From a4bd7cb88c9cbebc75e90e026bcbacf0d3074b4a Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:05:22 +0100 Subject: [PATCH 087/100] removes builtin check since we are always in non-app mode --- .../isaaclab/sim/simulation_context.py | 40 ++++++++----------- 1 file changed, 17 insertions(+), 23 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 259db045097..0ffb11c5469 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -253,15 +253,12 @@ def __init__(self, cfg: SimulationCfg | None = None): # add callback to deal the simulation app when simulation is stopped. # this is needed because physics views go invalid once we stop the simulation - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() - self._app_control_on_stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( - int(omni.timeline.TimelineEventType.STOP), - lambda *args, obj=weakref.proxy(self): obj._app_control_on_stop_handle_fn(*args), - order=15, - ) - else: - self._app_control_on_stop_handle = None + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._app_control_on_stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda *args, obj=weakref.proxy(self): obj._app_control_on_stop_handle_fn(*args), + order=15, + ) self._disable_app_control_on_stop_handle = False # obtain interfaces for simulation @@ -610,10 +607,9 @@ def play(self) -> None: # check for callback exceptions self._check_for_callback_exceptions() # perform one step to propagate all physics handles properly - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - self.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self.set_setting("/app/player/playSimulations", True) + self.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self.set_setting("/app/player/playSimulations", True) def pause(self) -> None: """Pause the simulation.""" @@ -621,11 +617,10 @@ def pause(self) -> None: self._timeline_iface.pause() # check for callback exceptions self._check_for_callback_exceptions() - # set the play simulations setting - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - self.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self.set_setting("/app/player/playSimulations", True) + # perform one step to propagate all physics handles properly + self.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self.set_setting("/app/player/playSimulations", True) def stop(self) -> None: """Stop the simulation. @@ -637,11 +632,10 @@ def stop(self) -> None: self._timeline_iface.stop() # check for callback exceptions self._check_for_callback_exceptions() - # set the play simulations setting - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - self.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self.set_setting("/app/player/playSimulations", True) + # perform one step to propagate all physics handles properly + self.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self.set_setting("/app/player/playSimulations", True) """ Operations - Override (standalone) From 2d6d8ad7da837a88fc4afafbf1225c7c0626e11f Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:09:28 +0100 Subject: [PATCH 088/100] removes terminal check from other places --- .../isaaclab/isaaclab/envs/direct_marl_env.py | 22 ++++++------- .../isaaclab/isaaclab/envs/direct_rl_env.py | 22 ++++++------- .../isaaclab/envs/manager_based_env.py | 32 ++++++++----------- .../isaaclab/sim/simulation_context.py | 2 +- source/isaaclab/isaaclab/sim/utils/stage.py | 6 ++-- 5 files changed, 36 insertions(+), 48 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index a37112a28bb..dbc6bb4c966 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -5,7 +5,6 @@ from __future__ import annotations -import builtins import gymnasium as gym import inspect import logging @@ -150,17 +149,16 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # play the simulator to activate physics handles # note: this activates the physics simulation view that exposes TensorAPIs # note: when started in extension mode, first call sim.reset_async() and then initialize the managers - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: - print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") - with Timer("[INFO]: Time taken for simulation start", "simulation_start"): - # since the reset can trigger callbacks which use the stage, - # we need to set the stage context here - with use_stage(self.sim.get_initial_stage()): - self.sim.reset() - # update scene to pre populate data buffers for assets and sensors. - # this is needed for the observation manager to get valid tensors for initialization. - # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. - self.scene.update(dt=self.physics_dt) + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) # check if debug visualization is has been implemented by the environment source_code = inspect.getsource(self._set_debug_vis_impl) diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 728edc73247..8922934cc15 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -5,7 +5,6 @@ from __future__ import annotations -import builtins import gymnasium as gym import inspect import logging @@ -157,17 +156,16 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # play the simulator to activate physics handles # note: this activates the physics simulation view that exposes TensorAPIs # note: when started in extension mode, first call sim.reset_async() and then initialize the managers - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: - print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") - with Timer("[INFO]: Time taken for simulation start", "simulation_start"): - # since the reset can trigger callbacks which use the stage, - # we need to set the stage context here - with use_stage(self.sim.get_initial_stage()): - self.sim.reset() - # update scene to pre populate data buffers for assets and sensors. - # this is needed for the observation manager to get valid tensors for initialization. - # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. - self.scene.update(dt=self.physics_dt) + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) # check if debug visualization is has been implemented by the environment source_code = inspect.getsource(self._set_debug_vis_impl) diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 8626dd7587a..9f03968b0a7 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -3,7 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -import builtins import logging import torch import warnings @@ -101,11 +100,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # since it gets confused with Isaac Sim's SimulationContext class self.sim: SimulationContext = SimulationContext(self.cfg.sim) else: - # simulation context should only be created before the environment - # when in extension mode - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - raise RuntimeError("Simulation context already exists. Cannot create a new one.") - self.sim: SimulationContext = SimulationContext.instance() + raise RuntimeError("Simulation context already exists. Cannot create a new one.") # make sure torch is running on the correct device if "cuda" in self.device: @@ -162,19 +157,18 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # play the simulator to activate physics handles # note: this activates the physics simulation view that exposes TensorAPIs # note: when started in extension mode, first call sim.reset_async() and then initialize the managers - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: - print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") - with Timer("[INFO]: Time taken for simulation start", "simulation_start"): - # since the reset can trigger callbacks which use the stage, - # we need to set the stage context here - with use_stage(self.sim.get_initial_stage()): - self.sim.reset() - # update scene to pre populate data buffers for assets and sensors. - # this is needed for the observation manager to get valid tensors for initialization. - # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. - self.scene.update(dt=self.physics_dt) - # add timeline event to load managers - self.load_managers() + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) + # add timeline event to load managers + self.load_managers() # extend UI elements # we need to do this here after all the managers are initialized diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 0ffb11c5469..d1958fd8c24 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -28,7 +28,7 @@ import omni.physx import omni.timeline import omni.usd -from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager +from isaacsim.core.simulation_manager import SimulationManager from isaacsim.core.utils.viewports import set_camera_view from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index 003863179dd..d10707c1b19 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -5,7 +5,6 @@ """Utilities for operating on the USD stage.""" -import builtins import contextlib import logging import threading @@ -344,9 +343,8 @@ def _predicate(prim: Usd.Prim) -> bool: prim_paths_to_delete = [prim.GetPath().pathString for prim in prims] # delete prims delete_prim(prim_paths_to_delete) - - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: # type: ignore - omni.kit.app.get_app_interface().update() + # update stage + update_stage() def is_stage_loading() -> bool: From 11a9c250fbf91bde027abb59c566e516a3d4c8e3 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:31:27 +0100 Subject: [PATCH 089/100] adds test for exception raised in callbacks --- .../isaaclab/sim/simulation_context.py | 28 ++- .../test/sim/test_simulation_context.py | 222 +++++++++++------- 2 files changed, 151 insertions(+), 99 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index d1958fd8c24..497b48083f5 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -340,13 +340,13 @@ def clear_instance(cls) -> None: cls._instance._app_control_on_stop_handle.unsubscribe() cls._instance._app_control_on_stop_handle = None # detach the stage from physx - # if cls._instance._physx_sim_iface is not None: - # cls._instance._physx_sim_iface.detach_stage() + if cls._instance._physx_sim_iface is not None: + cls._instance._physx_sim_iface.detach_stage() # detach the stage from the USD stage cache - # stage_cache = UsdUtils.StageCache.Get() - # stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() - # if stage_id > 0: - # stage_cache.Erase(cls._instance._initial_stage) + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() + if stage_id > 0: + stage_cache.Erase(cls._instance._initial_stage) # clear the instance and the flag cls._instance = None cls._is_initialized = False @@ -773,7 +773,8 @@ def render(self, mode: RenderMode | None = None): if "cuda" in self.device: torch.cuda.set_device(self.device) - def clear(self): + @classmethod + def clear(cls): """Clear the current USD stage.""" def _predicate(prim: Usd.Prim) -> bool: @@ -1105,7 +1106,10 @@ def _check_for_callback_exceptions(self): self._disable_app_control_on_stop_handle = True # check if we need to raise an exception that was raised in a callback if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # type: ignore - raise builtins.ISAACLAB_CALLBACK_EXCEPTION # type: ignore + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore + raise exception_to_raise + # re-enable simulation stopping control self._disable_app_control_on_stop_handle = False """ @@ -1246,10 +1250,10 @@ def _app_control_on_stop_handle_fn(self, event: carb.events.IEvent): This callback is used only when running the simulation in a standalone python script. In an extension, it is expected that the user handles the extension shutdown. """ - if not self._disable_app_control_on_stop_handle: - while not omni.timeline.get_timeline_interface().is_playing(): - self.render() - return + pass + # if not self._disable_app_control_on_stop_handle: + # while not omni.timeline.get_timeline_interface().is_playing(): + # self.render() @contextmanager diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 916ebc89066..d66d4e17ec6 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -12,6 +12,7 @@ """Rest everything follows.""" +import builtins import numpy as np import weakref @@ -33,6 +34,7 @@ def test_setup_teardown(): yield # Teardown: Clear the simulation context after each test + SimulationContext.clear() SimulationContext.clear_instance() @@ -740,8 +742,12 @@ def on_pause_callback(event): @pytest.mark.isaacsim_ci -def test_isaac_event_physics_warmup(): - """Test that PHYSICS_WARMUP Isaac event is triggered during reset.""" +@pytest.mark.parametrize( + "event_type", + [IsaacEvents.PHYSICS_WARMUP, IsaacEvents.SIMULATION_VIEW_CREATED, IsaacEvents.PHYSICS_READY], +) +def test_isaac_event_triggered_on_reset(event_type): + """Test that Isaac events are triggered during reset.""" cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) @@ -750,99 +756,23 @@ def test_isaac_event_physics_warmup(): cube_cfg.func("/World/Cube", cube_cfg) # create callback tracker - callback_state = {"warmup_called": False} + callback_state = {"called": False} - def on_physics_warmup(event): - callback_state["warmup_called"] = True + def on_event(event): + callback_state["called"] = True - # register callback for PHYSICS_WARMUP event - callback_id = SimulationManager.register_callback( - lambda event: on_physics_warmup(event), event=IsaacEvents.PHYSICS_WARMUP - ) - - try: - # verify callback hasn't been called yet - assert not callback_state["warmup_called"] - - # reset the simulation - should trigger PHYSICS_WARMUP - sim.reset() - - # verify callback was triggered - assert callback_state["warmup_called"] - - finally: - # cleanup callback - if callback_id is not None: - SimulationManager.deregister_callback(callback_id) - - -@pytest.mark.isaacsim_ci -def test_isaac_event_simulation_view_created(): - """Test that SIMULATION_VIEW_CREATED Isaac event is triggered during reset.""" - cfg = SimulationCfg(dt=0.01) - sim = SimulationContext(cfg) - - # create simple scene - cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) - cube_cfg.func("/World/Cube", cube_cfg) - - # create callback tracker - callback_state = {"view_created_called": False} - - def on_simulation_view_created(event): - callback_state["view_created_called"] = True - - # register callback for SIMULATION_VIEW_CREATED event - callback_id = SimulationManager.register_callback( - lambda event: on_simulation_view_created(event), event=IsaacEvents.SIMULATION_VIEW_CREATED - ) - - try: - # verify callback hasn't been called yet - assert not callback_state["view_created_called"] - - # reset the simulation - should trigger SIMULATION_VIEW_CREATED - sim.reset() - - # verify callback was triggered - assert callback_state["view_created_called"] - - finally: - # cleanup callback - if callback_id is not None: - SimulationManager.deregister_callback(callback_id) - - -@pytest.mark.isaacsim_ci -def test_isaac_event_physics_ready(): - """Test that PHYSICS_READY Isaac event is triggered during reset.""" - cfg = SimulationCfg(dt=0.01) - sim = SimulationContext(cfg) - - # create simple scene - cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) - cube_cfg.func("/World/Cube", cube_cfg) - - # create callback tracker - callback_state = {"physics_ready_called": False} - - def on_physics_ready(event): - callback_state["physics_ready_called"] = True - - # register callback for PHYSICS_READY event - callback_id = SimulationManager.register_callback( - lambda event: on_physics_ready(event), event=IsaacEvents.PHYSICS_READY - ) + # register callback for the event + callback_id = SimulationManager.register_callback(lambda event: on_event(event), event=event_type) try: # verify callback hasn't been called yet - assert not callback_state["physics_ready_called"] + assert not callback_state["called"] - # reset the simulation - should trigger PHYSICS_READY + # reset the simulation - should trigger the event sim.reset() # verify callback was triggered - assert callback_state["physics_ready_called"] + assert callback_state["called"] finally: # cleanup callback @@ -869,7 +799,7 @@ def on_prim_deletion(event): callback_state["prim_deleted"] = True # event payload should contain the deleted prim path if hasattr(event, "payload") and event.payload: - callback_state["deleted_path"] = event.payload.get("prim_path", None) + callback_state["deleted_path"] = event.payload.get("prim_path") # register callback for PRIM_DELETION event callback_id = SimulationManager.register_callback( @@ -1058,3 +988,121 @@ def callback3(event): SimulationManager.deregister_callback(id2) if id3 is not None: SimulationManager.deregister_callback(id3) + + +""" +Exception Handling in Callbacks Tests. +""" + + +@pytest.mark.isaacsim_ci +def test_exception_in_callback_on_reset(): + """Test that exceptions in callbacks during reset are properly captured and raised.""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a callback that raises an exception + def on_physics_ready_with_exception(event): + raise RuntimeError("Test exception in callback during reset!") + + # register callback that will raise an exception + callback_id = SimulationManager.register_callback( + lambda event: on_physics_ready_with_exception(event), event=IsaacEvents.PHYSICS_READY + ) + + try: + # reset should capture and re-raise the exception + with pytest.raises(RuntimeError, match="Test exception in callback during reset!"): + sim.reset() + + # verify the exception was cleared after being raised + assert builtins.ISAACLAB_CALLBACK_EXCEPTION is None # type: ignore + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + # ensure exception is cleared + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore + + +@pytest.mark.isaacsim_ci +def test_exception_in_callback_on_play(): + """Test that exceptions in callbacks during play are properly captured and raised.""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback tracker + callback_state = {"called": False} + + def on_play_with_exception(event): + callback_state["called"] = True + raise ValueError("Test exception in play callback!") + + # register callback via timeline + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: on_play_with_exception(event), order=20 + ) + + try: + # play should capture and re-raise the exception + with pytest.raises(ValueError, match="Test exception in play callback!"): + sim.play() + + # verify callback was called + assert callback_state["called"] + + # verify the exception was cleared + assert builtins.ISAACLAB_CALLBACK_EXCEPTION is None # type: ignore + + finally: + # cleanup + if play_handle is not None: + play_handle.unsubscribe() + # ensure exception is cleared + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore + + +@pytest.mark.isaacsim_ci +def test_exception_in_callback_on_stop(): + """Test that exceptions in callbacks during stop are properly captured and raised.""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # start the simulation first + sim.play() + + def on_stop_with_exception(event): + raise OSError("Test exception in stop callback!") + + # register callback via timeline + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), lambda event: on_stop_with_exception(event), order=20 + ) + + try: + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # stop should capture and re-raise the exception + with pytest.raises(IOError, match="Test exception in stop callback!"): + sim.stop() + + # verify the exception was cleared + assert builtins.ISAACLAB_CALLBACK_EXCEPTION is None # type: ignore + + finally: + # cleanup + if stop_handle is not None: + stop_handle.unsubscribe() + # ensure exception is cleared + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore From db9e25d0da9ef3a23056bb37cd844badab45acb9 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:45:44 +0100 Subject: [PATCH 090/100] adds stage attribute to clear stage --- source/isaaclab/isaaclab/sim/utils/stage.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index d10707c1b19..6ddf564c28c 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -288,7 +288,7 @@ def close_stage(callback_fn: Callable[[bool, str], None] | None = None) -> bool: return result -def clear_stage(predicate: Callable[[Usd.Prim], bool] | None = None) -> None: +def clear_stage(stage: Usd.Stage | None = None, predicate: Callable[[Usd.Prim], bool] | None = None) -> None: """Deletes all prims in the stage without populating the undo command buffer. The function will delete all prims in the stage that satisfy the predicate. If the predicate @@ -301,6 +301,7 @@ def clear_stage(predicate: Callable[[Usd.Prim], bool] | None = None) -> None: * Ancestral prims (prims that are ancestors of other prims in a reference/payload chain) Args: + stage: The stage to clear. Defaults to None, in which case the current stage is used. predicate: A user-defined function that takes a USD prim as an argument and returns a boolean indicating if the prim should be deleted. If None, all prims will be considered for deletion (subject to the default exclusions above). @@ -337,12 +338,15 @@ def _predicate(prim: Usd.Prim) -> bool: # Additional predicate check return predicate is None or predicate(prim) + # get stage handle + stage = get_current_stage() if stage is None else stage + # get all prims to delete - prims = get_all_matching_child_prims("/", _predicate) + prims = get_all_matching_child_prims("/", _predicate, stage=stage, traverse_instance_prims=False) # convert prims to prim paths prim_paths_to_delete = [prim.GetPath().pathString for prim in prims] # delete prims - delete_prim(prim_paths_to_delete) + delete_prim(prim_paths_to_delete, stage) # update stage update_stage() From 06728281b1079d543154254bcab7a771dd907ea1 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:45:53 +0100 Subject: [PATCH 091/100] fixes callback calls --- .../isaaclab/sim/simulation_context.py | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 497b48083f5..545d59eb664 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -604,23 +604,21 @@ def play(self) -> None: # play the simulation self._timeline_iface.play() self._timeline_iface.commit() - # check for callback exceptions - self._check_for_callback_exceptions() # perform one step to propagate all physics handles properly self.set_setting("/app/player/playSimulations", False) self._app_iface.update() self.set_setting("/app/player/playSimulations", True) + # check for callback exceptions + self._check_for_callback_exceptions() def pause(self) -> None: """Pause the simulation.""" # pause the simulation self._timeline_iface.pause() + self._timeline_iface.commit() + # we don't need to propagate physics handles since no callbacks are triggered during pause # check for callback exceptions self._check_for_callback_exceptions() - # perform one step to propagate all physics handles properly - self.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self.set_setting("/app/player/playSimulations", True) def stop(self) -> None: """Stop the simulation. @@ -630,12 +628,13 @@ def stop(self) -> None: """ # stop the simulation self._timeline_iface.stop() - # check for callback exceptions - self._check_for_callback_exceptions() + self._timeline_iface.commit() # perform one step to propagate all physics handles properly self.set_setting("/app/player/playSimulations", False) self._app_iface.update() self.set_setting("/app/player/playSimulations", True) + # check for callback exceptions + self._check_for_callback_exceptions() """ Operations - Override (standalone) @@ -787,8 +786,12 @@ def _predicate(prim: Usd.Prim) -> bool: if prim.GetTypeName() == "PhysicsScene": return False return True - - sim_utils.clear_stage(predicate=_predicate) + # clear the stage + if cls._instance is not None: + stage = cls._instance._initial_stage + sim_utils.clear_stage(stage=stage, predicate=_predicate) + else: + logger.error("Simulation context is not initialized. Unable to clear the stage.") """ Helper Functions From a120b91c33bc9547b740ef8affb23153b7cf3d3b Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:46:09 +0100 Subject: [PATCH 092/100] ensures exception are caught --- .../test/sim/test_simulation_context.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index d66d4e17ec6..354c36928ef 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -1008,7 +1008,10 @@ def test_exception_in_callback_on_reset(): # create a callback that raises an exception def on_physics_ready_with_exception(event): - raise RuntimeError("Test exception in callback during reset!") + try: + raise RuntimeError("Test exception in callback during reset!") + except Exception as e: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore # register callback that will raise an exception callback_id = SimulationManager.register_callback( @@ -1042,8 +1045,11 @@ def test_exception_in_callback_on_play(): callback_state = {"called": False} def on_play_with_exception(event): - callback_state["called"] = True - raise ValueError("Test exception in play callback!") + try: + callback_state["called"] = True + raise ValueError("Test exception in play callback!") + except Exception as e: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore # register callback via timeline timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() @@ -1081,7 +1087,10 @@ def test_exception_in_callback_on_stop(): sim.play() def on_stop_with_exception(event): - raise OSError("Test exception in stop callback!") + try: + raise OSError("Test exception in stop callback!") + except Exception as e: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore # register callback via timeline timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() @@ -1094,7 +1103,7 @@ def on_stop_with_exception(event): sim._disable_app_control_on_stop_handle = True # type: ignore # stop should capture and re-raise the exception - with pytest.raises(IOError, match="Test exception in stop callback!"): + with pytest.raises(OSError, match="Test exception in stop callback!"): sim.stop() # verify the exception was cleared From 235a9e85655e114bec356c043e86c15f3f9cc644 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 19:47:42 +0100 Subject: [PATCH 093/100] makes sure to not cache if added manually --- source/isaaclab/isaaclab/sim/utils/prims.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 020aa5d4d6f..0cf8347486f 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -202,9 +202,15 @@ def delete_prim(prim_path: str | Sequence[str], stage: Usd.Stage | None = None) stage_cache = UsdUtils.StageCache.Get() stage_id = stage_cache.GetId(stage).ToLongInt() if stage_id < 0: + is_stage_inserted = True stage_id = stage_cache.Insert(stage).ToLongInt() + else: + is_stage_inserted = False # delete prims DeletePrimsCommand(prim_path, stage=stage).do() + # erase from cache to prevent memory leaks + if is_stage_inserted: + stage_cache.Erase(stage) def move_prim(path_from: str, path_to: str, keep_world_transform: bool = True, stage: Usd.Stage | None = None) -> None: From 031f171fd92e317337528bfbdb2d2141ab875640 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 20:15:57 +0100 Subject: [PATCH 094/100] runs formatter --- source/isaaclab/isaaclab/sim/simulation_context.py | 1 + 1 file changed, 1 insertion(+) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 545d59eb664..5cdd28455ed 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -786,6 +786,7 @@ def _predicate(prim: Usd.Prim) -> bool: if prim.GetTypeName() == "PhysicsScene": return False return True + # clear the stage if cls._instance is not None: stage = cls._instance._initial_stage From f2484245280d3b0ffe8a16d19a3e35d42cfa4a43 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 20:19:20 +0100 Subject: [PATCH 095/100] add future import --- source/isaaclab/isaaclab/sim/utils/stage.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index 6ddf564c28c..c6cf0e45dea 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -5,6 +5,8 @@ """Utilities for operating on the USD stage.""" +from __future__ import annotations + import contextlib import logging import threading From f9e29e3aca1099054891e8ce3d56d23e538af161 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 20:45:13 +0100 Subject: [PATCH 096/100] mark unused --- source/isaaclab/isaaclab/sim/simulation_context.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 5cdd28455ed..d4342020fff 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -1237,7 +1237,7 @@ def _finish_anim_recording(self): Callbacks. """ - def _app_control_on_stop_handle_fn(self, event: carb.events.IEvent): + def _app_control_on_stop_handle_fn(self, _): """Callback to deal with the app when the simulation is stopped. Once the simulation is stopped, the physics handles go invalid. After that, it is not possible to From 17ce717799340c31ab438a950534933dcc2a18df Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 23:55:59 +0100 Subject: [PATCH 097/100] adds other settings --- source/isaaclab/isaaclab/sim/simulation_context.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index d4342020fff..50e09b9690e 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -1098,10 +1098,13 @@ def _load_fabric_interface(self): self._fabric_iface = None # set carb settings for fabric + self.carb_settings.set_bool("/physics/fabricEnabled", self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateToUsd", not self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateParticlesToUsd", not self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateVelocitiesToUsd", not self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateForceSensorsToUsd", not self.cfg.use_fabric) + # disable simulation output window visibility + self.carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) def _check_for_callback_exceptions(self): """Checks for callback exceptions and raises them if found.""" From 3d6f1a46cc31bed92f37951f837911ff149935ee Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sat, 3 Jan 2026 23:58:18 +0100 Subject: [PATCH 098/100] adds missing residuals --- source/isaaclab/isaaclab/sim/simulation_context.py | 1 + 1 file changed, 1 insertion(+) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 50e09b9690e..195249f189d 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -1103,6 +1103,7 @@ def _load_fabric_interface(self): self.carb_settings.set_bool("/physics/updateParticlesToUsd", not self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateVelocitiesToUsd", not self.cfg.use_fabric) self.carb_settings.set_bool("/physics/updateForceSensorsToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateResidualsToUsd", not self.cfg.use_fabric) # disable simulation output window visibility self.carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) From e1e1302a197b90efb5c27c6324cf4b99ab0d8cb8 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 5 Jan 2026 21:26:33 +0100 Subject: [PATCH 099/100] passes stage over for creating mdl material --- .../sim/spawners/materials/visual_materials.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 25658786467..e619de1a142 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -59,14 +59,18 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa # since stage in memory is not supported by the "CreatePreviewSurfaceMaterialPrim" kit command attach_stage_to_usd_context(attaching_early=True) + # note: this command does not support passing 'stage' as an argument omni.kit.commands.execute("CreatePreviewSurfaceMaterialPrim", mtl_path=prim_path, select_new_prim=False) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") # obtain prim prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + # check prim is valid + if not prim.IsValid(): + raise ValueError(f"Failed to create preview surface material at path: '{prim_path}'.") # apply properties - cfg = cfg.to_dict() + cfg = cfg.to_dict() # type: ignore del cfg["func"] for attr_name, attr_value in cfg.items(): safe_set_attribute_on_usd_prim(prim, f"inputs:{attr_name}", attr_value, camel_case=True) @@ -108,10 +112,6 @@ def spawn_from_mdl_file(prim_path: str, cfg: visual_materials_cfg.MdlMaterialCfg # spawn material if it doesn't exist. if not stage.GetPrimAtPath(prim_path).IsValid(): - # early attach stage to usd context if stage is in memory - # since stage in memory is not supported by the "CreateMdlMaterialPrim" kit command - attach_stage_to_usd_context(attaching_early=True) - # extract material name from path material_name = cfg.mdl_path.split("/")[-1].split(".")[0] omni.kit.commands.execute( @@ -119,12 +119,16 @@ def spawn_from_mdl_file(prim_path: str, cfg: visual_materials_cfg.MdlMaterialCfg mtl_url=cfg.mdl_path.format(NVIDIA_NUCLEUS_DIR=NVIDIA_NUCLEUS_DIR), mtl_name=material_name, mtl_path=prim_path, + stage=stage, select_new_prim=False, ) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") # obtain prim prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + # check prim is valid + if not prim.IsValid(): + raise ValueError(f"Failed to create MDL material at path: '{prim_path}'.") # apply properties cfg = cfg.to_dict() del cfg["func"] From 8c9ae3af7dbb9134568168a942a1b21bcae416f2 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Thu, 8 Jan 2026 20:58:01 +0000 Subject: [PATCH 100/100] adding fixes for stage in mem unit tests --- source/isaaclab/isaaclab/sim/utils/stage.py | 5 +---- .../isaaclab/test/sim/test_simulation_stage_in_memory.py | 2 ++ source/isaaclab_tasks/test/env_test_utils.py | 8 ++++++++ 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index c6cf0e45dea..ecdfca45526 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -467,14 +467,11 @@ def attach_stage_to_usd_context(attaching_early: bool = False): " does not support stage in memory." ) - # skip this callback to avoid wiping the stage after attachment - SimulationContext.instance().skip_next_stage_open_callback() - # disable stage open callback to avoid clearing callbacks SimulationManager.enable_stage_open_callback(False) # enable physics fabric - SimulationContext.instance()._physics_context.enable_fabric(True) # type: ignore + SimulationContext.instance().carb_settings.set_bool("/physics/fabricEnabled", True) # attach stage to usd context omni.usd.get_context().attach_stage_with_callback(stage_id) diff --git a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py index 2657bbea9de..5f1dd306a3d 100644 --- a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py @@ -41,6 +41,8 @@ def sim(): sim.stop() sim.clear() sim.clear_instance() + # Create a new stage in the USD context to ensure subsequent tests have a valid context stage + sim_utils.create_new_stage() """ diff --git a/source/isaaclab_tasks/test/env_test_utils.py b/source/isaaclab_tasks/test/env_test_utils.py index 43973e8994a..93d926218dc 100644 --- a/source/isaaclab_tasks/test/env_test_utils.py +++ b/source/isaaclab_tasks/test/env_test_utils.py @@ -12,8 +12,10 @@ import torch import carb +import omni.physx import omni.usd +import isaaclab.sim as sim_utils from isaaclab.envs.utils.spaces import sample_space from isaaclab.utils.version import get_isaac_sim_version @@ -188,6 +190,7 @@ def _check_random_actions( # reset the rtx sensors carb setting to False carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: # parse config env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) @@ -262,6 +265,11 @@ def _check_random_actions( # close environment env.close() + # Create a new stage in the USD context to ensure subsequent tests have a valid context stage + # This is necessary when using stage in memory, as the in-memory stage is destroyed on close + if create_stage_in_memory: + sim_utils.create_new_stage() + def _check_valid_tensor(data: torch.Tensor | dict) -> bool: """Checks if given data does not have corrupted values.