From 6fd9082544df2065ec873c8fb8c4b83816604d0d Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 30 Dec 2025 11:14:42 +0100 Subject: [PATCH 001/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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/130] 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. From d4c99f11a46d6b7545e81abafbe360967fd53ec6 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 27 Jan 2026 00:49:34 -0800 Subject: [PATCH 101/130] fixes singleton destory, physics context removal, and consistent torch backend --- .../isaaclab/sim/simulation_context.py | 21 +++++++++++++++---- .../isaacsim/check_rep_texture_randomizer.py | 2 +- .../test/sensors/check_contact_sensor.py | 2 +- .../sensors/check_multi_mesh_ray_caster.py | 2 +- .../isaaclab/test/sensors/check_ray_caster.py | 2 +- .../test/terrains/check_terrain_importer.py | 2 +- .../test/terrains/test_terrain_importer.py | 2 +- 7 files changed, 23 insertions(+), 10 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 8f7a675e6be..dd4f806acef 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -53,10 +53,10 @@ class SimulationContext: * playing, pausing, stepping and stopping the simulation * adding and removing callbacks to different simulation events such as physics stepping, rendering, etc. - This class inherits from the :class:`isaacsim.core.api.simulation_context.SimulationContext` class and - adds additional functionalities such as setting up the simulation context with a configuration object, - exposing other commonly used simulator-related functions, and performing version checks of Isaac Sim - to ensure compatibility between releases. + This class provides a standalone simulation context for Isaac Lab, with additional functionalities + such as setting up the simulation context with a configuration object, exposing commonly used + simulator-related functions, and performing version checks of Isaac Sim to ensure compatibility + between releases. The simulation context is a singleton object. This means that there can only be one instance of the simulation context at any given time. This is enforced by the parent class. Therefore, it is @@ -459,6 +459,11 @@ def get_physics_dt(self) -> float: """ return self.cfg.dt + @property + def physics_prim_path(self) -> str: + """The path to the physics scene prim.""" + return self.cfg.physics_prim_path + def get_rendering_dt(self) -> float: """Returns the rendering time step of the simulation. @@ -897,6 +902,11 @@ def _apply_physics_settings(self): # save the device self._physics_device = "cpu" + # Configure simulation manager backend + # Isaac Lab always uses torch tensors for consistency, even on CPU + SimulationManager.set_backend("torch") + SimulationManager.set_physics_sim_device(self._physics_device) + # -------------------------- # USDPhysics API settings # -------------------------- @@ -1313,6 +1323,9 @@ def build_simulation_context( """ try: if create_new_stage: + # Clear any existing simulation context before creating a new stage + # This ensures proper resource cleanup and allows a fresh initialization + SimulationContext.clear_instance() sim_utils.create_new_stage() if sim_cfg is None: diff --git a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py index e6d43b00f9f..437ec99f4c9 100644 --- a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py +++ b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py @@ -91,7 +91,7 @@ def main(): env_positions = cloner.clone( source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True, copy_from_source=True ) - physics_scene_path = sim.get_physics_context().prim_path + physics_scene_path = sim.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index c556e732658..d7ba46342e8 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -110,7 +110,7 @@ def main(): ) contact_sensor = ContactSensor(cfg=contact_sensor_cfg) # filter collisions within each environment instance - physics_scene_path = sim.get_physics_context().prim_path + physics_scene_path = sim.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"] ) diff --git a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py index 3d137ece466..ca5ab4fab34 100644 --- a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py +++ b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py @@ -102,7 +102,7 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048): cloner.define_base_env("/World/envs") envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) - physics_scene_path = sim.get_physics_context().prim_path + physics_scene_path = sim.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index c2e12da4ea6..1d71eec2416 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -79,7 +79,7 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048): cloner.define_base_env("/World/envs") envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) - physics_scene_path = sim.get_physics_context().prim_path + physics_scene_path = sim.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index fdc305a07af..1c6ae2c2050 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -158,7 +158,7 @@ def main(): cloner.define_base_env("/World/envs") envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_balls) cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) - physics_scene_path = sim.get_physics_context().prim_path + physics_scene_path = sim.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index 63d9230560b..2f0d33482b4 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -317,7 +317,7 @@ def _populate_scene(sim: SimulationContext, num_balls: int = 2048, geom_sphere: prim_paths=envs_prim_paths, replicate_physics=True, ) - physics_scene_path = sim.get_physics_context().prim_path + physics_scene_path = sim.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) From d3e4f7335accb6728f1c722f2e99cb8d6754d9ff Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 27 Jan 2026 10:23:32 -0800 Subject: [PATCH 102/130] simulation manager addition --- apps/isaaclab.python.kit | 2 +- apps/isaacsim_4_5/isaaclab.python.kit | 2 +- .../benchmarks/benchmark_view_comparison.py | 3 +- scripts/tools/check_instanceable.py | 2 +- .../assets/articulation/articulation.py | 2 +- .../assets/articulation/articulation_data.py | 2 +- source/isaaclab/isaaclab/assets/asset_base.py | 15 +- .../deformable_object/deformable_object.py | 2 +- .../assets/rigid_object/rigid_object.py | 2 +- .../rigid_object_collection.py | 7 +- .../isaaclab/isaaclab/controllers/rmp_flow.py | 2 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 2 +- .../isaaclab/envs/manager_based_env.py | 2 +- .../sensors/contact_sensor/contact_sensor.py | 2 +- .../frame_transformer/frame_transformer.py | 2 +- source/isaaclab/isaaclab/sensors/imu/imu.py | 2 +- .../isaaclab/sensors/ray_caster/ray_caster.py | 2 +- .../isaaclab/isaaclab/sensors/sensor_base.py | 7 +- source/isaaclab/isaaclab/sim/__init__.py | 22 + .../isaaclab/sim/simulation_context.py | 6 +- .../isaaclab/sim/simulation_manager.py | 414 ++++++++++++++++++ source/isaaclab/isaaclab/sim/utils/stage.py | 2 +- .../isaaclab/isaaclab/ui/widgets/line_plot.py | 2 +- .../ui/widgets/manager_live_visualizer.py | 2 +- .../test/deps/isaacsim/check_ref_count.py | 2 +- .../isaacsim/check_rep_texture_randomizer.py | 2 +- .../isaaclab/test/devices/check_keyboard.py | 2 +- .../markers/test_visualization_markers.py | 2 +- .../test/sensors/check_contact_sensor.py | 2 +- .../isaaclab/test/sensors/check_imu_sensor.py | 2 +- .../sensors/check_multi_mesh_ray_caster.py | 2 +- .../isaaclab/test/sensors/check_ray_caster.py | 2 +- .../isaaclab/test/sim/test_mesh_converter.py | 2 +- .../isaaclab/test/sim/test_mjcf_converter.py | 2 +- source/isaaclab/test/sim/test_schemas.py | 2 +- .../test/sim/test_simulation_context.py | 8 +- .../test/sim/test_spawn_from_files.py | 2 +- source/isaaclab/test/sim/test_spawn_lights.py | 2 +- .../isaaclab/test/sim/test_spawn_materials.py | 2 +- source/isaaclab/test/sim/test_spawn_meshes.py | 2 +- .../isaaclab/test/sim/test_spawn_sensors.py | 2 +- source/isaaclab/test/sim/test_spawn_shapes.py | 2 +- .../isaaclab/test/sim/test_spawn_wrappers.py | 2 +- .../isaaclab/test/sim/test_urdf_converter.py | 2 +- .../test/terrains/check_terrain_importer.py | 2 +- source/isaaclab_tasks/test/env_test_utils.py | 113 ++--- 46 files changed, 562 insertions(+), 107 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/simulation_manager.py diff --git a/apps/isaaclab.python.kit b/apps/isaaclab.python.kit index de4252f2995..dbf93553c2d 100644 --- a/apps/isaaclab.python.kit +++ b/apps/isaaclab.python.kit @@ -17,7 +17,7 @@ keywords = ["experience", "app", "usd"] "isaacsim.core.api" = {} "isaacsim.core.cloner" = {} "isaacsim.core.nodes" = {} -"isaacsim.core.simulation_manager" = {} +# "isaacsim.core.simulation_manager" = {} # Replaced by isaaclab.sim.simulation_manager "isaacsim.core.throttling" = {} "isaacsim.core.utils" = {} "isaacsim.core.version" = {} diff --git a/apps/isaacsim_4_5/isaaclab.python.kit b/apps/isaacsim_4_5/isaaclab.python.kit index 89db9ffb0d6..101be98e738 100644 --- a/apps/isaacsim_4_5/isaaclab.python.kit +++ b/apps/isaacsim_4_5/isaaclab.python.kit @@ -17,7 +17,7 @@ keywords = ["experience", "app", "usd"] "isaacsim.core.api" = {} "isaacsim.core.cloner" = {} "isaacsim.core.nodes" = {} -"isaacsim.core.simulation_manager" = {} +# "isaacsim.core.simulation_manager" = {} # Replaced by isaaclab.sim.simulation_manager "isaacsim.core.throttling" = {} "isaacsim.core.utils" = {} "isaacsim.core.version" = {} diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 6775a40d070..44439d857f5 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -67,8 +67,7 @@ import time import torch -from isaacsim.core.simulation_manager import SimulationManager - +from isaaclab.sim.simulation_manager import SimulationManager import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.sim.views import XformPrimView diff --git a/scripts/tools/check_instanceable.py b/scripts/tools/check_instanceable.py index fedb771f611..c4a43de487f 100644 --- a/scripts/tools/check_instanceable.py +++ b/scripts/tools/check_instanceable.py @@ -64,7 +64,7 @@ """Rest everything follows.""" -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index 552e9b2d92e..e791ce06b69 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -15,7 +15,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import PhysxSchema, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index f1ab1d05586..ad5285d02be 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -8,7 +8,7 @@ import weakref import omni.physics.tensors.impl.api as physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager import isaaclab.utils.math as math_utils from isaaclab.utils.buffers import TimestampedBuffer diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index 6dc450baca1..26aaf851c89 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -16,7 +16,7 @@ import omni.kit.app import omni.timeline -from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager +from isaaclab.sim import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils from isaaclab.sim.utils.stage import get_current_stage @@ -330,15 +330,16 @@ def _invalidate_initialize_callback(self, event): self._debug_vis_handle.unsubscribe() self._debug_vis_handle = None - def _on_prim_deletion(self, prim_path: str) -> None: + def _on_prim_deletion(self, event) -> None: """Invalidates and deletes the callbacks when the prim is deleted. Args: - prim_path: The path to the prim that is being deleted. + event: The prim deletion event containing the prim path in payload. Note: This function is called when the prim is deleted. """ + prim_path = event.payload["prim_path"] if prim_path == "/": self._clear_callbacks() return @@ -348,8 +349,12 @@ def _on_prim_deletion(self, prim_path: str) -> None: if result: self._clear_callbacks() - def _clear_callbacks(self) -> None: - """Clears the callbacks.""" + def _clear_callbacks(self, event: Any = None) -> None: + """Clears the callbacks. + + Args: + event: Optional event that triggered the callback (unused but required for event handlers). + """ if self._prim_deletion_callback_id: SimulationManager.deregister_callback(self._prim_deletion_callback_id) self._prim_deletion_callback_id = None diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py index 1d2ecffb612..151db451ee2 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import PhysxSchema, UsdShade import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py index 04a31ace553..6e21517fe56 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py index 0c9e24fba12..74873065cf4 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py @@ -12,7 +12,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import UsdPhysics import isaaclab.sim as sim_utils @@ -737,15 +737,16 @@ def _invalidate_initialize_callback(self, event): # set all existing views to None to invalidate them self._root_physx_view = None - def _on_prim_deletion(self, prim_path: str) -> None: + def _on_prim_deletion(self, event) -> None: """Invalidates and deletes the callbacks when the prim is deleted. Args: - prim_path: The path to the prim that is being deleted. + event: The prim deletion event containing the prim path in payload. Note: This function is called when the prim is deleted. """ + prim_path = event.payload["prim_path"] if prim_path == "/": self._clear_callbacks() return diff --git a/source/isaaclab/isaaclab/controllers/rmp_flow.py b/source/isaaclab/isaaclab/controllers/rmp_flow.py index 61dfe6a8cc4..1cbe79e5cf8 100644 --- a/source/isaaclab/isaaclab/controllers/rmp_flow.py +++ b/source/isaaclab/isaaclab/controllers/rmp_flow.py @@ -6,7 +6,7 @@ import torch from dataclasses import MISSING -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.prims import SingleArticulation # enable motion generation extensions diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 8922934cc15..f028dc7eb2c 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -20,7 +20,7 @@ import omni.kit.app import omni.physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from isaaclab.managers import EventManager from isaaclab.scene import InteractiveScene diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 9f03968b0a7..5c544c39a61 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -10,7 +10,7 @@ from typing import Any import omni.physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager from isaaclab.scene import InteractiveScene diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 3a3f4d5c2e9..b873c7df250 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -14,7 +14,7 @@ import carb import omni.physics.tensors.impl.api as physx -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import PhysxSchema import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index 50f75b565e1..2768c58924b 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -11,7 +11,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/imu/imu.py b/source/isaaclab/isaaclab/sensors/imu/imu.py index 1cf0dda12b1..a75683e80f1 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu.py @@ -9,7 +9,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index d7aa07419d4..2d649b618a7 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -14,7 +14,7 @@ from typing import TYPE_CHECKING, ClassVar import omni -from isaacsim.core.simulation_manager import SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 7d6bdf88183..45c7ccdcee4 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -22,7 +22,7 @@ import omni.kit.app import omni.timeline -from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager +from isaaclab.sim import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils from isaaclab.sim.utils.stage import get_current_stage @@ -311,15 +311,16 @@ def _invalidate_initialize_callback(self, event): self._debug_vis_handle.unsubscribe() self._debug_vis_handle = None - def _on_prim_deletion(self, prim_path: str) -> None: + def _on_prim_deletion(self, event) -> None: """Invalidates and deletes the callbacks when the prim is deleted. Args: - prim_path: The path to the prim that is being deleted. + event: The prim deletion event containing the prim path in payload. Note: This function is called when the prim is deleted. """ + prim_path = event.payload["prim_path"] if prim_path == "/": self._clear_callbacks() return diff --git a/source/isaaclab/isaaclab/sim/__init__.py b/source/isaaclab/isaaclab/sim/__init__.py index 1dc920f4e10..ad741eba239 100644 --- a/source/isaaclab/isaaclab/sim/__init__.py +++ b/source/isaaclab/isaaclab/sim/__init__.py @@ -30,6 +30,28 @@ from .schemas import * # noqa: F401, F403 from .simulation_cfg import PhysxCfg, RenderCfg, SimulationCfg # noqa: F401, F403 from .simulation_context import SimulationContext, build_simulation_context # noqa: F401, F403 +from .simulation_manager import SimulationManager, IsaacEvents # noqa: F401, F403 from .spawners import * # noqa: F401, F403 from .utils import * # noqa: F401, F403 from .views import * # noqa: F401, F403 + +# Monkey-patch Isaac Sim's SimulationManager to use Isaac Lab's implementation +# This ensures all code (including Isaac Sim internals) uses our manager +try: + import isaacsim.core.simulation_manager as _isaacsim_sim_manager + import isaacsim.core.simulation_manager.impl.simulation_manager as _isaacsim_sim_manager_impl + + # Get reference to the ORIGINAL Isaac Sim SimulationManager before patching + _OriginalSimManager = _isaacsim_sim_manager_impl.SimulationManager + + # Disable all default callbacks from Isaac Sim's manager to prevent double-dispatch + # These callbacks were already set up when the extension loaded, so we need to disable them + _OriginalSimManager.enable_all_default_callbacks(False) + + # Replace the class in both the module and impl module + _isaacsim_sim_manager.SimulationManager = SimulationManager + _isaacsim_sim_manager.IsaacEvents = IsaacEvents + _isaacsim_sim_manager_impl.SimulationManager = SimulationManager + _isaacsim_sim_manager_impl.IsaacEvents = IsaacEvents +except ImportError: + pass # Isaac Sim extension not loaded yet, will be patched when available diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index dd4f806acef..f0298e9c78c 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 SimulationManager +from isaaclab.sim.simulation_manager import SimulationManager from isaacsim.core.utils.viewports import set_camera_view from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils @@ -292,6 +292,8 @@ def __init__(self, cfg: SimulationCfg | None = None): self._configure_simulation_dt() # apply physics settings (carb and physx) self._apply_physics_settings() + # initialize the simulation manager callbacks + SimulationManager.initialize() # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes # when in headless mode @@ -339,6 +341,8 @@ 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 + # clear the simulation manager state (notifies assets to cleanup) + SimulationManager.clear() # detach the stage from physx if cls._instance._physx_sim_iface is not None: cls._instance._physx_sim_iface.detach_stage() diff --git a/source/isaaclab/isaaclab/sim/simulation_manager.py b/source/isaaclab/isaaclab/sim/simulation_manager.py new file mode 100644 index 00000000000..e980b478f14 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/simulation_manager.py @@ -0,0 +1,414 @@ +# 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 + +"""Simulation manager for Isaac Lab. + +This module manages physics simulation lifecycle, callbacks, and physics views. +""" + +from __future__ import annotations + +import weakref +from collections import OrderedDict +from enum import Enum +from typing import Any, Callable + +import carb +import omni.kit +import omni.physics.tensors +import omni.physx +import omni.timeline +import omni.usd +from pxr import PhysxSchema + +__all__ = ["IsaacEvents", "SimulationManager"] + + +class IsaacEvents(Enum): + """Events dispatched during simulation lifecycle.""" + + PHYSICS_WARMUP = "isaac.physics_warmup" + SIMULATION_VIEW_CREATED = "isaac.simulation_view_created" + PHYSICS_READY = "isaac.physics_ready" + POST_RESET = "isaac.post_reset" + PRIM_DELETION = "isaac.prim_deletion" + PRE_PHYSICS_STEP = "isaac.pre_physics_step" + POST_PHYSICS_STEP = "isaac.post_physics_step" + TIMELINE_STOP = "isaac.timeline_stop" + + +class SimulationManager: + """Manages physics simulation lifecycle and callbacks. + + This is a class-level (singleton-like) manager for the simulation. + """ + + # Core interfaces (names must match Isaac Sim's expected attributes) + _timeline = omni.timeline.get_timeline_interface() + _message_bus = carb.eventdispatcher.get_eventdispatcher() + _physx_interface = omni.physx.get_physx_interface() + _physx_sim_interface = omni.physx.get_physx_simulation_interface() + _carb_settings = carb.settings.get_settings() + + # State + _view: omni.physics.tensors.SimulationView | None = None + _view_warp: omni.physics.tensors.SimulationView | None = None + _backend: str = "torch" + _warmup_needed: bool = True + _view_created: bool = False + _assets_loaded: bool = True + + # Physics scenes (name must match Isaac Sim's expected attribute) + _physics_scene_apis: OrderedDict = OrderedDict() + + # Callbacks: id -> subscription + _callbacks: dict = {} + _callback_id: int = 0 + _handles: dict = {} # Named internal handles + + # Compatibility stub for Isaac Sim code that calls _simulation_manager_interface + class _SimulationManagerInterfaceStub: + """Minimal stub for Isaac Sim compatibility.""" + + @staticmethod + def reset(): + pass + + @staticmethod + def get_simulation_time() -> float: + try: + return omni.physx.get_physx_interface().get_simulation_time() + except Exception: + return 0.0 + + @staticmethod + def is_simulating() -> bool: + try: + return omni.physx.get_physx_interface().is_simulating() + except Exception: + return False + + # No-ops for unused methods + get_num_physics_steps = staticmethod(lambda: 0) + is_paused = staticmethod(lambda: False) + get_callback_iter = staticmethod(lambda: 0) + set_callback_iter = staticmethod(lambda v: None) + register_deletion_callback = staticmethod(lambda cb: None) + register_physics_scene_addition_callback = staticmethod(lambda cb: None) + deregister_callback = staticmethod(lambda id: False) + enable_usd_notice_handler = staticmethod(lambda f: None) + enable_fabric_usd_notice_handler = staticmethod(lambda s, f: None) + is_fabric_usd_notice_handler_enabled = staticmethod(lambda s: False) + get_sample_count = staticmethod(lambda: 0) + get_all_samples = staticmethod(lambda: []) + get_buffer_capacity = staticmethod(lambda: 1000) + get_current_time = staticmethod(lambda: carb.RationalTime(-1, 1)) + get_simulation_time_at_time = staticmethod(lambda t: 0.0) + get_sample_range = staticmethod(lambda: None) + log_statistics = staticmethod(lambda: None) + + _simulation_manager_interface = _SimulationManagerInterfaceStub() + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + @classmethod + def initialize(cls) -> None: + """Initialize the manager and set up timeline callbacks.""" + cls._setup_callbacks() + cls._track_physics_scenes() + + @classmethod + def clear(cls) -> None: + """Clear all state and callbacks.""" + # Notify assets to clean up (PRIM_DELETION with "/" = clear all) + cls.dispatch_prim_deletion("/") + # Properly unsubscribe handles before clearing + # Timeline subscriptions are auto-cleaned by omni.timeline + # Message bus observers just need to be deleted + cls._handles.clear() + cls._callbacks.clear() + # Invalidate views before clearing + if cls._view: + cls._view.invalidate() + cls._view = None + if cls._view_warp: + cls._view_warp.invalidate() + cls._view_warp = None + # Reset state + cls._warmup_needed = True + cls._view_created = False + cls._assets_loaded = True + cls._physics_scene_apis.clear() + + @classmethod + def initialize_physics(cls) -> None: + """Warm-start physics and create simulation views.""" + if not cls._warmup_needed: + return + cls._physx_interface.force_load_physics_from_usd() + cls._physx_interface.start_simulation() + cls._physx_interface.update_simulation(cls.get_physics_dt(), 0.0) + cls._physx_sim_interface.fetch_results() + cls._message_bus.dispatch_event(IsaacEvents.PHYSICS_WARMUP.value, payload={}) + cls._warmup_needed = False + cls._create_views() + + @classmethod + def get_physics_sim_view(cls) -> omni.physics.tensors.SimulationView | None: + """Get the physics simulation view.""" + return cls._view + + @classmethod + def get_backend(cls) -> str: + """Get the tensor backend ("torch" or "warp").""" + return cls._backend + + @classmethod + def set_backend(cls, backend: str) -> None: + """Set the tensor backend.""" + if backend not in ("torch", "warp"): + raise ValueError(f"Backend must be 'torch' or 'warp', got '{backend}'") + cls._backend = backend + + @classmethod + def get_physics_dt(cls) -> float: + """Get the physics timestep in seconds.""" + if cls._physics_scene_apis: + api = list(cls._physics_scene_apis.values())[0] + hz = api.GetTimeStepsPerSecondAttr().Get() + return 1.0 / hz if hz else 0.0 + return 1.0 / 60.0 + + @classmethod + def get_physics_sim_device(cls) -> str: + """Get the physics simulation device.""" + suppress = cls._carb_settings.get_as_bool("/physics/suppressReadback") + if suppress and cls._is_gpu_enabled(): + device_id = max(0, cls._carb_settings.get_as_int("/physics/cudaDevice")) + return f"cuda:{device_id}" + return "cpu" + + @classmethod + def set_physics_sim_device(cls, device: str) -> None: + """Set the physics simulation device.""" + if "cuda" in device: + parts = device.split(":") + device_id = int(parts[1]) if len(parts) > 1 else max(0, cls._carb_settings.get_as_int("/physics/cudaDevice")) + cls._carb_settings.set_int("/physics/cudaDevice", device_id) + cls._carb_settings.set_bool("/physics/suppressReadback", True) + cls._set_gpu_dynamics(True) + cls._enable_fabric(True) + elif device == "cpu": + cls._carb_settings.set_bool("/physics/suppressReadback", False) + cls._set_gpu_dynamics(False) + else: + raise ValueError(f"Device must be 'cuda[:N]' or 'cpu', got '{device}'") + + @classmethod + def assets_loading(cls) -> bool: + """Check if assets are currently loading.""" + return not cls._assets_loaded + + @classmethod + def dispatch_prim_deletion(cls, prim_path: str) -> None: + """Dispatch prim deletion event.""" + cls._message_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": prim_path}) + + # ------------------------------------------------------------------ + # Callback Registration + # ------------------------------------------------------------------ + + @classmethod + def register_callback(cls, callback: Callable, event: IsaacEvents, order: int = 0, name: str | None = None) -> int: + """Register a callback for a simulation event. + + Args: + callback: Function to call when event fires. + event: The event type to listen for. + order: Priority (lower = earlier). + name: Optional name for the callback. + + Returns: + Callback ID for deregistration. + """ + cid = cls._callback_id + cls._callback_id += 1 + + # Handle bound methods with weak references + if hasattr(callback, "__self__"): + obj_ref = weakref.proxy(callback.__self__) + method_name = callback.__name__ + cb = lambda e, o=obj_ref, m=method_name: getattr(o, m)(e) + else: + cb = callback + + if event in (IsaacEvents.PHYSICS_WARMUP, IsaacEvents.PHYSICS_READY, IsaacEvents.POST_RESET, + IsaacEvents.SIMULATION_VIEW_CREATED, IsaacEvents.PRIM_DELETION): + cls._callbacks[cid] = cls._message_bus.observe_event(event_name=event.value, order=order, on_event=cb) + + elif event == IsaacEvents.POST_PHYSICS_STEP: + cls._callbacks[cid] = cls._physx_interface.subscribe_physics_on_step_events( + lambda dt, c=cb: c(dt) if cls._view_created else None, pre_step=False, order=order + ) + + elif event == IsaacEvents.PRE_PHYSICS_STEP: + cls._callbacks[cid] = cls._physx_interface.subscribe_physics_on_step_events( + lambda dt, c=cb: c(dt) if cls._view_created else None, pre_step=True, order=order + ) + + elif event == IsaacEvents.TIMELINE_STOP: + cls._callbacks[cid] = cls._timeline.get_timeline_event_stream().create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), cb, order=order, name=name + ) + else: + raise ValueError(f"Unsupported event: {event}") + + return cid + + @classmethod + def deregister_callback(cls, callback_id: int) -> None: + """Deregister a callback.""" + cls._callbacks.pop(callback_id, None) + + @classmethod + def enable_stage_open_callback(cls, enable: bool) -> None: + """Enable or disable stage open tracking.""" + if enable and "stage_open" not in cls._handles: + cls._handles["stage_open"] = cls._message_bus.observe_event( + event_name=omni.usd.get_context().stage_event_name(omni.usd.StageEventType.OPENED), + on_event=cls._on_stage_open, + ) + elif not enable and "stage_open" in cls._handles: + del cls._handles["stage_open"] + cls._assets_loaded = True + + # ------------------------------------------------------------------ + # Internal + # ------------------------------------------------------------------ + + @classmethod + def _setup_callbacks(cls) -> None: + """Set up internal timeline callbacks.""" + # Guard against duplicate subscriptions + if "play" in cls._handles: + return + stream = cls._timeline.get_timeline_event_stream() + cls._handles["play"] = stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), cls._on_play + ) + cls._handles["stop"] = stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), cls._on_stop + ) + cls.enable_stage_open_callback(True) + + @classmethod + def _on_play(cls, event: Any) -> None: + """Handle timeline play.""" + if cls._carb_settings.get_as_bool("/app/player/playSimulations"): + cls.initialize_physics() + + @classmethod + def _on_stop(cls, event: Any) -> None: + """Handle timeline stop.""" + cls._warmup_needed = True + if cls._view: + cls._view.invalidate() + cls._view = None + if cls._view_warp: + cls._view_warp.invalidate() + cls._view_warp = None + cls._view_created = False + + @classmethod + def _on_stage_open(cls, event: Any) -> None: + """Handle stage open.""" + cls._physics_scene_apis.clear() + cls._callbacks.clear() + cls._track_physics_scenes() + cls._assets_loaded = True + + def on_loading(e): + cls._assets_loaded = False + + def on_loaded(e): + cls._assets_loaded = True + + ctx = omni.usd.get_context() + cls._handles["assets_loading"] = cls._message_bus.observe_event( + event_name=ctx.stage_event_name(omni.usd.StageEventType.ASSETS_LOADING), on_event=on_loading + ) + cls._handles["assets_loaded"] = cls._message_bus.observe_event( + event_name=ctx.stage_event_name(omni.usd.StageEventType.ASSETS_LOADED), on_event=on_loaded + ) + + @classmethod + def _create_views(cls) -> None: + """Create physics simulation views.""" + if cls._view_created: + return + + from isaaclab.sim.utils.stage import get_current_stage_id + + stage_id = get_current_stage_id() + cls._view = omni.physics.tensors.create_simulation_view(cls._backend, stage_id=stage_id) + cls._view.set_subspace_roots("/") + cls._view_warp = omni.physics.tensors.create_simulation_view("warp", stage_id=stage_id) + cls._view_warp.set_subspace_roots("/") + + cls._physx_interface.update_simulation(cls.get_physics_dt(), 0.0) + cls._view_created = True + cls._message_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) + cls._message_bus.dispatch_event(IsaacEvents.PHYSICS_READY.value, payload={}) + + @classmethod + def _track_physics_scenes(cls) -> None: + """Scan stage for physics scenes.""" + stage = omni.usd.get_context().get_stage() + if stage: + for prim in stage.Traverse(): + if prim.GetTypeName() == "PhysicsScene": + cls._physics_scene_apis[prim.GetPath().pathString] = PhysxSchema.PhysxSceneAPI.Apply(prim) + + @classmethod + def _is_gpu_enabled(cls) -> bool: + """Check if GPU dynamics is enabled.""" + if cls._physics_scene_apis: + api = list(cls._physics_scene_apis.values())[0] + bp = api.GetBroadphaseTypeAttr().Get() + gpu_dyn = api.GetEnableGPUDynamicsAttr().Get() + return bp == "GPU" and gpu_dyn + return False + + @classmethod + def _set_gpu_dynamics(cls, enable: bool) -> None: + """Enable/disable GPU dynamics on all physics scenes.""" + bp_type = "GPU" if enable else "MBP" + for api in cls._physics_scene_apis.values(): + if not api.GetPrim().IsValid(): + continue + if api.GetBroadphaseTypeAttr().Get() is None: + api.CreateBroadphaseTypeAttr(bp_type) + else: + api.GetBroadphaseTypeAttr().Set(bp_type) + if api.GetEnableGPUDynamicsAttr().Get() is None: + api.CreateEnableGPUDynamicsAttr(enable) + else: + api.GetEnableGPUDynamicsAttr().Set(enable) + + @classmethod + def _enable_fabric(cls, enable: bool) -> None: + """Enable/disable physics fabric.""" + mgr = omni.kit.app.get_app().get_extension_manager() + was_enabled = mgr.is_extension_enabled("omni.physx.fabric") + if enable and not was_enabled: + mgr.set_extension_enabled_immediate("omni.physx.fabric", True) + elif not enable and was_enabled: + mgr.set_extension_enabled_immediate("omni.physx.fabric", False) + cls._carb_settings.set_bool("/physics/updateToUsd", not enable) + cls._carb_settings.set_bool("/physics/updateParticlesToUsd", not enable) + cls._carb_settings.set_bool("/physics/updateVelocitiesToUsd", not enable) + cls._carb_settings.set_bool("/physics/updateForceSensorsToUsd", not enable) diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index ecdfca45526..bae5735c794 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -435,7 +435,7 @@ def attach_stage_to_usd_context(attaching_early: bool = False): import carb import omni.physx import omni.usd - from isaacsim.core.simulation_manager import SimulationManager + from isaaclab.sim.simulation_manager import SimulationManager from isaaclab.sim.simulation_context import SimulationContext diff --git a/source/isaaclab/isaaclab/ui/widgets/line_plot.py b/source/isaaclab/isaaclab/ui/widgets/line_plot.py index 90a84e201a1..e4c6706a861 100644 --- a/source/isaaclab/isaaclab/ui/widgets/line_plot.py +++ b/source/isaaclab/isaaclab/ui/widgets/line_plot.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext with suppress(ImportError): # isaacsim.gui is not available when running in headless mode. diff --git a/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py b/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py index 685a1be5bcd..b75db289b31 100644 --- a/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py +++ b/source/isaaclab/isaaclab/ui/widgets/manager_live_visualizer.py @@ -12,7 +12,7 @@ from typing import TYPE_CHECKING import omni.kit.app -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaaclab.managers import ManagerBase from isaaclab.utils import configclass diff --git a/source/isaaclab/test/deps/isaacsim/check_ref_count.py b/source/isaaclab/test/deps/isaacsim/check_ref_count.py index 2df1bff66ec..b0d5127e5c8 100644 --- a/source/isaaclab/test/deps/isaacsim/check_ref_count.py +++ b/source/isaaclab/test/deps/isaacsim/check_ref_count.py @@ -40,7 +40,7 @@ import isaacsim.core.utils.nucleus as nucleus_utils import isaacsim.core.utils.prims as prim_utils -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.prims import Articulation # import logger diff --git a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py index 437ec99f4c9..ffb89eafb55 100644 --- a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py +++ b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py @@ -46,7 +46,7 @@ import torch import omni.replicator.core as rep -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.objects import DynamicSphere from isaacsim.core.prims import RigidPrim diff --git a/source/isaaclab/test/devices/check_keyboard.py b/source/isaaclab/test/devices/check_keyboard.py index 4b821bfb113..82259538709 100644 --- a/source/isaaclab/test/devices/check_keyboard.py +++ b/source/isaaclab/test/devices/check_keyboard.py @@ -23,7 +23,7 @@ import ctypes -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg diff --git a/source/isaaclab/test/markers/test_visualization_markers.py b/source/isaaclab/test/markers/test_visualization_markers.py index 7183eb15a03..15615c4d5fd 100644 --- a/source/isaaclab/test/markers/test_visualization_markers.py +++ b/source/isaaclab/test/markers/test_visualization_markers.py @@ -15,7 +15,7 @@ import pytest import torch -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext import isaaclab.sim as sim_utils from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index d7ba46342e8..f01ba84b1ce 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -36,7 +36,7 @@ import torch -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.utils.viewports import set_camera_view diff --git a/source/isaaclab/test/sensors/check_imu_sensor.py b/source/isaaclab/test/sensors/check_imu_sensor.py index 3616b296043..30b16ab0cbe 100644 --- a/source/isaaclab/test/sensors/check_imu_sensor.py +++ b/source/isaaclab/test/sensors/check_imu_sensor.py @@ -40,7 +40,7 @@ import traceback import omni -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.utils.viewports import set_camera_view from pxr import PhysxSchema diff --git a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py index ca5ab4fab34..2b1a084abcc 100644 --- a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py +++ b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py @@ -45,7 +45,7 @@ import random import torch -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import RigidPrim from isaacsim.core.utils.viewports import set_camera_view diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index 1d71eec2416..4554c18e508 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -41,7 +41,7 @@ import torch -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import RigidPrim from isaacsim.core.utils.viewports import set_camera_view diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index a094f57b002..d207be3b16f 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -19,7 +19,7 @@ import tempfile import omni -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_mjcf_converter.py b/source/isaaclab/test/sim/test_mjcf_converter.py index 2dd0c3ed156..d8d2e99dd1d 100644 --- a/source/isaaclab/test/sim/test_mjcf_converter.py +++ b/source/isaaclab/test/sim/test_mjcf_converter.py @@ -15,7 +15,7 @@ import os import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 53437c3a77c..595e5984818 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -15,7 +15,7 @@ import math import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from pxr import UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 02a56af1fe4..5c0084e1d64 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -18,7 +18,7 @@ import weakref import omni.timeline -from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager +from isaaclab.sim import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -27,14 +27,16 @@ @pytest.fixture(autouse=True) def test_setup_teardown(): """Setup and teardown for each test.""" - # Setup: Clear any existing simulation context + # Setup: Clear any existing simulation context and create a fresh stage SimulationContext.clear_instance() + sim_utils.create_new_stage() # Yield for the test yield # Teardown: Clear the simulation context after each test - SimulationContext.clear() + if SimulationContext.instance() is not None: + SimulationContext.clear() SimulationContext.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 a8c84cb8d94..451aa6b0c8b 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -16,7 +16,7 @@ from packaging.version import Version import omni.kit.app -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext import isaaclab.sim as sim_utils from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab/test/sim/test_spawn_lights.py b/source/isaaclab/test/sim/test_spawn_lights.py index f39a795bf8c..e0acb6e01de 100644 --- a/source/isaaclab/test/sim/test_spawn_lights.py +++ b/source/isaaclab/test/sim/test_spawn_lights.py @@ -15,7 +15,7 @@ import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from pxr import Usd, UsdLux import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_spawn_materials.py b/source/isaaclab/test/sim/test_spawn_materials.py index fe104165fca..f5f097aa4f2 100644 --- a/source/isaaclab/test/sim/test_spawn_materials.py +++ b/source/isaaclab/test/sim/test_spawn_materials.py @@ -15,7 +15,7 @@ import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from pxr import UsdPhysics, UsdShade import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index 2884707a633..353a6a42bb5 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -15,7 +15,7 @@ import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_spawn_sensors.py b/source/isaaclab/test/sim/test_spawn_sensors.py index d32bbde24be..bade39d5ba4 100644 --- a/source/isaaclab/test/sim/test_spawn_sensors.py +++ b/source/isaaclab/test/sim/test_spawn_sensors.py @@ -15,7 +15,7 @@ import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from pxr import Usd import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index eb4b8fa5aa7..ebbf8684737 100644 --- a/source/isaaclab/test/sim/test_spawn_shapes.py +++ b/source/isaaclab/test/sim/test_spawn_shapes.py @@ -14,7 +14,7 @@ import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index 35b3a875839..6b238611cf9 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -15,7 +15,7 @@ import pytest -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext import isaaclab.sim as sim_utils from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab/test/sim/test_urdf_converter.py b/source/isaaclab/test/sim/test_urdf_converter.py index 6f4a2120444..06f3ecdd4ff 100644 --- a/source/isaaclab/test/sim/test_urdf_converter.py +++ b/source/isaaclab/test/sim/test_urdf_converter.py @@ -18,7 +18,7 @@ from packaging.version import Version import omni.kit.app -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.prims import Articulation import isaaclab.sim as sim_utils diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index 1c6ae2c2050..7bb766c7130 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -69,7 +69,7 @@ from isaacsim.core.api.materials import PhysicsMaterial from isaacsim.core.api.materials.preview_surface import PreviewSurface from isaacsim.core.api.objects import DynamicSphere -from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.sim import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import RigidPrim, SingleGeometryPrim, SingleRigidPrim from isaacsim.core.utils.extensions import enable_extension diff --git a/source/isaaclab_tasks/test/env_test_utils.py b/source/isaaclab_tasks/test/env_test_utils.py index 93d926218dc..fc758852f61 100644 --- a/source/isaaclab_tasks/test/env_test_utils.py +++ b/source/isaaclab_tasks/test/env_test_utils.py @@ -184,6 +184,9 @@ def _check_random_actions( create_stage_in_memory: Whether to create stage in memory. disable_clone_in_fabric: Whether to disable fabric cloning. """ + # Import here to use in exception handler + from isaaclab.sim import SimulationContext + # create a new context stage, if stage in memory is not enabled if not create_stage_in_memory: omni.usd.get_context().new_stage() @@ -191,6 +194,7 @@ def _check_random_actions( # reset the rtx sensors carb setting to False carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + env = None try: # parse config env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) @@ -211,64 +215,67 @@ def _check_random_actions( return env = gym.make(task_name, cfg=env_cfg) - except Exception as e: - # try to close environment on exception - if "env" in locals() and hasattr(env, "_is_closed"): - env.close() - else: - if hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): - e.obj.close() - pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") + # disable control on stop + env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore - # disable control on stop - env.unwrapped.sim._app_control_on_stop_handle = None # type: ignore + # override action space if set to inf for `Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0` + if task_name == "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0": + for i in range(env.unwrapped.single_action_space.shape[0]): + if env.unwrapped.single_action_space.low[i] == float("-inf"): + env.unwrapped.single_action_space.low[i] = -1.0 + if env.unwrapped.single_action_space.high[i] == float("inf"): + env.unwrapped.single_action_space.low[i] = 1.0 - # override action space if set to inf for `Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0` - if task_name == "Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0": - for i in range(env.unwrapped.single_action_space.shape[0]): - if env.unwrapped.single_action_space.low[i] == float("-inf"): - env.unwrapped.single_action_space.low[i] = -1.0 - if env.unwrapped.single_action_space.high[i] == float("inf"): - env.unwrapped.single_action_space.low[i] = 1.0 - - # reset environment - obs, _ = env.reset() - - # check signal - assert _check_valid_tensor(obs) - - # simulate environment for num_steps - with torch.inference_mode(): - for _ in range(num_steps): - # sample actions according to the defined space - if multi_agent: - actions = { - agent: sample_space( - env.unwrapped.action_spaces[agent], device=env.unwrapped.device, batch_size=num_envs - ) - for agent in env.unwrapped.possible_agents - } - else: - actions = sample_space( - env.unwrapped.single_action_space, device=env.unwrapped.device, batch_size=num_envs - ) - # apply actions - transition = env.step(actions) - # check signals - for data in transition[:-1]: # exclude info + # reset environment + obs, _ = env.reset() + + # check signal + assert _check_valid_tensor(obs) + + # simulate environment for num_steps + with torch.inference_mode(): + for _ in range(num_steps): + # sample actions according to the defined space if multi_agent: - for agent, agent_data in data.items(): - assert _check_valid_tensor(agent_data), f"Invalid data ('{agent}'): {agent_data}" + actions = { + agent: sample_space( + env.unwrapped.action_spaces[agent], device=env.unwrapped.device, batch_size=num_envs + ) + for agent in env.unwrapped.possible_agents + } else: - assert _check_valid_tensor(data), f"Invalid data: {data}" - - # close environment - env.close() + actions = sample_space( + env.unwrapped.single_action_space, device=env.unwrapped.device, batch_size=num_envs + ) + # apply actions + transition = env.step(actions) + # check signals + for data in transition[:-1]: # exclude info + if multi_agent: + for agent, agent_data in data.items(): + assert _check_valid_tensor(agent_data), f"Invalid data ('{agent}'): {agent_data}" + else: + assert _check_valid_tensor(data), f"Invalid data: {data}" + + # 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() - # 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() + except Exception as e: + # try to close environment on exception + if env is not None and hasattr(env, "_is_closed"): + env.close() + elif hasattr(e, "obj") and hasattr(e.obj, "_is_closed"): + e.obj.close() + else: + # Ensure simulation context is cleared even if env creation failed + SimulationContext.clear_instance() + + pytest.fail(f"Failed to set-up the environment for task {task_name}. Error: {e}") def _check_valid_tensor(data: torch.Tensor | dict) -> bool: From 83d7c8b0efdb6ccdbaedfc38702312a8f89cd48d Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 07:05:54 -0800 Subject: [PATCH 103/130] refactor step 1: removed has_gui api and refactor animation related code our of simulation context --- .../tutorials/04_sensors/run_usd_camera.py | 4 +- .../isaaclab/isaaclab/envs/direct_marl_env.py | 4 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 4 +- .../isaaclab/envs/manager_based_env.py | 4 +- .../isaaclab/envs/manager_based_rl_env.py | 2 +- .../isaaclab/sim/simulation_context.py | 309 ++++++++++-------- .../test/sim/test_simulation_context.py | 2 +- .../direct/automate/disassembly_env.py | 2 +- 8 files changed, 178 insertions(+), 153 deletions(-) diff --git a/scripts/tutorials/04_sensors/run_usd_camera.py b/scripts/tutorials/04_sensors/run_usd_camera.py index 0b953db0d17..0dd99bb335a 100644 --- a/scripts/tutorials/04_sensors/run_usd_camera.py +++ b/scripts/tutorials/04_sensors/run_usd_camera.py @@ -195,7 +195,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict): camera_index = args_cli.camera_id # Create the markers for the --draw option outside of is_running() loop - if sim.has_gui() and args_cli.draw: + if sim.carb_settings.get("/isaaclab/has_gui") and args_cli.draw: cfg = RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/CameraPointCloud") cfg.markers["hit"].radius = 0.002 pc_markers = VisualizationMarkers(cfg) @@ -247,7 +247,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict): rep_writer.write(rep_output) # Draw pointcloud if there is a GUI and --draw has been passed - if sim.has_gui() and args_cli.draw and "distance_to_image_plane" in camera.data.output.keys(): + if sim.carb_settings.get("/isaaclab/has_gui") and args_cli.draw and "distance_to_image_plane" in camera.data.output.keys(): # Derive pointcloud from camera at camera_index pointcloud = create_pointcloud_from_depth( intrinsic_matrix=camera.data.intrinsic_matrices[camera_index], diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index dbc6bb4c966..2a3fc91db82 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -168,7 +168,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # extend UI elements # we need to do this here after all the managers are initialized # this is because they dictate the sensors and commands right now - if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + if self.sim.carb_settings.get("/isaaclab/has_gui") and self.cfg.ui_window_class_type is not None: self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") else: # if no window, then we don't need to store the window @@ -370,7 +370,7 @@ def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() # perform physics stepping for _ in range(self.cfg.decimation): diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index f028dc7eb2c..c3e968a977b 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -175,7 +175,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # extend UI elements # we need to do this here after all the managers are initialized # this is because they dictate the sensors and commands right now - if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + if self.sim.carb_settings.get("/isaaclab/has_gui") and self.cfg.ui_window_class_type is not None: self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") else: # if no window, then we don't need to store the window @@ -360,7 +360,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() # perform physics stepping for _ in range(self.cfg.decimation): diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 5c544c39a61..4ed1ad26d32 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -173,7 +173,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # extend UI elements # we need to do this here after all the managers are initialized # this is because they dictate the sensors and commands right now - if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + if self.sim.carb_settings.get("/isaaclab/has_gui") and self.cfg.ui_window_class_type is not None: # setup live visualizers self.setup_manager_visualizers() self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") @@ -461,7 +461,7 @@ def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() # perform physics stepping for _ in range(self.cfg.decimation): diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py index 44f6ed07ce1..8a875e132e1 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py @@ -174,7 +174,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() # perform physics stepping for _ in range(self.cfg.decimation): diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index f0298e9c78c..1dda05338bb 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -43,6 +43,157 @@ logger = logging.getLogger(__name__) +class AnimationRecorder: + """Handles animation recording for the simulation. + + This class manages the recording of physics animations using the PhysX PVD + (Physics Visual Debugger) interface. It handles the setup, update, and + finalization of animation recordings. + """ + + def __init__(self, carb_settings: carb.settings.ISettings, app_iface: omni.kit.app.IApp): + """Initialize the animation recorder. + + Args: + carb_settings: The Carbonite settings interface. + app_iface: The Omniverse Kit application interface. + """ + self._carb_settings = carb_settings + self._app_iface = app_iface + self._enabled = False + self._start_time: float = 0.0 + self._stop_time: float = 0.0 + self._started_timestamp: float | None = None + self._output_dir: str = "" + self._timestamp: str = "" + self._physx_pvd_interface = None + + self._setup() + + @property + def enabled(self) -> bool: + """Whether animation recording is enabled.""" + return self._enabled + + def _setup(self) -> None: + """Sets up animation recording settings and initializes the recording.""" + self._enabled = bool(self._carb_settings.get("/isaaclab/anim_recording/enabled")) + if not self._enabled: + return + + # Import omni.physx.pvd.bindings here since it is not available by default + from omni.physxpvd.bindings import _physxPvd + + # Init anim recording settings + self._start_time = self._carb_settings.get("/isaaclab/anim_recording/start_time") + self._stop_time = self._carb_settings.get("/isaaclab/anim_recording/stop_time") + self._started_timestamp = None + + # Make output path relative to repo path + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + self._timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") + self._output_dir = ( + os.path.join(repo_path, "anim_recordings", self._timestamp).replace("\\", "/").rstrip("/") + "/" + ) + os.makedirs(self._output_dir, exist_ok=True) + + # Acquire physx pvd interface and set output directory + self._physx_pvd_interface = _physxPvd.acquire_physx_pvd_interface() + + # Set carb settings for the output path and enabling pvd recording + self._carb_settings.set_string("/persistent/physics/omniPvdOvdRecordingDirectory", self._output_dir) + self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) + + def update(self) -> bool: + """Tracks timestamps and triggers finish if total time has elapsed. + + Returns: + True if animation recording has finished, False otherwise. + """ + if not self._enabled: + return False + + if self._started_timestamp is None: + self._started_timestamp = time.time() + + total_time = time.time() - self._started_timestamp + if total_time > self._stop_time: + self._finish() + return True + return False + + def _finish(self) -> bool: + """Finishes the animation recording and outputs the baked animation recording. + + Returns: + True if the recording was successfully finished. + """ + logger.warning( + "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." + ) + + # Detaching the stage will also close it and force the serialization of the OVD file + physx = omni.physx.get_physx_simulation_interface() + physx.detach_stage() + + # Save stage to disk + stage_path = os.path.join(self._output_dir, "stage_simulation.usdc") + sim_utils.save_stage(stage_path, save_and_reload_in_place=False) + + # Find the latest ovd file not named tmp.ovd + ovd_files = [f for f in glob.glob(os.path.join(self._output_dir, "*.ovd")) if not f.endswith("tmp.ovd")] + input_ovd_path = max(ovd_files, key=os.path.getctime) + + # Invoke pvd interface to create recording + stage_filename = "baked_animation_recording.usda" + result = self._physx_pvd_interface.ovd_to_usd_over_with_layer_creation( + input_ovd_path, + stage_path, + self._output_dir, + stage_filename, + self._start_time, + self._stop_time, + True, # True: ASCII layers / False : USDC layers + False, # True: verify over layer + ) + + # Workaround for manually setting the truncated start time in the baked animation recording + self._update_usda_start_time(os.path.join(self._output_dir, stage_filename), self._start_time) + + # Disable recording + self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) + + return result + + @staticmethod + def _update_usda_start_time(file_path: str, start_time: float) -> None: + """Updates the start time of the USDA baked animation recording file. + + Args: + file_path: Path to the USDA file. + start_time: The new start time to set. + """ + # Read the USDA file + with open(file_path) as file: + content = file.read() + + # Extract the timeCodesPerSecond value + time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) + if not time_code_match: + raise ValueError("timeCodesPerSecond not found in the file.") + time_codes_per_second = int(time_code_match.group(1)) + + # Compute the new start time code + new_start_time_code = int(start_time * time_codes_per_second) + + # Replace the startTimeCode in the file + content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) + + # Write the updated content back to the file + with open(file_path, "w") as file: + file.write(content) + + class SimulationContext: """A class to control simulation-related events such as physics stepping and rendering. @@ -180,9 +331,6 @@ def __init__(self, cfg: SimulationCfg | None = None): # read flag for whether XR GUI is enabled self._xr_gui = self.carb_settings.get("/app/xr/enabled") - # read flags anim recording config and init timestamps - self._setup_anim_recording() - # read flag for whether the Isaac Lab viewport capture pipeline will be used, # casting None to False if the flag doesn't exist # this flag is set from the AppLauncher class @@ -190,20 +338,20 @@ def __init__(self, cfg: SimulationCfg | None = None): # read flag for whether the default viewport should be enabled self._render_viewport = bool(self.carb_settings.get("/isaaclab/render/active_viewport")) # flag for whether any GUI will be rendered (local, livestreamed or viewport) - self._has_gui = self._local_gui or self._livestream_gui or self._xr_gui - + has_gui = self._local_gui or self._livestream_gui or self._xr_gui + self.carb_settings.set_bool("/isaaclab/has_gui", has_gui) # apply render settings from render config self._apply_render_settings_from_cfg() # store the default render mode - if not self._has_gui and not self._offscreen_render: + if not self.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: # set default render mode # note: this is the terminal state: cannot exit from this render mode self.render_mode = self.RenderMode.NO_GUI_OR_RENDERING # set viewport context to None self._viewport_context = None self._viewport_window = None - elif not self._has_gui and self._offscreen_render: + elif not self.carb_settings.get("/isaaclab/has_gui") and self._offscreen_render: # set default render mode # note: this is the terminal state: cannot exit from this render mode self.render_mode = self.RenderMode.PARTIAL_RENDERING @@ -239,7 +387,7 @@ def __init__(self, cfg: SimulationCfg | None = None): # override enable scene querying if rendering is enabled # this is needed for some GUI features - if self._has_gui: + if self.carb_settings.get("/isaaclab/has_gui"): self.cfg.enable_scene_query_support = True # create a tensor for gravity @@ -268,6 +416,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() + # initialize animation recorder + self._anim_recorder = AnimationRecorder(self.carb_settings, self._app_iface) + # set timeline auto update to True self._timeline_iface.set_auto_update(True) @@ -388,13 +539,6 @@ def physics_sim_view(self) -> omni.physics.tensors.SimulationView: Operations - Simulation Information. """ - def has_gui(self) -> bool: - """Returns whether the simulation has a GUI enabled. - - True if the simulation has a GUI enabled either locally or live-streamed. - """ - return self._has_gui - def has_rtx_sensors(self) -> bool: """Returns whether the simulation has any RTX-rendering related sensors. @@ -499,7 +643,7 @@ def set_camera_view( "/OmniverseKit_Persp". """ # safe call only if we have a GUI or viewport rendering enabled - if self._has_gui or self._offscreen_render or self._render_viewport: + if self.carb_settings.get("/isaaclab/has_gui") or self._offscreen_render or self._render_viewport: set_camera_view(eye, target, camera_prim_path) def set_render_mode(self, mode: RenderMode): @@ -520,7 +664,7 @@ def set_render_mode(self, mode: RenderMode): ValueError: If the input mode is not supported. """ # check if mode change is possible -- not possible when no GUI is available - if not self._has_gui: + if not self.carb_settings.get("/isaaclab/has_gui"): self.logger.warning( f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." ) @@ -707,12 +851,10 @@ 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. """ - # 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("Animation recording finished. Closing app.") - self._app_iface.shutdown() + # update animation recorder if enabled + if self._anim_recorder.enabled and self._anim_recorder.update(): + 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 if not self.is_playing(): @@ -1134,123 +1276,6 @@ def _check_for_callback_exceptions(self): # re-enable simulation stopping control self._disable_app_control_on_stop_handle = False - """ - Helper functions - Animation Recording. - """ - - def _update_anim_recording(self): - """Tracks anim recording timestamps and triggers finish animation recording if the total time has elapsed.""" - if self._anim_recording_started_timestamp is None: - self._anim_recording_started_timestamp = time.time() - - if self._anim_recording_started_timestamp is not None: - anim_recording_total_time = time.time() - self._anim_recording_started_timestamp - if anim_recording_total_time > self._anim_recording_stop_time: - self._finish_anim_recording() - return True - return False - - def _setup_anim_recording(self): - """Sets up anim recording settings and initializes the recording.""" - - self._anim_recording_enabled = bool(self.carb_settings.get("/isaaclab/anim_recording/enabled")) - if not self._anim_recording_enabled: - return - - # Import omni.physx.pvd.bindings here since it is not available by default - from omni.physxpvd.bindings import _physxPvd - - # Init anim recording settings - self._anim_recording_start_time = self.carb_settings.get("/isaaclab/anim_recording/start_time") - self._anim_recording_stop_time = self.carb_settings.get("/isaaclab/anim_recording/stop_time") - self._anim_recording_first_step_timestamp = None - self._anim_recording_started_timestamp = None - - # Make output path relative to repo path - repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") - self._anim_recording_timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") - self._anim_recording_output_dir = ( - os.path.join(repo_path, "anim_recordings", self._anim_recording_timestamp).replace("\\", "/").rstrip("/") - + "/" - ) - os.makedirs(self._anim_recording_output_dir, exist_ok=True) - - # Acquire physx pvd interface and set output directory - self._physxPvdInterface = _physxPvd.acquire_physx_pvd_interface() - - # Set carb settings for the output path and enabling pvd recording - self.carb_settings.set_string( - "/persistent/physics/omniPvdOvdRecordingDirectory", self._anim_recording_output_dir - ) - self.carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) - - def _update_usda_start_time(self, file_path, start_time): - """Updates the start time of the USDA baked anim recordingfile.""" - - # Read the USDA file - with open(file_path) as file: - content = file.read() - - # Extract the timeCodesPerSecond value - time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) - if not time_code_match: - raise ValueError("timeCodesPerSecond not found in the file.") - time_codes_per_second = int(time_code_match.group(1)) - - # Compute the new start time code - new_start_time_code = int(start_time * time_codes_per_second) - - # Replace the startTimeCode in the file - content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) - - # Write the updated content back to the file - with open(file_path, "w") as file: - file.write(content) - - def _finish_anim_recording(self): - """Finishes the animation recording and outputs the baked animation recording.""" - - logger.warning( - "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." - ) - - # Detaching the stage will also close it and force the serialization of the OVD file - physx = omni.physx.get_physx_simulation_interface() - physx.detach_stage() - - # Save stage to disk - stage_path = os.path.join(self._anim_recording_output_dir, "stage_simulation.usdc") - sim_utils.save_stage(stage_path, save_and_reload_in_place=False) - - # Find the latest ovd file not named tmp.ovd - ovd_files = [ - f for f in glob.glob(os.path.join(self._anim_recording_output_dir, "*.ovd")) if not f.endswith("tmp.ovd") - ] - input_ovd_path = max(ovd_files, key=os.path.getctime) - - # Invoke pvd interface to create recording - stage_filename = "baked_animation_recording.usda" - result = self._physxPvdInterface.ovd_to_usd_over_with_layer_creation( - input_ovd_path, - stage_path, - self._anim_recording_output_dir, - stage_filename, - self._anim_recording_start_time, - self._anim_recording_stop_time, - True, # True: ASCII layers / False : USDC layers - False, # True: verify over layer - ) - - # Workaround for manually setting the truncated start time in the baked animation recording - self._update_usda_start_time( - os.path.join(self._anim_recording_output_dir, stage_filename), self._anim_recording_start_time - ) - - # Disable recording - self.carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) - - return result - """ Callbacks. """ @@ -1353,7 +1378,7 @@ def build_simulation_context( cfg = GroundPlaneCfg() cfg.func("/World/defaultGroundPlane", cfg) - if add_lighting or (auto_add_lighting and sim.has_gui()): + if add_lighting or (auto_add_lighting and sim.carb_settings.get("/isaaclab/has_gui")): # Lighting cfg = DomeLightCfg( color=(0.1, 0.1, 0.1), @@ -1370,7 +1395,7 @@ def build_simulation_context( sim.logger.error(traceback.format_exc()) raise finally: - if not sim.has_gui(): + if not sim.carb_settings.get("/isaaclab/has_gui"): # Stop simulation only if we aren't rendering otherwise the app will hang indefinitely sim.stop() diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 5c0084e1d64..d4caef54bf6 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -145,7 +145,7 @@ 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() + assert not sim.carb_settings.get("/isaaclab/has_gui") """ diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py index b78c80a71b9..3e2231e0836 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py @@ -570,7 +570,7 @@ def _move_gripper_to_eef_pose(self, env_ids, goal_pos, goal_quat, sim_steps, if_ self.actions *= 0.0 self.actions[env_ids, :6] = delta_hand_pose - is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() # perform physics stepping for _ in range(self.cfg.decimation): self._sim_step_counter += 1 From 1f297d146bc0cb1b523050ca5d662f0bd7c1ec90 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 07:11:19 -0800 Subject: [PATCH 104/130] refactor step 2: removed has rtx sensor api --- .../isaaclab/isaaclab/envs/direct_marl_env.py | 4 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 10 ++--- .../isaaclab/envs/manager_based_env.py | 8 ++-- .../isaaclab/envs/manager_based_rl_env.py | 6 +-- .../isaaclab/sim/simulation_context.py | 16 -------- source/isaaclab/test/sensors/test_camera.py | 2 +- .../test/sensors/test_multi_tiled_camera.py | 6 +-- .../test/sensors/test_tiled_camera.py | 38 +++++++++---------- .../test/sim/test_simulation_context.py | 2 +- .../direct/automate/disassembly_env.py | 2 +- 10 files changed, 39 insertions(+), 55 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 2a3fc91db82..822798926f2 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -370,7 +370,7 @@ def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # perform physics stepping for _ in range(self.cfg.decimation): @@ -488,7 +488,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: """ # run a rendering step of the simulator # if we have rtx sensors, we do not need to render again sin - if not self.sim.has_rtx_sensors() and not recompute: + if not self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and not recompute: self.sim.render() # decide the rendering mode if self.render_mode == "human" or self.render_mode is None: diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index c3e968a977b..dd721359398 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -315,11 +315,11 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) self.sim.forward() # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + if self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and self.cfg.num_rerenders_on_reset > 0: for _ in range(self.cfg.num_rerenders_on_reset): self.sim.render() - if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): + if self.cfg.wait_for_textures and self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors"): while SimulationManager.assets_loading(): self.sim.render() @@ -360,7 +360,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # perform physics stepping for _ in range(self.cfg.decimation): @@ -393,7 +393,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: if len(reset_env_ids) > 0: self._reset_idx(reset_env_ids) # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + if self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and self.cfg.num_rerenders_on_reset > 0: for _ in range(self.cfg.num_rerenders_on_reset): self.sim.render() @@ -457,7 +457,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: """ # run a rendering step of the simulator # if we have rtx sensors, we do not need to render again sin - if not self.sim.has_rtx_sensors() and not recompute: + if not self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and not recompute: self.sim.render() # decide the rendering mode if self.render_mode == "human" or self.render_mode is None: diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 4ed1ad26d32..88336ddfa85 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -365,7 +365,7 @@ def reset( self.scene.write_data_to_sim() self.sim.forward() # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + if self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and self.cfg.num_rerenders_on_reset > 0: for _ in range(self.cfg.num_rerenders_on_reset): self.sim.render() @@ -375,7 +375,7 @@ def reset( # compute observations self.obs_buf = self.observation_manager.compute(update_history=True) - if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): + if self.cfg.wait_for_textures and self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors"): while SimulationManager.assets_loading(): self.sim.render() @@ -426,7 +426,7 @@ def reset_to( self.sim.forward() # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + if self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and self.cfg.num_rerenders_on_reset > 0: for _ in range(self.cfg.num_rerenders_on_reset): self.sim.render() @@ -461,7 +461,7 @@ def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # perform physics stepping for _ in range(self.cfg.decimation): diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py index 8a875e132e1..22bfdfc21dc 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py @@ -174,7 +174,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: # check if we need to do rendering within the physics loop # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # perform physics stepping for _ in range(self.cfg.decimation): @@ -219,7 +219,7 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: self._reset_idx(reset_env_ids) # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + if self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and self.cfg.num_rerenders_on_reset > 0: for _ in range(self.cfg.num_rerenders_on_reset): self.sim.render() @@ -262,7 +262,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: """ # run a rendering step of the simulator # if we have rtx sensors, we do not need to render again sin - if not self.sim.has_rtx_sensors() and not recompute: + if not self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") and not recompute: self.sim.render() # decide the rendering mode if self.render_mode == "human" or self.render_mode is None: diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 1dda05338bb..97ca8144244 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -538,22 +538,6 @@ def physics_sim_view(self) -> omni.physics.tensors.SimulationView: """ Operations - Simulation Information. """ - - def has_rtx_sensors(self) -> bool: - """Returns whether the simulation has any RTX-rendering related sensors. - - This function returns the value of the simulation parameter ``"/isaaclab/render/rtx_sensors"``. - The parameter is set to True when instances of RTX-related sensors (cameras or LiDARs) are - created using Isaac Lab's sensor classes. - - True if the simulation has RTX sensors (such as USD Cameras or LiDARs). - - For more information, please check `NVIDIA RTX documentation`_. - - .. _NVIDIA RTX documentation: https://developer.nvidia.com/rendering-technologies - """ - return self.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") - def is_fabric_enabled(self) -> bool: """Returns whether the fabric interface is enabled. diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index cecbadd0936..7b8d59b998f 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -97,7 +97,7 @@ def test_camera_init(setup_sim_camera): # Create camera camera = Camera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized diff --git a/source/isaaclab/test/sensors/test_multi_tiled_camera.py b/source/isaaclab/test/sensors/test_multi_tiled_camera.py index 59a1b673759..158752b9d3c 100644 --- a/source/isaaclab/test/sensors/test_multi_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_multi_tiled_camera.py @@ -84,7 +84,7 @@ def test_multi_tiled_camera_init(setup_camera): tiled_cameras.append(camera) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() @@ -180,7 +180,7 @@ def test_all_annotators_multi_tiled_camera(setup_camera): tiled_cameras.append(camera) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() @@ -281,7 +281,7 @@ def test_different_resolution_multi_tiled_camera(setup_camera): tiled_cameras.append(camera) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() diff --git a/source/isaaclab/test/sensors/test_tiled_camera.py b/source/isaaclab/test/sensors/test_tiled_camera.py index 9dc059a68ab..b1176e07ea9 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_tiled_camera.py @@ -71,7 +71,7 @@ def test_single_camera_init(setup_camera, device): # Create camera camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -250,7 +250,7 @@ def test_multi_camera_init(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -307,7 +307,7 @@ def test_rgb_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -409,7 +409,7 @@ def test_depth_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -463,7 +463,7 @@ def test_rgba_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -517,7 +517,7 @@ def test_distance_to_camera_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -571,7 +571,7 @@ def test_distance_to_image_plane_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -625,7 +625,7 @@ def test_normals_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -682,7 +682,7 @@ def test_motion_vectors_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -736,7 +736,7 @@ def test_semantic_segmentation_colorize_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -791,7 +791,7 @@ def test_instance_segmentation_fast_colorize_only_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -846,7 +846,7 @@ def test_instance_id_segmentation_fast_colorize_only_camera(setup_camera, device camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -902,7 +902,7 @@ def test_semantic_segmentation_non_colorize_only_camera(setup_camera, device): camera_cfg.colorize_semantic_segmentation = False camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -959,7 +959,7 @@ def test_instance_segmentation_fast_non_colorize_only_camera(setup_camera, devic camera_cfg.colorize_instance_segmentation = False camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -1014,7 +1014,7 @@ def test_instance_id_segmentation_fast_non_colorize_only_camera(setup_camera, de camera_cfg.colorize_instance_id_segmentation = False camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -1082,7 +1082,7 @@ def test_all_annotators_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -1184,7 +1184,7 @@ def test_all_annotators_low_resolution_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -1284,7 +1284,7 @@ def test_all_annotators_non_perfect_square_number_camera(setup_camera, device): camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized @@ -1404,7 +1404,7 @@ def test_all_annotators_instanceable(setup_camera, device): camera_cfg.offset.pos = (0.0, 0.0, 5.5) camera = TiledCamera(camera_cfg) # Check simulation parameter is set correctly - assert sim.has_rtx_sensors() + assert sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # Play sim sim.reset() # Check if camera is initialized diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index d4caef54bf6..a9b239d8ff2 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -62,7 +62,7 @@ def test_init(device): # verify device property assert sim.device == device # verify no RTX sensors are available - assert not sim.has_rtx_sensors() + assert not sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # obtain physics scene api physx_scene_api = sim._physx_scene_api # type: ignore diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py index 3e2231e0836..950bbfc2fda 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py @@ -570,7 +570,7 @@ def _move_gripper_to_eef_pose(self, env_ids, goal_pos, goal_quat, sim_steps, if_ self.actions *= 0.0 self.actions[env_ids, :6] = delta_hand_pose - is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.has_rtx_sensors() + is_rendering = self.sim.carb_settings.get("/isaaclab/has_gui") or self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") # perform physics stepping for _ in range(self.cfg.decimation): self._sim_step_counter += 1 From 8d277e936fb989541c5621f5caf850c29d56325e Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 07:20:38 -0800 Subject: [PATCH 105/130] refactor step 3: removed is_fabric_enabled api --- .../isaaclab/isaaclab/envs/ui/base_env_window.py | 2 +- source/isaaclab/isaaclab/sim/simulation_context.py | 14 +------------- .../isaaclab/test/sim/test_simulation_context.py | 2 +- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 2aafe5e6bba..52570b31b7a 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -149,7 +149,7 @@ def _build_sim_frame(self): **record_animate_cfg ) # disable the button if fabric is not enabled - self.ui_window_elements["record_animation"].enabled = not self.env.sim.is_fabric_enabled() + self.ui_window_elements["record_animation"].enabled = not self.env.sim..carb_settings.get_as_bool("/isaaclab/fabric_enabled") def _build_viewer_frame(self): """Build the viewer-related control frame for the UI.""" diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 97ca8144244..bc3db403c6d 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -538,19 +538,6 @@ def physics_sim_view(self) -> omni.physics.tensors.SimulationView: """ Operations - Simulation Information. """ - def is_fabric_enabled(self) -> bool: - """Returns whether the fabric interface is enabled. - - When fabric interface is enabled, USD read/write operations are disabled. Instead all applications - read and write the simulation state directly from the fabric interface. This reduces a lot of overhead - that occurs during USD read/write operations. - - For more information, please check `Fabric documentation`_. - - .. _Fabric documentation: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/usd_fabric_usdrt.html - """ - return self._fabric_iface is not None - def get_version(self) -> tuple[int, int, int]: """Returns the version of the simulator. @@ -1222,6 +1209,7 @@ def _load_fabric_interface(self): # acquire fabric interface self._fabric_iface = get_physx_fabric_interface() + self.carb_settings.set_bool("/isaaclab/fabric_enabled", True) if hasattr(self._fabric_iface, "force_update"): # The update method in the fabric interface only performs an update if a physics step has occurred. # However, for rendering, we need to force an update since any element of the scene might have been diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index a9b239d8ff2..fdee7454c13 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -368,7 +368,7 @@ def test_fabric_setting(use_fabric): sim = SimulationContext(cfg) # check fabric is enabled - assert sim.is_fabric_enabled() == use_fabric + assert sim..carb_settings.get_as_bool("/isaaclab/fabric_enabled") == use_fabric @pytest.mark.isaacsim_ci From 3707bb01ab8d309ae91ca70a0da76397020d559c Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 07:24:55 -0800 Subject: [PATCH 106/130] refactor step 4: remove get_initial_stage api --- docs/source/how-to/optimize_stage_creation.rst | 2 +- source/isaaclab/isaaclab/envs/direct_marl_env.py | 4 ++-- source/isaaclab/isaaclab/envs/direct_rl_env.py | 4 ++-- source/isaaclab/isaaclab/envs/manager_based_env.py | 4 ++-- source/isaaclab/isaaclab/sim/simulation_context.py | 8 -------- source/isaaclab/test/sim/test_simulation_context.py | 2 +- .../isaaclab/test/sim/test_simulation_stage_in_memory.py | 6 +++--- 7 files changed, 11 insertions(+), 19 deletions(-) diff --git a/docs/source/how-to/optimize_stage_creation.rst b/docs/source/how-to/optimize_stage_creation.rst index b262878d667..cbffce377e8 100644 --- a/docs/source/how-to/optimize_stage_creation.rst +++ b/docs/source/how-to/optimize_stage_creation.rst @@ -60,7 +60,7 @@ be called after the stage is created. sim = SimulationContext(cfg=SimulationCfg(create_stage_in_memory=True)) # grab stage in memory and set stage context - stage_in_memory = sim.get_initial_stage() + stage_in_memory = sim.stage with stage_utils.use_stage(stage_in_memory): # create cartpole scene scene_cfg = CartpoleSceneCfg(num_envs=1024) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 822798926f2..807cedd0ee2 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -121,7 +121,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # generate scene with Timer("[INFO]: Time taken for scene creation", "scene_creation"): # set the stage context for scene creation steps which use the stage - with use_stage(self.sim.get_initial_stage()): + with use_stage(self.sim.stage): self.scene = InteractiveScene(self.cfg.scene) self._setup_scene() attach_stage_to_usd_context() @@ -153,7 +153,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar 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()): + with use_stage(self.sim.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. diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index dd721359398..d8fa1fac0b6 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -128,7 +128,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # generate scene with Timer("[INFO]: Time taken for scene creation", "scene_creation"): # set the stage context for scene creation steps which use the stage - with use_stage(self.sim.get_initial_stage()): + with use_stage(self.sim.stage): self.scene = InteractiveScene(self.cfg.scene) self._setup_scene() attach_stage_to_usd_context() @@ -160,7 +160,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs 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()): + with use_stage(self.sim.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. diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 88336ddfa85..4b86181a009 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -131,7 +131,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # generate scene with Timer("[INFO]: Time taken for scene creation", "scene_creation"): # set the stage context for scene creation steps which use the stage - with use_stage(self.sim.get_initial_stage()): + with use_stage(self.sim.stage): self.scene = InteractiveScene(self.cfg.scene) attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) @@ -161,7 +161,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): 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()): + with use_stage(self.sim.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. diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index bc3db403c6d..a5d5891582e 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -562,14 +562,6 @@ 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 - def get_physics_dt(self) -> float: """Returns the physics time step of the simulation. diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index fdee7454c13..b1441b212a3 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -307,7 +307,7 @@ def test_get_initial_stage(): sim = SimulationContext() # get initial stage - stage = sim.get_initial_stage() + stage = sim.stage # verify stage is valid assert stage is not 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 5f1dd306a3d..84c6d68bfcc 100644 --- a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py @@ -61,7 +61,7 @@ def test_stage_in_memory_with_shapes(sim): num_clones = 10 # grab stage in memory and set as current stage via the with statement - stage_in_memory = sim.get_initial_stage() + stage_in_memory = sim.stage with sim_utils.use_stage(stage_in_memory): # create cloned cone stage for i in range(num_clones): @@ -122,7 +122,7 @@ def test_stage_in_memory_with_usds(sim): ] # grab stage in memory and set as current stage via the with statement - stage_in_memory = sim.get_initial_stage() + stage_in_memory = sim.stage with sim_utils.use_stage(stage_in_memory): # create cloned robot stage for i in range(num_clones): @@ -184,7 +184,7 @@ def test_stage_in_memory_with_clone_in_fabric(sim): num_clones = 100 # grab stage in memory and set as current stage via the with statement - stage_in_memory = sim.get_initial_stage() + stage_in_memory = sim.stage with sim_utils.use_stage(stage_in_memory): # set up paths base_env_path = "/World/envs" From 17e62fc3fb64fd970058df45635a5ef13534d3be Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 07:28:24 -0800 Subject: [PATCH 107/130] fix bugs in refactor --- source/isaaclab/isaaclab/envs/ui/base_env_window.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 52570b31b7a..db047afb08d 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -149,7 +149,7 @@ def _build_sim_frame(self): **record_animate_cfg ) # disable the button if fabric is not enabled - self.ui_window_elements["record_animation"].enabled = not self.env.sim..carb_settings.get_as_bool("/isaaclab/fabric_enabled") + self.ui_window_elements["record_animation"].enabled = not self.env.sim.carb_settings.get_as_bool("/isaaclab/fabric_enabled") def _build_viewer_frame(self): """Build the viewer-related control frame for the UI.""" From 53cb6fa948d9d9120c08a13624543bf0010cd5e8 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 08:57:18 -0800 Subject: [PATCH 108/130] refactor step 5: refactored physx_backend from simulation context --- source/isaaclab/isaaclab/envs/mdp/events.py | 2 +- source/isaaclab/isaaclab/sim/physx_backend.py | 580 ++++++++++++++++++ .../isaaclab/sim/simulation_context.py | 470 +------------- .../test/sim/test_simulation_context.py | 4 +- .../direct/automate/assembly_env.py | 2 +- .../direct/automate/disassembly_env.py | 2 +- .../direct/factory/factory_env.py | 2 +- 7 files changed, 598 insertions(+), 464 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/physx_backend.py diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index df7216d9d2e..cf024553b94 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -532,7 +532,7 @@ def randomize_physics_scene_gravity( gravity = gravity[0].tolist() # set the gravity into the physics simulation - physics_sim_view: physx.SimulationView = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view: physx.SimulationView = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view physics_sim_view.set_gravity(carb.Float3(*gravity)) diff --git a/source/isaaclab/isaaclab/sim/physx_backend.py b/source/isaaclab/isaaclab/sim/physx_backend.py new file mode 100644 index 00000000000..5e53ff6e0d9 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/physx_backend.py @@ -0,0 +1,580 @@ +# 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 + +"""PhysX physics backend for SimulationContext.""" + +from __future__ import annotations + +import glob +import logging +import os +import re +import time +import torch +from datetime import datetime +from typing import TYPE_CHECKING + +import carb +import omni.kit.app +import omni.physx +import omni.physics.tensors +from pxr import Usd, Gf, PhysxSchema, Sdf, UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.sim.simulation_manager import SimulationManager + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext + +logger = logging.getLogger(__name__) + + +class AnimationRecorder: + """Handles animation recording for the simulation. + + This class manages the recording of physics animations using the PhysX PVD + (Physics Visual Debugger) interface. It handles the setup, update, and + finalization of animation recordings. + """ + + def __init__(self, carb_settings: carb.settings.ISettings, app_iface: omni.kit.app.IApp): + """Initialize the animation recorder. + + Args: + carb_settings: The Carbonite settings interface. + app_iface: The Omniverse Kit application interface. + """ + self._carb_settings = carb_settings + self._app_iface = app_iface + self._enabled = False + self._start_time: float = 0.0 + self._stop_time: float = 0.0 + self._started_timestamp: float | None = None + self._output_dir: str = "" + self._timestamp: str = "" + self._physx_pvd_interface = None + + self._setup() + + @property + def enabled(self) -> bool: + """Whether animation recording is enabled.""" + return self._enabled + + def _setup(self) -> None: + """Sets up animation recording settings and initializes the recording.""" + self._enabled = bool(self._carb_settings.get("/isaaclab/anim_recording/enabled")) + if not self._enabled: + return + + # Import omni.physx.pvd.bindings here since it is not available by default + from omni.physxpvd.bindings import _physxPvd + + # Init anim recording settings + self._start_time = self._carb_settings.get("/isaaclab/anim_recording/start_time") + self._stop_time = self._carb_settings.get("/isaaclab/anim_recording/stop_time") + self._started_timestamp = None + + # Make output path relative to repo path + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + self._timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") + self._output_dir = ( + os.path.join(repo_path, "anim_recordings", self._timestamp).replace("\\", "/").rstrip("/") + "/" + ) + os.makedirs(self._output_dir, exist_ok=True) + + # Acquire physx pvd interface and set output directory + self._physx_pvd_interface = _physxPvd.acquire_physx_pvd_interface() + + # Set carb settings for the output path and enabling pvd recording + self._carb_settings.set_string("/persistent/physics/omniPvdOvdRecordingDirectory", self._output_dir) + self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) + + def update(self) -> bool: + """Tracks timestamps and triggers finish if total time has elapsed. + + Returns: + True if animation recording has finished, False otherwise. + """ + if not self._enabled: + return False + + if self._started_timestamp is None: + self._started_timestamp = time.time() + + total_time = time.time() - self._started_timestamp + if total_time > self._stop_time: + self._finish() + return True + return False + + def _finish(self) -> bool: + """Finishes the animation recording and outputs the baked animation recording. + + Returns: + True if the recording was successfully finished. + """ + logger.warning( + "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." + ) + + # Detaching the stage will also close it and force the serialization of the OVD file + physx = omni.physx.get_physx_simulation_interface() + physx.detach_stage() + + # Save stage to disk + stage_path = os.path.join(self._output_dir, "stage_simulation.usdc") + sim_utils.save_stage(stage_path, save_and_reload_in_place=False) + + # Find the latest ovd file not named tmp.ovd + ovd_files = [f for f in glob.glob(os.path.join(self._output_dir, "*.ovd")) if not f.endswith("tmp.ovd")] + input_ovd_path = max(ovd_files, key=os.path.getctime) + + # Invoke pvd interface to create recording + stage_filename = "baked_animation_recording.usda" + result = self._physx_pvd_interface.ovd_to_usd_over_with_layer_creation( + input_ovd_path, + stage_path, + self._output_dir, + stage_filename, + self._start_time, + self._stop_time, + True, # True: ASCII layers / False : USDC layers + False, # True: verify over layer + ) + + # Workaround for manually setting the truncated start time in the baked animation recording + self._update_usda_start_time(os.path.join(self._output_dir, stage_filename), self._start_time) + + # Disable recording + self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) + + return result + + @staticmethod + def _update_usda_start_time(file_path: str, start_time: float) -> None: + """Updates the start time of the USDA baked animation recording file. + + Args: + file_path: Path to the USDA file. + start_time: The new start time to set. + """ + # Read the USDA file + with open(file_path) as file: + content = file.read() + + # Extract the timeCodesPerSecond value + time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) + if not time_code_match: + raise ValueError("timeCodesPerSecond not found in the file.") + time_codes_per_second = int(time_code_match.group(1)) + + # Compute the new start time code + new_start_time_code = int(start_time * time_codes_per_second) + + # Replace the startTimeCode in the file + content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) + + # Write the updated content back to the file + with open(file_path, "w") as file: + file.write(content) + + +class PhysXBackend: + """PhysX physics backend. + + This class manages the PhysX physics simulation, including: + - Physics scene creation and configuration + - Device settings (CPU/GPU) + - Timestep and solver configuration + - Fabric interface for fast data access + - Physics stepping and reset + + Lifecycle: __init__() -> reset() -> step() (repeated) -> close() + """ + + def __init__(self, sim_context: "SimulationContext"): + """Initialize the PhysX backend. + + Args: + sim_context: Parent simulation context. + """ + self._sim = sim_context + + # acquire physics interfaces + self._physx_iface = omni.physx.get_physx_interface() + self._physx_sim_iface = omni.physx.get_physx_simulation_interface() + + # Initialize physics device (will be set in _apply_physics_settings) + self._physics_device: str = "cpu" + + # Fabric interface (will be set in _load_fabric_interface) + self._fabric_iface = None + self._update_fabric = None + + # Physics scene references (will be set in _init_physics_scene) + self._physics_scene = None + self._physx_scene_api = None + + # Initialize physics + self._init_physics_scene() + self._configure_simulation_dt() + self._apply_physics_settings() + SimulationManager.initialize() + + # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes + # when in headless mode + self._sim.set_setting("/app/player/playSimulations", False) + self._sim.app.update() + self._sim.set_setting("/app/player/playSimulations", True) + + # load flatcache/fabric interface + self._load_fabric_interface() + + # initialize animation recorder + self._anim_recorder = AnimationRecorder(self._sim.carb_settings, self._sim.app) + + @property + def device(self) -> str: + """Device used for physics simulation.""" + return self._physics_device + + @property + def physics_dt(self) -> float: + """Physics timestep.""" + return self._sim.cfg.dt + + @property + def physics_sim_view(self) -> "omni.physics.tensors.SimulationView": + """Physics simulation view with torch backend.""" + return SimulationManager.get_physics_sim_view() + + def reset(self, soft: bool = False) -> None: + """Reset the physics simulation. + + Args: + soft: If True, skip full reinitialization. + """ + if not soft: + # initialize the physics simulation + if SimulationManager.get_physics_sim_view() is None: + SimulationManager.initialize_physics() + + # app.update() may be changing the cuda device in reset, so we force it back to our desired device here + if "cuda" in self._physics_device: + torch.cuda.set_device(self._physics_device) + + # enable kinematic rendering with fabric + physics_sim_view = SimulationManager.get_physics_sim_view() + if physics_sim_view is not None: + physics_sim_view._backend.initialize_kinematic_bodies() + + def forward(self) -> None: + """Update articulation kinematics and fabric for rendering.""" + if self._fabric_iface is not None: + physics_sim_view = SimulationManager.get_physics_sim_view() + if physics_sim_view is not None and self._sim.is_playing(): + # Update the articulations' link's poses before rendering + physics_sim_view.update_articulations_kinematic() + self._update_fabric(0.0, 0.0) + + def step(self, render: bool = True) -> bool: + """Step the physics simulation. + + Args: + render: If True, step via app.update() which includes rendering. + If False, step physics only without rendering. + + Returns: + True if animation recording finished and app should shutdown, False otherwise. + """ + # update animation recorder if enabled + if self._anim_recorder.enabled and self._anim_recorder.update(): + logger.warning("Animation recording finished. Closing app.") + self._sim.app.shutdown() + return True + + if render: + self._sim.app.update() + else: + self._physx_sim_iface.simulate(self._sim.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._physics_device: + torch.cuda.set_device(self._physics_device) + + return False + + def close(self) -> None: + """Clean up physics resources.""" + # clear the simulation manager state (notifies assets to cleanup) + SimulationManager.clear() + # detach the stage from physx + if self._physx_sim_iface is not None: + self._physx_sim_iface.detach_stage() + + # ------------------------- + # Private initialization methods + # ------------------------- + + def _init_physics_scene(self) -> None: + """Initialize the USD physics scene.""" + stage = self._sim.stage + cfg = self._sim.cfg + + with sim_utils.use_stage(stage): + # correct conventions for metric units + UsdGeom.SetStageUpAxis(stage, "Z") + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + UsdPhysics.SetStageKilogramsPerUnit(stage, 1.0) + + # find if any physics prim already exists and delete it + for prim in stage.Traverse(): + if prim.HasAPI(PhysxSchema.PhysxSceneAPI) or prim.GetTypeName() == "PhysicsScene": + sim_utils.delete_prim(prim.GetPath().pathString, stage=stage) + + # create a new physics scene + if stage.GetPrimAtPath(cfg.physics_prim_path).IsValid(): + raise RuntimeError(f"A prim already exists at path '{cfg.physics_prim_path}'.") + + self._physics_scene = UsdPhysics.Scene.Define(stage, cfg.physics_prim_path) + self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene.GetPrim()) + + def _configure_simulation_dt(self): + """Configures the simulation step size based on the physics and rendering step sizes.""" + cfg = self._sim.cfg + stage = self._sim.stage + carb_settings = self._sim.carb_settings + timeline_iface = self._sim._timeline_iface + + # 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(cfg.render_interval, 1) + + # set simulation step per second + steps_per_second = int(1.0 / 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) + carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) + + # compute rendering frequency + rendering_hz = int(1.0 / (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 carb_settings.get("/app/runLoops/main/rateLimitEnabled"): + carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + timeline_iface.set_target_framerate(rendering_hz) + with Usd.EditContext(stage, stage.GetRootLayer()): + stage.SetTimeCodesPerSecond(rendering_hz) + 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(cfg.dt * render_interval) + _loop_runner.set_manual_mode(True) + except Exception: + self._sim.logger.warning( + "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" + ) + carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) + carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + timeline_iface.set_target_framerate(rendering_hz) + + def _apply_physics_settings(self): + """Sets various carb physics settings.""" + cfg = self._sim.cfg + carb_settings = self._sim.carb_settings + + # -------------------------- + # Carb Physics API settings + # -------------------------- + + # enable hydra scene-graph instancing + # note: this allows rendering of instanceable assets on the GUI + carb_settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) + # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking + # note: dispatcher handles how threads are launched for multi-threaded physics + carb_settings.set_bool("/physics/physxDispatcher", True) + # disable contact processing in omni.physx + # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. + # The physics flag gets enabled when a contact sensor is created. + if hasattr(cfg, "disable_contact_processing"): + self._sim.logger.warning( + "The `disable_contact_processing` attribute is deprecated and always set to True" + " to avoid unnecessary overhead. Contact processing is automatically enabled when" + " a contact sensor is created, so manual configuration is no longer required." + ) + # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts + # are always processed. The issue is reported to the PhysX team by @mmittal. + carb_settings.set_bool("/physics/disableContactProcessing", True) + # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them + # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags + # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry + carb_settings.set_bool("/physics/collisionConeCustomGeometry", False) + carb_settings.set_bool("/physics/collisionCylinderCustomGeometry", False) + # hide the Simulation Settings window + carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) + + # handle device settings + if "cuda" in cfg.device: + parsed_device = 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 = carb_settings.get_as_int("/physics/cudaDevice") + if device_id < 0: + carb_settings.set_int("/physics/cudaDevice", 0) + device_id = 0 + else: + # if users specified "cuda:N", we use the provided device id + device_id = int(parsed_device[1]) + carb_settings.set_int("/physics/cudaDevice", device_id) + # suppress readback for GPU physics + 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 + carb_settings.set_int("/physics/cudaDevice", -1) + carb_settings.set_bool("/physics/suppressReadback", False) + # save the device + self._physics_device = "cpu" + + # Configure simulation manager backend + # Isaac Lab always uses torch tensors for consistency, even on CPU + SimulationManager.set_backend("torch") + SimulationManager.set_physics_sim_device(self._physics_device) + + # -------------------------- + # USDPhysics API settings + # -------------------------- + + # set gravity + gravity = self._sim._gravity_tensor + gravity_magnitude = torch.norm(gravity).item() + # avoid division by zero + 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)) + 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"{cfg.physics_prim_path}/defaultMaterial" + cfg.physics_material.func(material_path, cfg.physics_material) + # bind the physics material to the scene + sim_utils.bind_physics_material(cfg.physics_prim_path, material_path) + + # -------------------------- + # PhysX API settings + # -------------------------- + + # set broadphase type + broadphase_type = "GPU" if "cuda" in cfg.device else "MBP" + self._physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) + # set gpu dynamics + enable_gpu_dynamics = "cuda" in 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 cfg.physx.enable_ccd: + cfg.physx.enable_ccd = False + self._sim.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(cfg.physx.enable_ccd) + + # set solver type + solver_type = "PGS" if 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(cfg.physx.solve_articulation_contact_last) + + # iterate over all the settings and set them + for key, value in cfg.physx.to_dict().items(): # type: ignore + 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 + if cfg.physx.solver_type == 1 and not 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 cfg.physx.enable_stabilization and cfg.dt > 0.0333: + self._sim.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 _load_fabric_interface(self): + """Loads the fabric interface if enabled.""" + import omni.kit.app + + cfg = self._sim.cfg + carb_settings = self._sim.carb_settings + + extension_manager = omni.kit.app.get_app().get_extension_manager() + fabric_enabled = extension_manager.is_extension_enabled("omni.physx.fabric") + + if 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 + self._fabric_iface = get_physx_fabric_interface() + if hasattr(self._fabric_iface, "force_update"): + # The update method in the fabric interface only performs an update if a physics step has occurred. + # However, for rendering, we need to force an update since any element of the scene might have been + # modified in a reset (which occurs after the physics step) and we want the renderer to be aware of + # these changes. + self._update_fabric = self._fabric_iface.force_update + 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 fabric interface to None + self._fabric_iface = None + + # set carb settings for fabric + carb_settings.set_bool("/physics/fabricEnabled", cfg.use_fabric) + carb_settings.set_bool("/physics/updateToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateParticlesToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateVelocitiesToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateForceSensorsToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateResidualsToUsd", not cfg.use_fabric) + # disable simulation output window visibility + carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) + + def is_fabric_enabled(self) -> bool: + """Returns whether the fabric interface is enabled.""" + return self._fabric_iface is not None diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index a5d5891582e..9dece47ec90 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -8,192 +8,34 @@ import builtins import enum import flatdict -import glob import logging import os -import re -import time import toml import torch import traceback import weakref from collections.abc import Iterator from contextlib import contextmanager -from datetime import datetime from typing import Any, ClassVar import carb import omni.kit.app -import omni.physics.tensors -import omni.physx import omni.timeline import omni.usd -from isaaclab.sim.simulation_manager import SimulationManager from isaacsim.core.utils.viewports import set_camera_view -from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils +from pxr import Usd, UsdUtils import isaaclab.sim as sim_utils from isaaclab.utils.logger import configure_logging from isaaclab.utils.version import get_isaac_sim_version +from .physx_backend import PhysXBackend from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg -# import logger logger = logging.getLogger(__name__) -class AnimationRecorder: - """Handles animation recording for the simulation. - - This class manages the recording of physics animations using the PhysX PVD - (Physics Visual Debugger) interface. It handles the setup, update, and - finalization of animation recordings. - """ - - def __init__(self, carb_settings: carb.settings.ISettings, app_iface: omni.kit.app.IApp): - """Initialize the animation recorder. - - Args: - carb_settings: The Carbonite settings interface. - app_iface: The Omniverse Kit application interface. - """ - self._carb_settings = carb_settings - self._app_iface = app_iface - self._enabled = False - self._start_time: float = 0.0 - self._stop_time: float = 0.0 - self._started_timestamp: float | None = None - self._output_dir: str = "" - self._timestamp: str = "" - self._physx_pvd_interface = None - - self._setup() - - @property - def enabled(self) -> bool: - """Whether animation recording is enabled.""" - return self._enabled - - def _setup(self) -> None: - """Sets up animation recording settings and initializes the recording.""" - self._enabled = bool(self._carb_settings.get("/isaaclab/anim_recording/enabled")) - if not self._enabled: - return - - # Import omni.physx.pvd.bindings here since it is not available by default - from omni.physxpvd.bindings import _physxPvd - - # Init anim recording settings - self._start_time = self._carb_settings.get("/isaaclab/anim_recording/start_time") - self._stop_time = self._carb_settings.get("/isaaclab/anim_recording/stop_time") - self._started_timestamp = None - - # Make output path relative to repo path - repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") - self._timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") - self._output_dir = ( - os.path.join(repo_path, "anim_recordings", self._timestamp).replace("\\", "/").rstrip("/") + "/" - ) - os.makedirs(self._output_dir, exist_ok=True) - - # Acquire physx pvd interface and set output directory - self._physx_pvd_interface = _physxPvd.acquire_physx_pvd_interface() - - # Set carb settings for the output path and enabling pvd recording - self._carb_settings.set_string("/persistent/physics/omniPvdOvdRecordingDirectory", self._output_dir) - self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) - - def update(self) -> bool: - """Tracks timestamps and triggers finish if total time has elapsed. - - Returns: - True if animation recording has finished, False otherwise. - """ - if not self._enabled: - return False - - if self._started_timestamp is None: - self._started_timestamp = time.time() - - total_time = time.time() - self._started_timestamp - if total_time > self._stop_time: - self._finish() - return True - return False - - def _finish(self) -> bool: - """Finishes the animation recording and outputs the baked animation recording. - - Returns: - True if the recording was successfully finished. - """ - logger.warning( - "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." - ) - - # Detaching the stage will also close it and force the serialization of the OVD file - physx = omni.physx.get_physx_simulation_interface() - physx.detach_stage() - - # Save stage to disk - stage_path = os.path.join(self._output_dir, "stage_simulation.usdc") - sim_utils.save_stage(stage_path, save_and_reload_in_place=False) - - # Find the latest ovd file not named tmp.ovd - ovd_files = [f for f in glob.glob(os.path.join(self._output_dir, "*.ovd")) if not f.endswith("tmp.ovd")] - input_ovd_path = max(ovd_files, key=os.path.getctime) - - # Invoke pvd interface to create recording - stage_filename = "baked_animation_recording.usda" - result = self._physx_pvd_interface.ovd_to_usd_over_with_layer_creation( - input_ovd_path, - stage_path, - self._output_dir, - stage_filename, - self._start_time, - self._stop_time, - True, # True: ASCII layers / False : USDC layers - False, # True: verify over layer - ) - - # Workaround for manually setting the truncated start time in the baked animation recording - self._update_usda_start_time(os.path.join(self._output_dir, stage_filename), self._start_time) - - # Disable recording - self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) - - return result - - @staticmethod - def _update_usda_start_time(file_path: str, start_time: float) -> None: - """Updates the start time of the USDA baked animation recording file. - - Args: - file_path: Path to the USDA file. - start_time: The new start time to set. - """ - # Read the USDA file - with open(file_path) as file: - content = file.read() - - # Extract the timeCodesPerSecond value - time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) - if not time_code_match: - raise ValueError("timeCodesPerSecond not found in the file.") - time_codes_per_second = int(time_code_match.group(1)) - - # Compute the new start time code - new_start_time_code = int(start_time * time_codes_per_second) - - # Replace the startTimeCode in the file - content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) - - # Write the updated content back to the file - with open(file_path, "w") as file: - file.write(content) - - class SimulationContext: """A class to control simulation-related events such as physics stepping and rendering. @@ -411,48 +253,13 @@ def __init__(self, cfg: SimulationCfg | None = None): # 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() - # initialize animation recorder - self._anim_recorder = AnimationRecorder(self.carb_settings, self._app_iface) - # 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 - 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) 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}'.") - self._physics_scene = UsdPhysics.Scene.Define(self._initial_stage, self.cfg.physics_prim_path) - self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene.GetPrim()) - - # set time codes per second - self._configure_simulation_dt() - # apply physics settings (carb and physx) - self._apply_physics_settings() - # initialize the simulation manager callbacks - SimulationManager.initialize() - - # 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_iface.update() - self.set_setting("/app/player/playSimulations", True) - # load flatcache/fabric interface - self._load_fabric_interface() + # initialize physics backend (handles scene creation, settings, fabric) + self._physics_backend = PhysXBackend(self) def __new__(cls, *args, **kwargs) -> SimulationContext: """Returns the instance of the simulation context. @@ -492,11 +299,8 @@ 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 - # clear the simulation manager state (notifies assets to cleanup) - SimulationManager.clear() - # detach the stage from physx - if cls._instance._physx_sim_iface is not None: - cls._instance._physx_sim_iface.detach_stage() + # close physics backend (clears SimulationManager, detaches physx stage) + cls._instance._physics_backend.close() # detach the stage from the USD stage cache stage_cache = UsdUtils.StageCache.Get() stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() @@ -528,12 +332,7 @@ def device(self) -> str: In Omniverse, it is possible to configure multiple GPUs for rendering, while physics engine operates on a single GPU. This function returns the device that is used for physics simulation. """ - return self._physics_device - - @property - def physics_sim_view(self) -> omni.physics.tensors.SimulationView: - """Physics simulation view with torch backend.""" - return SimulationManager.get_physics_sim_view() + return self._physics_backend.device """ Operations - Simulation Information. @@ -777,19 +576,12 @@ def reset(self, soft: bool = False): self._disable_app_control_on_stop_handle = False # play the simulation self.play() - # initialize the physics simulation - if SimulationManager.get_physics_sim_view() is None: - SimulationManager.initialize_physics() # 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) + # reset physics backend (initializes physics, resets cuda device, kinematic bodies) + self._physics_backend.reset(soft) - # enable kinematic rendering with fabric - 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: @@ -798,11 +590,7 @@ def reset(self, soft: bool = False): def forward(self) -> None: """Updates articulation kinematics and fabric for rendering.""" - if self._fabric_iface is not None: - if self.physics_sim_view is not None and self.is_playing(): - # Update the articulations' link's poses before rendering - self.physics_sim_view.update_articulations_kinematic() - self._update_fabric(0.0, 0.0) + self._physics_backend.forward() def step(self, render: bool = True): """Steps the simulation. @@ -814,11 +602,6 @@ 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. """ - # update animation recorder if enabled - if self._anim_recorder.enabled and self._anim_recorder.update(): - 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 if not self.is_playing(): # step the simulator (but not the physics) to have UI still active @@ -833,16 +616,8 @@ def step(self, render: bool = True): # FIXME: This steps physics as well, which we is not good in general. self._app_iface.update() - # step the simulation - if render: - self._app_iface.update() - else: - 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: - torch.cuda.set_device(self.device) + # step the physics simulation + self._physics_backend.step(render) def render(self, mode: RenderMode | None = None): """Refreshes the rendering components including UI elements and view-ports depending on the render mode. @@ -912,187 +687,6 @@ def _predicate(prim: Usd.Prim) -> bool: 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) - # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking - # note: dispatcher handles how threads are launched for multi-threaded physics - self.carb_settings.set_bool("/physics/physxDispatcher", True) - # disable contact processing in omni.physx - # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. - # The physics flag gets enabled when a contact sensor is created. - if hasattr(self.cfg, "disable_contact_processing"): - self.logger.warning( - "The `disable_contact_processing` attribute is deprecated and always set to True" - " to avoid unnecessary overhead. Contact processing is automatically enabled when" - " a contact sensor is created, so manual configuration is no longer required." - ) - # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts - # are always processed. The issue is reported to the PhysX team by @mmittal. - self.carb_settings.set_bool("/physics/disableContactProcessing", True) - # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them - # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags - # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry - self.carb_settings.set_bool("/physics/collisionConeCustomGeometry", False) - self.carb_settings.set_bool("/physics/collisionCylinderCustomGeometry", False) - # 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 - 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 - 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" - - # Configure simulation manager backend - # Isaac Lab always uses torch tensors for consistency, even on CPU - SimulationManager.set_backend("torch") - SimulationManager.set_physics_sim_device(self._physics_device) - - # -------------------------- - # USDPhysics API settings - # -------------------------- - - # set gravity - gravity = self._gravity_tensor - gravity_magnitude = torch.norm(gravity).item() - # avoid division by zero - 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)) - 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) - - # 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", "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 - 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.""" @@ -1187,46 +781,6 @@ 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 _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 - self._fabric_iface = get_physx_fabric_interface() - self.carb_settings.set_bool("/isaaclab/fabric_enabled", True) - if hasattr(self._fabric_iface, "force_update"): - # The update method in the fabric interface only performs an update if a physics step has occurred. - # However, for rendering, we need to force an update since any element of the scene might have been - # modified in a reset (which occurs after the physics step) and we want the renderer to be aware of - # these changes. - self._update_fabric = self._fabric_iface.force_update - 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 fabric interface to None - 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) - 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) - 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 diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index b1441b212a3..c66c2b2e066 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -212,7 +212,7 @@ def test_reset(): assert sim.is_playing() # check that physics sim view is created - assert sim.physics_sim_view is not None + assert sim._physics_backend.physics_sim_view is not None @pytest.mark.isaacsim_ci @@ -368,7 +368,7 @@ def test_fabric_setting(use_fabric): sim = SimulationContext(cfg) # check fabric is enabled - assert sim..carb_settings.get_as_bool("/isaaclab/fabric_enabled") == use_fabric + assert sim.carb_settings.get_as_bool("/isaaclab/fabric_enabled") == use_fabric @pytest.mark.isaacsim_ci diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py index 22130288757..cf6234b6354 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py @@ -822,7 +822,7 @@ def randomize_held_initial_state(self, env_ids, pre_grasp): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) self.randomize_fixed_initial_state(env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py index 950bbfc2fda..19bd89e26da 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py @@ -689,7 +689,7 @@ def close_gripper(self, env_ids): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) self.randomize_fixed_initial_state(env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index a4e9c6d9ece..960232218cb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -614,7 +614,7 @@ def step_sim_no_action(self): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) # (1.) Randomize fixed asset pose. From c89b550452c8a1d0236313607d5ab692de3b48e6 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 09:28:19 -0800 Subject: [PATCH 109/130] refactor step 6: refactored visualizer_backend --- source/isaaclab/isaaclab/sim/physx_backend.py | 33 +- .../isaaclab/sim/simulation_context.py | 353 +----------- .../isaaclab/sim/visualizer_interface.py | 510 ++++++++++++++++++ 3 files changed, 536 insertions(+), 360 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/visualizer_interface.py diff --git a/source/isaaclab/isaaclab/sim/physx_backend.py b/source/isaaclab/isaaclab/sim/physx_backend.py index 5e53ff6e0d9..44351972080 100644 --- a/source/isaaclab/isaaclab/sim/physx_backend.py +++ b/source/isaaclab/isaaclab/sim/physx_backend.py @@ -20,7 +20,7 @@ import omni.kit.app import omni.physx import omni.physics.tensors -from pxr import Usd, Gf, PhysxSchema, Sdf, UsdGeom, UsdPhysics +from pxr import Gf, PhysxSchema, Sdf, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.sim.simulation_manager import SimulationManager @@ -344,11 +344,9 @@ def _init_physics_scene(self) -> None: self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene.GetPrim()) def _configure_simulation_dt(self): - """Configures the simulation step size based on the physics and rendering step sizes.""" + """Configures the physics simulation step size.""" cfg = self._sim.cfg - stage = self._sim.stage carb_settings = self._sim.carb_settings - timeline_iface = self._sim._timeline_iface # if rendering is called the substeps term is used to determine # how many physics steps to perform per rendering step. @@ -362,33 +360,6 @@ def _configure_simulation_dt(self): min_steps = int(steps_per_second / render_interval) carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) - # compute rendering frequency - rendering_hz = int(1.0 / (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 carb_settings.get("/app/runLoops/main/rateLimitEnabled"): - carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) - timeline_iface.set_target_framerate(rendering_hz) - with Usd.EditContext(stage, stage.GetRootLayer()): - stage.SetTimeCodesPerSecond(rendering_hz) - 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(cfg.dt * render_interval) - _loop_runner.set_manual_mode(True) - except Exception: - self._sim.logger.warning( - "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" - ) - carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) - carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) - timeline_iface.set_target_framerate(rendering_hz) - def _apply_physics_settings(self): """Sets various carb physics settings.""" cfg = self._sim.cfg diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 9dece47ec90..f318786d486 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -6,23 +6,16 @@ from __future__ import annotations import builtins -import enum -import flatdict import logging -import os -import toml import torch import traceback -import weakref from collections.abc import Iterator from contextlib import contextmanager from typing import Any, ClassVar import carb import omni.kit.app -import omni.timeline import omni.usd -from isaacsim.core.utils.viewports import set_camera_view from pxr import Usd, UsdUtils import isaaclab.sim as sim_utils @@ -32,6 +25,7 @@ from .physx_backend import PhysXBackend from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg +from .visualizer_interface import RenderMode, VisualizerInterface logger = logging.getLogger(__name__) @@ -83,40 +77,8 @@ class SimulationContext: _is_initialized: ClassVar[bool] = False """Whether the simulation context is initialized.""" - class RenderMode(enum.IntEnum): - """Different rendering modes for the simulation. - - Render modes correspond to how the viewport and other UI elements (such as listeners to keyboard or mouse - events) are updated. There are three main components that can be updated when the simulation is rendered: - - 1. **UI elements and other extensions**: These are UI elements (such as buttons, sliders, etc.) and other - extensions that are running in the background that need to be updated when the simulation is running. - 2. **Cameras**: These are typically based on Hydra textures and are used to render the scene from different - viewpoints. They can be attached to a viewport or be used independently to render the scene. - 3. **Viewports**: These are windows where you can see the rendered scene. - - Updating each of the above components has a different overhead. For example, updating the viewports is - computationally expensive compared to updating the UI elements. Therefore, it is useful to be able to - control what is updated when the simulation is rendered. This is where the render mode comes in. There are - four different render modes: - - * :attr:`NO_GUI_OR_RENDERING`: The simulation is running without a GUI and off-screen rendering flag is disabled, - so none of the above are updated. - * :attr:`NO_RENDERING`: No rendering, where only 1 is updated at a lower rate. - * :attr:`PARTIAL_RENDERING`: Partial rendering, where only 1 and 2 are updated. - * :attr:`FULL_RENDERING`: Full rendering, where everything (1, 2, 3) is updated. - - .. _Viewports: https://docs.omniverse.nvidia.com/extensions/latest/ext_viewport.html - """ - - NO_GUI_OR_RENDERING = -1 - """The simulation is running without a GUI and off-screen rendering is disabled.""" - NO_RENDERING = 0 - """No rendering, where only other UI elements are updated at a lower rate.""" - PARTIAL_RENDERING = 1 - """Partial rendering, where the simulation cameras and UI elements are updated.""" - FULL_RENDERING = 2 - """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" + # Expose RenderMode as class attribute for backwards compatibility + RenderMode = RenderMode def __init__(self, cfg: SimulationCfg | None = None): """Creates a simulation context to control the simulator. @@ -165,72 +127,8 @@ def __init__(self, cfg: SimulationCfg | None = None): # acquire settings interface self.carb_settings = carb.settings.get_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") - # read flag for whether livestreaming GUI is enabled - self._livestream_gui = self.carb_settings.get("/app/livestream/enabled") - # read flag for whether XR GUI is enabled - self._xr_gui = self.carb_settings.get("/app/xr/enabled") - - # read flag for whether the Isaac Lab viewport capture pipeline will be used, - # casting None to False if the flag doesn't exist - # this flag is set from the AppLauncher class - self._offscreen_render = bool(self.carb_settings.get("/isaaclab/render/offscreen")) - # read flag for whether the default viewport should be enabled - self._render_viewport = bool(self.carb_settings.get("/isaaclab/render/active_viewport")) - # flag for whether any GUI will be rendered (local, livestreamed or viewport) - has_gui = self._local_gui or self._livestream_gui or self._xr_gui - self.carb_settings.set_bool("/isaaclab/has_gui", has_gui) - # apply render settings from render config - self._apply_render_settings_from_cfg() - - # store the default render mode - if not self.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: - # set default render mode - # note: this is the terminal state: cannot exit from this render mode - self.render_mode = self.RenderMode.NO_GUI_OR_RENDERING - # set viewport context to None - self._viewport_context = None - self._viewport_window = None - elif not self.carb_settings.get("/isaaclab/has_gui") and self._offscreen_render: - # set default render mode - # note: this is the terminal state: cannot exit from this render mode - self.render_mode = self.RenderMode.PARTIAL_RENDERING - # set viewport context to None - self._viewport_context = None - self._viewport_window = None - else: - # note: need to import here in case the UI is not available (ex. headless mode) - import omni.ui as ui - from omni.kit.viewport.utility import get_active_viewport - - # set default render mode - # note: this can be changed by calling the `set_render_mode` function - self.render_mode = self.RenderMode.FULL_RENDERING - # acquire viewport context - self._viewport_context = get_active_viewport() - self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] - # acquire viewport window - # TODO @mayank: Why not just use get_active_viewport_and_window() directly? - self._viewport_window = ui.Workspace.get_window("Viewport") - # counter for periodic rendering - self._render_throttle_counter = 0 - # rendering frequency in terms of number of render calls - self._render_throttle_period = 5 - - # check the case where we don't need to render the viewport - # since render_viewport can only be False in headless mode, we only need to check for offscreen_render - if not self._render_viewport and self._offscreen_render: - # disable the viewport if offscreen_render is enabled - from omni.kit.viewport.utility import get_active_viewport - - get_active_viewport().updates_enabled = False - - # override enable scene querying if rendering is enabled - # this is needed for some GUI features - if self.carb_settings.get("/isaaclab/has_gui"): - self.cfg.enable_scene_query_support = True + # initialize visualizer interface (handles viewport, render mode, render settings) + self._visualizer = VisualizerInterface(self) # 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. @@ -241,22 +139,8 @@ def __init__(self, cfg: SimulationCfg | None = None): # define a global variable to store the exceptions raised in the callback stack builtins.ISAACLAB_CALLBACK_EXCEPTION = 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 - 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 self._app_iface = omni.kit.app.get_app_interface() - self._timeline_iface = omni.timeline.get_timeline_interface() - - # set timeline auto update to True - self._timeline_iface.set_auto_update(True) # initialize physics backend (handles scene creation, settings, fabric) self._physics_backend = PhysXBackend(self) @@ -295,10 +179,8 @@ def clear_instance(cls) -> None: 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 + # close visualizer (unsubscribes stop handle) + cls._instance._visualizer.close() # close physics backend (clears SimulationManager, detaches physx stage) cls._instance._physics_backend.close() # detach the stage from the USD stage cache @@ -334,6 +216,11 @@ def device(self) -> str: """ return self._physics_backend.device + @property + def render_mode(self) -> RenderMode: + """Current render mode.""" + return self._visualizer.render_mode + """ Operations - Simulation Information. """ @@ -404,9 +291,7 @@ def set_camera_view( camera_prim_path: The path to the camera primitive in the stage. Defaults to "/OmniverseKit_Persp". """ - # safe call only if we have a GUI or viewport rendering enabled - if self.carb_settings.get("/isaaclab/has_gui") or self._offscreen_render or self._render_viewport: - set_camera_view(eye, target, camera_prim_path) + self._visualizer.set_camera_view(eye, target, camera_prim_path) def set_render_mode(self, mode: RenderMode): """Change the current render mode of the simulation. @@ -425,34 +310,7 @@ def set_render_mode(self, mode: RenderMode): Raises: ValueError: If the input mode is not supported. """ - # check if mode change is possible -- not possible when no GUI is available - if not self.carb_settings.get("/isaaclab/has_gui"): - self.logger.warning( - f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." - ) - return - # check if there is a mode change - # note: this is mostly needed for GUI when we want to switch between full rendering and no rendering. - if mode != self.render_mode: - if mode == self.RenderMode.FULL_RENDERING: - # display the viewport and enable updates - self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] - self._viewport_window.visible = True # pyright: ignore [reportOptionalMemberAccess] - elif mode == self.RenderMode.PARTIAL_RENDERING: - # hide the viewport and disable updates - self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] - self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] - elif mode == self.RenderMode.NO_RENDERING: - # hide the viewport and disable updates - if self._viewport_context is not None: - self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] - self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] - # reset the throttle counter - self._render_throttle_counter = 0 - else: - raise ValueError(f"Unsupported render mode: {mode}! Please check `RenderMode` for details.") - # update render mode - self.render_mode = mode + self._visualizer.set_render_mode(mode) def set_setting(self, name: str, value: Any): """Set simulation settings using the Carbonite SDK. @@ -504,7 +362,7 @@ def is_playing(self) -> bool: Returns: True if the simulator is playing. """ - return self._timeline_iface.is_playing() + return self._visualizer.is_playing() def is_stopped(self) -> bool: """Check whether the simulation is stopped. @@ -512,26 +370,17 @@ def is_stopped(self) -> bool: Returns: True if the simulator is stopped. """ - return self._timeline_iface.is_stopped() + return self._visualizer.is_stopped() def play(self) -> None: """Start playing the simulation.""" - # play the simulation - self._timeline_iface.play() - 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) + self._visualizer.play() # 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 + self._visualizer.pause() # check for callback exceptions self._check_for_callback_exceptions() @@ -541,13 +390,7 @@ def stop(self) -> None: Note: Stopping the simulation will lead to the simulation state being lost. """ - # stop the simulation - self._timeline_iface.stop() - 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) + self._visualizer.stop() # check for callback exceptions self._check_for_callback_exceptions() @@ -570,10 +413,10 @@ 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 + self._visualizer.set_stop_handle_enabled(False) if not self.is_stopped(): self.stop() - self._disable_app_control_on_stop_handle = False + self._visualizer.set_stop_handle_enabled(True) # play the simulation self.play() # check for callback exceptions @@ -631,35 +474,7 @@ 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 change the render mode - if mode is not None: - self.set_render_mode(mode) - # render based on the render mode - if self.render_mode == self.RenderMode.NO_GUI_OR_RENDERING: - # we never want to render anything here (this is for complete headless mode) - pass - elif self.render_mode == self.RenderMode.NO_RENDERING: - # throttle the rendering frequency to keep the UI responsive - self._render_throttle_counter += 1 - 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 - self.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self.set_setting("/app/player/playSimulations", True) - else: - # manually flush the fabric data to update Hydra textures - self.forward() - # render the simulation - # 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_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 - if "cuda" in self.device: - torch.cuda.set_device(self.device) + self._visualizer.render(mode) @classmethod def clear(cls): @@ -687,138 +502,18 @@ def _predicate(prim: Usd.Prim) -> bool: Helper Functions """ - def _apply_render_settings_from_cfg(self): # noqa: C901 - """Sets rtx settings specified in the RenderCfg.""" - - # define mapping of user-friendly RenderCfg names to native carb names - rendering_setting_name_mapping = { - "enable_translucency": "/rtx/translucency/enabled", - "enable_reflections": "/rtx/reflections/enabled", - "enable_global_illumination": "/rtx/indirectDiffuse/enabled", - "enable_dlssg": "/rtx-transient/dlssg/enabled", - "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", - "dlss_mode": "/rtx/post/dlss/execMode", - "enable_direct_lighting": "/rtx/directLighting/enabled", - "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", - "enable_shadows": "/rtx/shadows/enabled", - "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", - "dome_light_upper_lower_strategy": "/rtx/domeLight/upperLowerStrategy", - } - - not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] - - # grab the rendering mode using the following priority: - # 1. command line argument --rendering_mode, if provided - # 2. rendering_mode from Render Config, if set - # 3. lastly, default to "balanced" mode, if neither is specified - rendering_mode = self.carb_settings.get("/isaaclab/rendering/rendering_mode") - if not rendering_mode: - rendering_mode = self.cfg.render.rendering_mode - if not rendering_mode: - rendering_mode = "balanced" - - # set preset settings (same behavior as the CLI arg --rendering_mode) - if rendering_mode is not None: - # check if preset is supported - supported_rendering_modes = ["performance", "balanced", "quality"] - if rendering_mode not in supported_rendering_modes: - raise ValueError( - f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." - ) - - # grab isaac lab apps path - isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") - # for Isaac Sim 4.5 compatibility, we use the 4.5 rendering mode app files in a different folder - if get_isaac_sim_version().major < 5: - isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") - - # grab preset settings - preset_filename = os.path.join(isaaclab_app_exp_path, f"rendering_modes/{rendering_mode}.kit") - with open(preset_filename) as file: - preset_dict = toml.load(file) - preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) - - # set presets - for key, value in preset_dict.items(): - key = "/" + key.replace(".", "/") # convert to carb setting format - self.set_setting(key, value) - - # set user-friendly named settings - for key, value in vars(self.cfg.render).items(): - if value is None or key in not_carb_settings: - # skip unset settings and non-carb settings - continue - if key not in rendering_setting_name_mapping: - raise ValueError( - f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" - " need to be updated." - ) - key = rendering_setting_name_mapping[key] - self.set_setting(key, value) - - # set general carb settings - carb_settings = self.cfg.render.carb_settings - if carb_settings is not None: - for key, value in carb_settings.items(): - if "_" in key: - key = "/" + key.replace("_", "/") # convert from python variable style string - elif "." in key: - key = "/" + key.replace(".", "/") # convert from .kit file style string - if self.get_setting(key) is None: - raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") - self.set_setting(key, value) - - # set denoiser mode - if self.cfg.render.antialiasing_mode is not None: - try: - import omni.replicator.core as rep - - rep.settings.set_render_rtx_realtime(antialiasing=self.cfg.render.antialiasing_mode) - except Exception: - pass - - # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. - if self.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": - self.carb_settings.set_string("/rtx/rendermode", "RaytracedLighting") - 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 + self._visualizer.set_stop_handle_enabled(False) # check if we need to raise an exception that was raised in a callback if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # 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 - - """ - Callbacks. - """ - - 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 - resume the simulation from the last state. This leaves the app in an inconsistent state, where - two possible actions can be taken: - - 1. **Keep the app rendering**: In this case, the simulation is kept running and the app is not shutdown. - However, the physics is not updated and the script cannot be resumed from the last state. The - user has to manually close the app to stop the simulation. - 2. **Shutdown the app**: This is the default behavior. In this case, the app is shutdown and - the simulation is stopped. - - Note: - 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. - """ - pass - # if not self._disable_app_control_on_stop_handle: - # while not omni.timeline.get_timeline_interface().is_playing(): - # self.render() + self._visualizer.set_stop_handle_enabled(True) @contextmanager diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py new file mode 100644 index 00000000000..2b63acb2f78 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -0,0 +1,510 @@ +# 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 + +"""Visualizer interface for SimulationContext (Omniverse/PhysX workflow).""" + +from __future__ import annotations + +import enum +import flatdict +import logging +import os +import toml +import torch +import weakref +from typing import TYPE_CHECKING + +import omni.timeline +from isaacsim.core.utils.viewports import set_camera_view + +from isaaclab.utils.version import get_isaac_sim_version + +if TYPE_CHECKING: + from .simulation_context import SimulationContext + +logger = logging.getLogger(__name__) + + +class RenderMode(enum.IntEnum): + """Different rendering modes for the simulation. + + Render modes correspond to how the viewport and other UI elements (such as listeners to keyboard or mouse + events) are updated. There are three main components that can be updated when the simulation is rendered: + + 1. **UI elements and other extensions**: These are UI elements (such as buttons, sliders, etc.) and other + extensions that are running in the background that need to be updated when the simulation is running. + 2. **Cameras**: These are typically based on Hydra textures and are used to render the scene from different + viewpoints. They can be attached to a viewport or be used independently to render the scene. + 3. **Viewports**: These are windows where you can see the rendered scene. + + Updating each of the above components has a different overhead. For example, updating the viewports is + computationally expensive compared to updating the UI elements. Therefore, it is useful to be able to + control what is updated when the simulation is rendered. This is where the render mode comes in. There are + four different render modes: + + * :attr:`NO_GUI_OR_RENDERING`: The simulation is running without a GUI and off-screen rendering flag is disabled, + so none of the above are updated. + * :attr:`NO_RENDERING`: No rendering, where only 1 is updated at a lower rate. + * :attr:`PARTIAL_RENDERING`: Partial rendering, where only 1 and 2 are updated. + * :attr:`FULL_RENDERING`: Full rendering, where everything (1, 2, 3) is updated. + + .. _Viewports: https://docs.omniverse.nvidia.com/extensions/latest/ext_viewport.html + """ + + NO_GUI_OR_RENDERING = -1 + """The simulation is running without a GUI and off-screen rendering is disabled.""" + NO_RENDERING = 0 + """No rendering, where only other UI elements are updated at a lower rate.""" + PARTIAL_RENDERING = 1 + """Partial rendering, where the simulation cameras and UI elements are updated.""" + FULL_RENDERING = 2 + """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" + + +class VisualizerInterface: + """Manages viewport/rendering for SimulationContext (Omniverse workflow). + + This class handles: + - Viewport context and window management + - Render mode switching + - Camera view setup + - Render settings from configuration + - Throttled rendering for UI responsiveness + """ + + # Expose RenderMode as class attribute for backwards compatibility + RenderMode = RenderMode + + def __init__(self, sim_context: "SimulationContext"): + """Initialize visualizer interface. + + Args: + sim_context: Parent simulation context. + """ + self._sim = sim_context + + # Detect render flags + self._local_gui = self._sim.carb_settings.get("/app/window/enabled") + self._livestream_gui = self._sim.carb_settings.get("/app/livestream/enabled") + self._xr_gui = self._sim.carb_settings.get("/app/xr/enabled") + self._offscreen_render = bool(self._sim.carb_settings.get("/isaaclab/render/offscreen")) + self._render_viewport = bool(self._sim.carb_settings.get("/isaaclab/render/active_viewport")) + + # Flag for whether any GUI will be rendered (local, livestreamed or viewport) + has_gui = self._local_gui or self._livestream_gui or self._xr_gui + self._sim.carb_settings.set_bool("/isaaclab/has_gui", has_gui) + + # Apply render settings from config + self._apply_render_settings_from_cfg() + + # Initialize viewport state + self._viewport_context = None + self._viewport_window = None + self._render_throttle_counter = 0 + self._render_throttle_period = 5 + + # Store the default render mode + if not self._sim.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: + # set default render mode + # note: this is the terminal state: cannot exit from this render mode + self.render_mode = RenderMode.NO_GUI_OR_RENDERING + elif not self._sim.carb_settings.get("/isaaclab/has_gui") and self._offscreen_render: + # set default render mode + # note: this is the terminal state: cannot exit from this render mode + self.render_mode = RenderMode.PARTIAL_RENDERING + else: + # note: need to import here in case the UI is not available (ex. headless mode) + import omni.ui as ui + from omni.kit.viewport.utility import get_active_viewport + + # set default render mode + # note: this can be changed by calling the `set_render_mode` function + self.render_mode = RenderMode.FULL_RENDERING + # acquire viewport context + self._viewport_context = get_active_viewport() + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + # acquire viewport window + self._viewport_window = ui.Workspace.get_window("Viewport") + + # Check the case where we don't need to render the viewport + # since render_viewport can only be False in headless mode, we only need to check for offscreen_render + if not self._render_viewport and self._offscreen_render: + # disable the viewport if offscreen_render is enabled + from omni.kit.viewport.utility import get_active_viewport + + get_active_viewport().updates_enabled = False + + # Override enable scene querying if rendering is enabled + # this is needed for some GUI features + if self._sim.carb_settings.get("/isaaclab/has_gui"): + self._sim.cfg.enable_scene_query_support = True + + # acquire timeline interface + self._timeline_iface = omni.timeline.get_timeline_interface() + + # set timeline auto update to True + self._timeline_iface.set_auto_update(True) + + # configure rendering/timeline rate + self._configure_rendering_dt() + + # add callback to deal the simulation app when simulation is stopped. + # this is needed because physics views go invalid once we stop the simulation + timeline_event_stream = self._timeline_iface.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 + + @property + def has_gui(self) -> bool: + """Whether any GUI is available (local, livestreamed, or XR).""" + return bool(self._sim.carb_settings.get("/isaaclab/has_gui")) + + def is_playing(self) -> bool: + """Check whether the simulation is playing.""" + return self._timeline_iface.is_playing() + + def is_stopped(self) -> bool: + """Check whether the simulation is stopped.""" + return self._timeline_iface.is_stopped() + + def play(self) -> None: + """Start playing the simulation.""" + self._timeline_iface.play() + self._timeline_iface.commit() + # perform one step to propagate all physics handles properly + self._sim.set_setting("/app/player/playSimulations", False) + self._sim.app.update() + self._sim.set_setting("/app/player/playSimulations", True) + + def pause(self) -> None: + """Pause the simulation.""" + self._timeline_iface.pause() + self._timeline_iface.commit() + + def stop(self) -> None: + """Stop the simulation.""" + self._timeline_iface.stop() + self._timeline_iface.commit() + # perform one step to propagate all physics handles properly + self._sim.set_setting("/app/player/playSimulations", False) + self._sim.app.update() + self._sim.set_setting("/app/player/playSimulations", True) + + @property + def offscreen_render(self) -> bool: + """Whether offscreen rendering is enabled.""" + return self._offscreen_render + + @property + def render_viewport(self) -> bool: + """Whether the default viewport should be rendered.""" + return self._render_viewport + + def set_render_mode(self, mode: RenderMode): + """Change the current render mode of the simulation. + + Please see :class:`RenderMode` for more information on the different render modes. + + .. note:: + When no GUI is available (locally or livestreamed), we do not need to choose whether the viewport + needs to render or not (since there is no GUI). Thus, in this case, calling the function will not + change the render mode. + + Args: + mode: The rendering mode. If different than current rendering mode, + the mode is changed to the new mode. + + Raises: + ValueError: If the input mode is not supported. + """ + # check if mode change is possible -- not possible when no GUI is available + if not self._sim.carb_settings.get("/isaaclab/has_gui"): + self._sim.logger.warning( + f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." + ) + return + # check if there is a mode change + # note: this is mostly needed for GUI when we want to switch between full rendering and no rendering. + if mode != self.render_mode: + if mode == RenderMode.FULL_RENDERING: + # display the viewport and enable updates + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = True # pyright: ignore [reportOptionalMemberAccess] + elif mode == RenderMode.PARTIAL_RENDERING: + # hide the viewport and disable updates + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + elif mode == RenderMode.NO_RENDERING: + # hide the viewport and disable updates + if self._viewport_context is not None: + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + # reset the throttle counter + self._render_throttle_counter = 0 + else: + raise ValueError(f"Unsupported render mode: {mode}! Please check `RenderMode` for details.") + # update render mode + self.render_mode = mode + + def set_camera_view( + self, + eye: tuple[float, float, float], + target: tuple[float, float, float], + camera_prim_path: str = "/OmniverseKit_Persp", + ): + """Set the location and target of the viewport camera in the stage. + + Note: + This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. + It is provided here for convenience to reduce the amount of imports needed. + + Args: + eye: The location of the camera eye. + target: The location of the camera target. + camera_prim_path: The path to the camera primitive in the stage. Defaults to + "/OmniverseKit_Persp". + """ + # safe call only if we have a GUI or viewport rendering enabled + if self._sim.carb_settings.get("/isaaclab/has_gui") or self._offscreen_render or self._render_viewport: + set_camera_view(eye, target, camera_prim_path) + + def render(self, mode: RenderMode | None = None): + """Refreshes the rendering components including UI elements and view-ports depending on the render mode. + + This function is used to refresh the rendering components of the simulation. This includes updating the + view-ports, UI elements, and other extensions (besides physics simulation) that are running in the + background. The rendering components are refreshed based on the render mode. + + Please see :class:`RenderMode` for more information on the different render modes. + + Args: + mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. + """ + # check if we need to change the render mode + if mode is not None: + self.set_render_mode(mode) + # render based on the render mode + if self.render_mode == RenderMode.NO_GUI_OR_RENDERING: + # we never want to render anything here (this is for complete headless mode) + pass + elif self.render_mode == RenderMode.NO_RENDERING: + # throttle the rendering frequency to keep the UI responsive + self._render_throttle_counter += 1 + 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 + self._sim.set_setting("/app/player/playSimulations", False) + self._sim.app.update() + self._sim.set_setting("/app/player/playSimulations", True) + else: + # manually flush the fabric data to update Hydra textures + self._sim.forward() + # render the simulation + # 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._sim.set_setting("/app/player/playSimulations", False) + self._sim.app.update() + self._sim.set_setting("/app/player/playSimulations", True) + + # app.update() may be changing the cuda device, so we force it back to our desired device here + if "cuda" in self._sim.device: + torch.cuda.set_device(self._sim.device) + + def reset(self, soft: bool = False) -> None: + """Reset visualizer (warmup renders on hard reset). + + Args: + soft: If True, skip warmup renders. + """ + if not soft: + for _ in range(2): + self.render() + + def forward(self) -> None: + """No-op for visualizer (rendering happens in render()).""" + pass + + def step(self, render: bool = True) -> None: + """Step visualizer (delegates to render if enabled). + + Args: + render: Whether to render after stepping. + """ + # Rendering is handled by SimulationContext.step() calling render() + pass + + def close(self) -> None: + """Clean up visualizer resources.""" + # Unsubscribe from stop handle + if self._app_control_on_stop_handle is not None: + self._app_control_on_stop_handle.unsubscribe() + self._app_control_on_stop_handle = None + + def set_stop_handle_enabled(self, enabled: bool) -> None: + """Enable or disable the app control on stop handle. + + Args: + enabled: If True, the stop handle callback is active. + """ + self._disable_app_control_on_stop_handle = not enabled + + 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 + resume the simulation from the last state. This leaves the app in an inconsistent state, where + two possible actions can be taken: + + 1. **Keep the app rendering**: In this case, the simulation is kept running and the app is not shutdown. + However, the physics is not updated and the script cannot be resumed from the last state. The + user has to manually close the app to stop the simulation. + 2. **Shutdown the app**: This is the default behavior. In this case, the app is shutdown and + the simulation is stopped. + + Note: + 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. + """ + pass + # if not self._disable_app_control_on_stop_handle: + # while not omni.timeline.get_timeline_interface().is_playing(): + # self.render() + + def _apply_render_settings_from_cfg(self): # noqa: C901 + """Sets rtx settings specified in the RenderCfg.""" + + # define mapping of user-friendly RenderCfg names to native carb names + rendering_setting_name_mapping = { + "enable_translucency": "/rtx/translucency/enabled", + "enable_reflections": "/rtx/reflections/enabled", + "enable_global_illumination": "/rtx/indirectDiffuse/enabled", + "enable_dlssg": "/rtx-transient/dlssg/enabled", + "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", + "dlss_mode": "/rtx/post/dlss/execMode", + "enable_direct_lighting": "/rtx/directLighting/enabled", + "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", + "enable_shadows": "/rtx/shadows/enabled", + "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", + "dome_light_upper_lower_strategy": "/rtx/domeLight/upperLowerStrategy", + } + + not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] + + # grab the rendering mode using the following priority: + # 1. command line argument --rendering_mode, if provided + # 2. rendering_mode from Render Config, if set + # 3. lastly, default to "balanced" mode, if neither is specified + rendering_mode = self._sim.carb_settings.get("/isaaclab/rendering/rendering_mode") + if not rendering_mode: + rendering_mode = self._sim.cfg.render.rendering_mode + if not rendering_mode: + rendering_mode = "balanced" + + # set preset settings (same behavior as the CLI arg --rendering_mode) + if rendering_mode is not None: + # check if preset is supported + supported_rendering_modes = ["performance", "balanced", "quality"] + if rendering_mode not in supported_rendering_modes: + raise ValueError( + f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." + ) + + # grab isaac lab apps path + isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") + # for Isaac Sim 4.5 compatibility, we use the 4.5 rendering mode app files in a different folder + if get_isaac_sim_version().major < 5: + isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") + + # grab preset settings + preset_filename = os.path.join(isaaclab_app_exp_path, f"rendering_modes/{rendering_mode}.kit") + with open(preset_filename) as file: + preset_dict = toml.load(file) + preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) + + # set presets + for key, value in preset_dict.items(): + key = "/" + key.replace(".", "/") # convert to carb setting format + self._sim.set_setting(key, value) + + # set user-friendly named settings + for key, value in vars(self._sim.cfg.render).items(): + if value is None or key in not_carb_settings: + # skip unset settings and non-carb settings + continue + if key not in rendering_setting_name_mapping: + raise ValueError( + f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" + " need to be updated." + ) + key = rendering_setting_name_mapping[key] + self._sim.set_setting(key, value) + + # set general carb settings + carb_settings = self._sim.cfg.render.carb_settings + if carb_settings is not None: + for key, value in carb_settings.items(): + if "_" in key: + key = "/" + key.replace("_", "/") # convert from python variable style string + elif "." in key: + key = "/" + key.replace(".", "/") # convert from .kit file style string + if self._sim.get_setting(key) is None: + raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") + self._sim.set_setting(key, value) + + # set denoiser mode + if self._sim.cfg.render.antialiasing_mode is not None: + try: + import omni.replicator.core as rep + + rep.settings.set_render_rtx_realtime(antialiasing=self._sim.cfg.render.antialiasing_mode) + except Exception: + pass + + # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. + if self._sim.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": + self._sim.carb_settings.set_string("/rtx/rendermode", "RaytracedLighting") + + def _configure_rendering_dt(self): + """Configures the rendering/timeline rate based on physics dt and render interval.""" + from pxr import Usd + + cfg = self._sim.cfg + stage = self._sim.stage + carb_settings = self._sim.carb_settings + + # compute rendering frequency + render_interval = max(cfg.render_interval, 1) + rendering_hz = int(1.0 / (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 carb_settings.get("/app/runLoops/main/rateLimitEnabled"): + carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) + + # set stage time codes per second + with Usd.EditContext(stage, stage.GetRootLayer()): + 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(cfg.dt * render_interval) + _loop_runner.set_manual_mode(True) + except Exception: + self._sim.logger.warning( + "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" + ) + carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) + carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) From 930985f8030f2502427c64521b6ba2f7fe64dda9 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 09:50:43 -0800 Subject: [PATCH 110/130] refactored visualizer interface further --- source/isaaclab/isaaclab/sim/physx_backend.py | 2 +- .../isaaclab/sim/simulation_context.py | 52 +++--------------- .../isaaclab/sim/visualizer_interface.py | 54 +++++++++++++++---- 3 files changed, 50 insertions(+), 58 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/physx_backend.py b/source/isaaclab/isaaclab/sim/physx_backend.py index 44351972080..9743d151fcc 100644 --- a/source/isaaclab/isaaclab/sim/physx_backend.py +++ b/source/isaaclab/isaaclab/sim/physx_backend.py @@ -430,7 +430,7 @@ def _apply_physics_settings(self): # -------------------------- # set gravity - gravity = self._sim._gravity_tensor + gravity = torch.tensor(self._sim.cfg.gravity, dtype=torch.float32, device=self._physics_device) gravity_magnitude = torch.norm(gravity).item() # avoid division by zero if gravity_magnitude == 0.0: diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index f318786d486..5ba3fdfbbe4 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -129,19 +129,9 @@ def __init__(self, cfg: SimulationCfg | None = None): # initialize visualizer interface (handles viewport, render mode, render settings) self._visualizer = VisualizerInterface(self) - - # 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. - # the issue is with some heap memory corruption when torch tensor is created inside the asset class. - # you can reproduce the issue by commenting out this line and running the test `test_articulation.py`. - self._gravity_tensor = torch.tensor(self.cfg.gravity, dtype=torch.float32, device=self.cfg.device) - # define a global variable to store the exceptions raised in the callback stack builtins.ISAACLAB_CALLBACK_EXCEPTION = None - # obtain interfaces for simulation - self._app_iface = omni.kit.app.get_app_interface() - # initialize physics backend (handles scene creation, settings, fabric) self._physics_backend = PhysXBackend(self) @@ -199,7 +189,7 @@ def clear_instance(cls) -> None: @property def app(self) -> omni.kit.app.IApp: """Omniverse Kit Application interface.""" - return self._app_iface + return self._visualizer.app @property def stage(self) -> Usd.Stage: @@ -410,26 +400,9 @@ 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. """ - # reset the simulation - if not soft: - # disable app control on stop handle - self._visualizer.set_stop_handle_enabled(False) - if not self.is_stopped(): - self.stop() - self._visualizer.set_stop_handle_enabled(True) - # play the simulation - self.play() - # check for callback exceptions - self._check_for_callback_exceptions() - - # reset physics backend (initializes physics, resets cuda device, kinematic bodies) self._physics_backend.reset(soft) - - # 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() + self._visualizer.reset(soft) + self._check_for_callback_exceptions() def forward(self) -> None: """Updates articulation kinematics and fabric for rendering.""" @@ -445,22 +418,9 @@ 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 the simulation timeline is paused. in that case keep stepping until it is playing - if not self.is_playing(): - # step the simulator (but not the physics) to have UI still active - while not self.is_playing(): - self.render() - # meantime if someone stops, break out of the loop - if self.is_stopped(): - break - # need to do one step to refresh the app - # 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_iface.update() - - # step the physics simulation - self._physics_backend.step(render) + self._visualizer.step(render) + if self.is_playing(): + self._physics_backend.step(render) def render(self, mode: RenderMode | None = None): """Refreshes the rendering components including UI elements and view-ports depending on the render mode. diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index 2b63acb2f78..abe2d6b1c61 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -16,6 +16,7 @@ import weakref from typing import TYPE_CHECKING +import omni.kit.app import omni.timeline from isaacsim.core.utils.viewports import set_camera_view @@ -85,6 +86,9 @@ def __init__(self, sim_context: "SimulationContext"): """ self._sim = sim_context + # acquire application interface + self._app_iface = omni.kit.app.get_app_interface() + # Detect render flags self._local_gui = self._sim.carb_settings.get("/app/window/enabled") self._livestream_gui = self._sim.carb_settings.get("/app/livestream/enabled") @@ -165,6 +169,11 @@ def has_gui(self) -> bool: """Whether any GUI is available (local, livestreamed, or XR).""" return bool(self._sim.carb_settings.get("/isaaclab/has_gui")) + @property + def app(self) -> omni.kit.app.IApp: + """Omniverse Kit Application interface.""" + return self._app_iface + def is_playing(self) -> bool: """Check whether the simulation is playing.""" return self._timeline_iface.is_playing() @@ -179,7 +188,7 @@ def play(self) -> None: self._timeline_iface.commit() # perform one step to propagate all physics handles properly self._sim.set_setting("/app/player/playSimulations", False) - self._sim.app.update() + self._app_iface.update() self._sim.set_setting("/app/player/playSimulations", True) def pause(self) -> None: @@ -193,7 +202,7 @@ def stop(self) -> None: self._timeline_iface.commit() # perform one step to propagate all physics handles properly self._sim.set_setting("/app/player/playSimulations", False) - self._sim.app.update() + self._app_iface.update() self._sim.set_setting("/app/player/playSimulations", True) @property @@ -300,7 +309,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._sim.set_setting("/app/player/playSimulations", False) - self._sim.app.update() + self._app_iface.update() self._sim.set_setting("/app/player/playSimulations", True) else: # manually flush the fabric data to update Hydra textures @@ -309,7 +318,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._sim.set_setting("/app/player/playSimulations", False) - self._sim.app.update() + self._app_iface.update() self._sim.set_setting("/app/player/playSimulations", True) # app.update() may be changing the cuda device, so we force it back to our desired device here @@ -317,12 +326,20 @@ def render(self, mode: RenderMode | None = None): torch.cuda.set_device(self._sim.device) def reset(self, soft: bool = False) -> None: - """Reset visualizer (warmup renders on hard reset). + """Reset visualizer (timeline control + warmup renders on hard reset). Args: - soft: If True, skip warmup renders. + soft: If True, skip timeline reset and warmup. """ if not soft: + # disable app control on stop handle + self.set_stop_handle_enabled(False) + if not self.is_stopped(): + self.stop() + self.set_stop_handle_enabled(True) + # play the simulation + self.play() + # warmup renders to initialize replicator buffers for _ in range(2): self.render() @@ -330,14 +347,29 @@ def forward(self) -> None: """No-op for visualizer (rendering happens in render()).""" pass - def step(self, render: bool = True) -> None: - """Step visualizer (delegates to render if enabled). + def step(self, render: bool = True) -> bool: + """Handle pause loop and prepare for physics step. + + This method blocks while the timeline is paused, keeping the UI responsive. + It returns whether physics should proceed (True if playing, False if stopped). Args: - render: Whether to render after stepping. + render: Whether to render while waiting. + + Returns: + True if physics should step (timeline is playing), False if stopped. """ - # Rendering is handled by SimulationContext.step() calling render() - pass + # if paused, keep rendering until playing or stopped + was_paused = not self.is_playing() + while not self.is_playing() and not self.is_stopped(): + self.render() + + # refresh app after resuming from pause (physics needs to re-parse the scene) + if was_paused and self.is_playing(): + self._app_iface.update() + + # return whether physics should proceed + return self.is_playing() def close(self) -> None: """Clean up visualizer resources.""" From 68a9a6cebe2d1d191ca7e2bd15b68fe54337cd6c Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 18:34:46 -0800 Subject: [PATCH 111/130] refactore step 7: deprecating sim.app --- source/isaaclab/isaaclab/envs/ui/base_env_window.py | 2 +- source/isaaclab/isaaclab/envs/ui/empty_window.py | 2 +- source/isaaclab/isaaclab/sim/simulation_context.py | 6 ------ source/isaaclab/test/sim/test_simulation_context.py | 2 +- 4 files changed, 3 insertions(+), 9 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index db047afb08d..3958dcf4959 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -448,7 +448,7 @@ async def _dock_window(self, window_title: str): for _ in range(5): if omni.ui.Workspace.get_window(window_title): break - await self.env.sim.app.next_update_async() + await self.env.sim._visualizer.app.next_update_async() # dock next to properties window custom_window = omni.ui.Workspace.get_window(window_title) diff --git a/source/isaaclab/isaaclab/envs/ui/empty_window.py b/source/isaaclab/isaaclab/envs/ui/empty_window.py index bc12f862d1b..3b6ecd80aaf 100644 --- a/source/isaaclab/isaaclab/envs/ui/empty_window.py +++ b/source/isaaclab/isaaclab/envs/ui/empty_window.py @@ -69,7 +69,7 @@ async def _dock_window(self, window_title: str): for _ in range(5): if omni.ui.Workspace.get_window(window_title): break - await self.env.sim.app.next_update_async() + await self.env.sim._visualizer.app.next_update_async() # dock next to properties window custom_window = omni.ui.Workspace.get_window(window_title) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 5ba3fdfbbe4..3ee466cf908 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -7,7 +7,6 @@ import builtins import logging -import torch import traceback from collections.abc import Iterator from contextlib import contextmanager @@ -186,11 +185,6 @@ def clear_instance(cls) -> None: Properties. """ - @property - def app(self) -> omni.kit.app.IApp: - """Omniverse Kit Application interface.""" - return self._visualizer.app - @property def stage(self) -> Usd.Stage: """USD Stage.""" diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index c66c2b2e066..504469daff1 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -56,7 +56,7 @@ def test_init(device): sim = SimulationContext(cfg=cfg) # verify app interface is valid - assert sim.app is not None + assert sim._visualizer.app is not None # verify stage is valid assert sim.stage is not None # verify device property From 7d13fb9db6ac9929178e5aeb0f4a387d67a33bf4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 20:00:34 -0800 Subject: [PATCH 112/130] refactor step 8: unified physics interface, physics backend, and physx_backend --- .../isaaclab/isaaclab/sim/_impl/__init__.py | 16 +++ .../isaaclab/sim/_impl/physics_backend.py | 53 +++++++ .../isaaclab/sim/{ => _impl}/physx_backend.py | 116 +++++----------- source/isaaclab/isaaclab/sim/interface.py | 65 +++++++++ .../isaaclab/sim/physics_interface.py | 130 ++++++++++++++++++ .../isaaclab/sim/simulation_context.py | 18 +-- 6 files changed, 310 insertions(+), 88 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/_impl/__init__.py create mode 100644 source/isaaclab/isaaclab/sim/_impl/physics_backend.py rename source/isaaclab/isaaclab/sim/{ => _impl}/physx_backend.py (85%) create mode 100644 source/isaaclab/isaaclab/sim/interface.py create mode 100644 source/isaaclab/isaaclab/sim/physics_interface.py diff --git a/source/isaaclab/isaaclab/sim/_impl/__init__.py b/source/isaaclab/isaaclab/sim/_impl/__init__.py new file mode 100644 index 00000000000..d9f28e295c4 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/_impl/__init__.py @@ -0,0 +1,16 @@ +# 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 + +"""Implementation backends for simulation interfaces.""" + +# from .newton_backend import NewtonBackend +from .physics_backend import PhysicsBackend +from .physx_backend import PhysXBackend + +__all__ = [ + # "NewtonBackend", + "PhysicsBackend", + "PhysXBackend", +] diff --git a/source/isaaclab/isaaclab/sim/_impl/physics_backend.py b/source/isaaclab/isaaclab/sim/_impl/physics_backend.py new file mode 100644 index 00000000000..e46028bfda0 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/_impl/physics_backend.py @@ -0,0 +1,53 @@ +# 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 + +"""Base class for physics backends.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext + + +class PhysicsBackend(ABC): + """Base class for physics simulation backends. + + Lifecycle: __init__() -> initialize() -> step() (repeated) -> close() + """ + + def __init__(self, sim_context: "SimulationContext"): + """Initialize backend with simulation context. + + Args: + sim_context: Parent simulation context. + """ + self._sim = sim_context + + @abstractmethod + def reset(self, soft: bool = False) -> None: + """Reset physics simulation. + + Args: + soft: If True, skip full reinitialization. + """ + pass + + @abstractmethod + def forward(self) -> None: + """Update kinematics without stepping physics.""" + pass + + @abstractmethod + def step(self) -> None: + """Step physics simulation by one timestep (physics only, no rendering).""" + pass + + @abstractmethod + def close(self) -> None: + """Clean up physics resources.""" + pass diff --git a/source/isaaclab/isaaclab/sim/physx_backend.py b/source/isaaclab/isaaclab/sim/_impl/physx_backend.py similarity index 85% rename from source/isaaclab/isaaclab/sim/physx_backend.py rename to source/isaaclab/isaaclab/sim/_impl/physx_backend.py index 9743d151fcc..499080ce6e2 100644 --- a/source/isaaclab/isaaclab/sim/physx_backend.py +++ b/source/isaaclab/isaaclab/sim/_impl/physx_backend.py @@ -20,9 +20,10 @@ import omni.kit.app import omni.physx import omni.physics.tensors -from pxr import Gf, PhysxSchema, Sdf, UsdGeom, UsdPhysics +from pxr import PhysxSchema, Sdf import isaaclab.sim as sim_utils +from isaaclab.sim._impl.physics_backend import PhysicsBackend from isaaclab.sim.simulation_manager import SimulationManager if TYPE_CHECKING: @@ -182,11 +183,10 @@ def _update_usda_start_time(file_path: str, start_time: float) -> None: file.write(content) -class PhysXBackend: +class PhysXBackend(PhysicsBackend): """PhysX physics backend. This class manages the PhysX physics simulation, including: - - Physics scene creation and configuration - Device settings (CPU/GPU) - Timestep and solver configuration - Fabric interface for fast data access @@ -201,7 +201,7 @@ def __init__(self, sim_context: "SimulationContext"): Args: sim_context: Parent simulation context. """ - self._sim = sim_context + super().__init__(sim_context) # acquire physics interfaces self._physx_iface = omni.physx.get_physx_interface() @@ -214,12 +214,7 @@ def __init__(self, sim_context: "SimulationContext"): self._fabric_iface = None self._update_fabric = None - # Physics scene references (will be set in _init_physics_scene) - self._physics_scene = None - self._physx_scene_api = None - - # Initialize physics - self._init_physics_scene() + # Configure physics self._configure_simulation_dt() self._apply_physics_settings() SimulationManager.initialize() @@ -227,14 +222,14 @@ def __init__(self, sim_context: "SimulationContext"): # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes # when in headless mode self._sim.set_setting("/app/player/playSimulations", False) - self._sim.app.update() + omni.kit.app.get_app().update() self._sim.set_setting("/app/player/playSimulations", True) # load flatcache/fabric interface self._load_fabric_interface() # initialize animation recorder - self._anim_recorder = AnimationRecorder(self._sim.carb_settings, self._sim.app) + self._anim_recorder = AnimationRecorder(self._sim.carb_settings, omni.kit.app.get_app()) @property def device(self) -> str: @@ -251,6 +246,10 @@ def physics_sim_view(self) -> "omni.physics.tensors.SimulationView": """Physics simulation view with torch backend.""" return SimulationManager.get_physics_sim_view() + def is_fabric_enabled(self) -> bool: + """Returns whether the fabric interface is enabled.""" + return self._fabric_iface is not None + def reset(self, soft: bool = False) -> None: """Reset the physics simulation. @@ -273,41 +272,29 @@ def reset(self, soft: bool = False) -> None: def forward(self) -> None: """Update articulation kinematics and fabric for rendering.""" - if self._fabric_iface is not None: + if self._fabric_iface is not None and self._update_fabric is not None: physics_sim_view = SimulationManager.get_physics_sim_view() if physics_sim_view is not None and self._sim.is_playing(): # Update the articulations' link's poses before rendering physics_sim_view.update_articulations_kinematic() self._update_fabric(0.0, 0.0) - def step(self, render: bool = True) -> bool: - """Step the physics simulation. - - Args: - render: If True, step via app.update() which includes rendering. - If False, step physics only without rendering. - - Returns: - True if animation recording finished and app should shutdown, False otherwise. - """ + def step(self) -> None: + """Step the physics simulation (physics only, no rendering).""" # update animation recorder if enabled if self._anim_recorder.enabled and self._anim_recorder.update(): logger.warning("Animation recording finished. Closing app.") - self._sim.app.shutdown() - return True + omni.kit.app.get_app().shutdown() + return - if render: - self._sim.app.update() - else: - self._physx_sim_iface.simulate(self._sim.cfg.dt, 0.0) - self._physx_sim_iface.fetch_results() + # step physics only + self._physx_sim_iface.simulate(self._sim.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 + # physics step may change cuda device, force it back if "cuda" in self._physics_device: torch.cuda.set_device(self._physics_device) - return False - def close(self) -> None: """Clean up physics resources.""" # clear the simulation manager state (notifies assets to cleanup) @@ -320,34 +307,16 @@ def close(self) -> None: # Private initialization methods # ------------------------- - def _init_physics_scene(self) -> None: - """Initialize the USD physics scene.""" - stage = self._sim.stage - cfg = self._sim.cfg - - with sim_utils.use_stage(stage): - # correct conventions for metric units - UsdGeom.SetStageUpAxis(stage, "Z") - UsdGeom.SetStageMetersPerUnit(stage, 1.0) - UsdPhysics.SetStageKilogramsPerUnit(stage, 1.0) - - # find if any physics prim already exists and delete it - for prim in stage.Traverse(): - if prim.HasAPI(PhysxSchema.PhysxSceneAPI) or prim.GetTypeName() == "PhysicsScene": - sim_utils.delete_prim(prim.GetPath().pathString, stage=stage) - - # create a new physics scene - if stage.GetPrimAtPath(cfg.physics_prim_path).IsValid(): - raise RuntimeError(f"A prim already exists at path '{cfg.physics_prim_path}'.") - - self._physics_scene = UsdPhysics.Scene.Define(stage, cfg.physics_prim_path) - self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene.GetPrim()) - def _configure_simulation_dt(self): """Configures the physics simulation step size.""" cfg = self._sim.cfg carb_settings = self._sim.carb_settings + # Get physics scene API from the physics interface + stage = self._sim.stage + physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) + physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) + # 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). @@ -355,7 +324,7 @@ def _configure_simulation_dt(self): # set simulation step per second steps_per_second = int(1.0 / cfg.dt) - self._physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) + physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) # set minimum number of steps per frame min_steps = int(steps_per_second / render_interval) carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) @@ -365,6 +334,11 @@ def _apply_physics_settings(self): cfg = self._sim.cfg carb_settings = self._sim.carb_settings + # Get physics scene API from the physics interface + stage = self._sim.stage + physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) + physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) + # -------------------------- # Carb Physics API settings # -------------------------- @@ -429,18 +403,6 @@ def _apply_physics_settings(self): # USDPhysics API settings # -------------------------- - # set gravity - gravity = torch.tensor(self._sim.cfg.gravity, dtype=torch.float32, device=self._physics_device) - gravity_magnitude = torch.norm(gravity).item() - # avoid division by zero - 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)) - 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"{cfg.physics_prim_path}/defaultMaterial" @@ -454,10 +416,10 @@ def _apply_physics_settings(self): # set broadphase type broadphase_type = "GPU" if "cuda" in cfg.device else "MBP" - self._physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) + physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) # set gpu dynamics enable_gpu_dynamics = "cuda" in cfg.device - self._physx_scene_api.CreateEnableGPUDynamicsAttr(enable_gpu_dynamics) + 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 cfg.physx.enable_ccd: @@ -466,14 +428,14 @@ def _apply_physics_settings(self): "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxCfg config to avoid this" " warning." ) - self._physx_scene_api.CreateEnableCCDAttr(cfg.physx.enable_ccd) + physx_scene_api.CreateEnableCCDAttr(cfg.physx.enable_ccd) # set solver type solver_type = "PGS" if cfg.physx.solver_type == 0 else "TGS" - self._physx_scene_api.CreateSolverTypeAttr(solver_type) + physx_scene_api.CreateSolverTypeAttr(solver_type) # set solve articulation contact last - attr = self._physx_scene_api.GetPrim().CreateAttribute( + attr = physx_scene_api.GetPrim().CreateAttribute( "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool ) attr.Set(cfg.physx.solve_articulation_contact_last) @@ -484,7 +446,7 @@ def _apply_physics_settings(self): 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) + sim_utils.safe_set_attribute_on_usd_schema(physx_scene_api, key, value, camel_case=True) # throw warnings for helpful guidance if cfg.physx.solver_type == 1 and not cfg.physx.enable_external_forces_every_iteration: @@ -545,7 +507,3 @@ def _load_fabric_interface(self): carb_settings.set_bool("/physics/updateResidualsToUsd", not cfg.use_fabric) # disable simulation output window visibility carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) - - def is_fabric_enabled(self) -> bool: - """Returns whether the fabric interface is enabled.""" - return self._fabric_iface is not None diff --git a/source/isaaclab/isaaclab/sim/interface.py b/source/isaaclab/isaaclab/sim/interface.py new file mode 100644 index 00000000000..e8bdb4423cd --- /dev/null +++ b/source/isaaclab/isaaclab/sim/interface.py @@ -0,0 +1,65 @@ +# 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 + +"""Base interface class for SimulationContext subsystems.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from .simulation_context import SimulationContext + + +class Interface(ABC): + """Base class for simulation subsystem interfaces. + + Provides a common lifecycle: __init__() -> reset() -> step()/forward() (repeated) -> close() + """ + + def __init__(self, sim_context: "SimulationContext"): + """Initialize interface with simulation context. + + Args: + sim_context: Parent simulation context. + """ + self._sim = sim_context + + @abstractmethod + def reset(self, soft: bool = False) -> None: + """Reset the subsystem. + + Args: + soft: If True, skip full reinitialization. + """ + pass + + @abstractmethod + def forward(self) -> None: + """Update state without stepping simulation.""" + pass + + @abstractmethod + def step(self, render: bool = True) -> None: + """Step the subsystem by one timestep. + + Args: + render: Whether to render after stepping. Defaults to True. + """ + pass + + @abstractmethod + def close(self) -> None: + """Clean up resources.""" + pass + + def play(self) -> None: + """Handle simulation start.""" + pass + + def stop(self) -> None: + """Handle simulation stop.""" + pass diff --git a/source/isaaclab/isaaclab/sim/physics_interface.py b/source/isaaclab/isaaclab/sim/physics_interface.py new file mode 100644 index 00000000000..98268ec0b67 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/physics_interface.py @@ -0,0 +1,130 @@ +# 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 + +"""Physics interface for SimulationContext.""" + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING + +import torch +from pxr import Gf, UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +from ._impl.physx_backend import PhysXBackend +from ._impl.physics_backend import PhysicsBackend +from .interface import Interface + +if TYPE_CHECKING: + from .simulation_context import SimulationContext + +logger = logging.getLogger(__name__) + + +class PhysicsInterface(Interface): + """Manages USD physics scene and delegates to physics backends. + + This interface handles: + - USD physics scene creation and configuration + - Gravity, timestep, and unit settings + - Delegating lifecycle operations to registered backends + """ + + def __init__(self, sim_context: "SimulationContext"): + """Initialize physics scene and backends. + + Args: + sim_context: Parent simulation context. + """ + super().__init__(sim_context) + + self.physics_prim_path = self._sim.cfg.physics_prim_path + self.backend = "torch" + # Initialize USD physics scene + self._init_usd_physics_scene() + self._backends: list[PhysicsBackend] = [PhysXBackend(sim_context)] + + @property + def backends(self) -> list[PhysicsBackend]: + """List of active physics backends.""" + return self._backends + + @property + def physics_dt(self) -> float: + """Physics timestep.""" + return self._sim.cfg.dt + + @property + def rendering_dt(self) -> float: + """Rendering timestep.""" + return self._sim.cfg.dt * self._sim.cfg.render_interval + + @property + def device(self) -> str: + """Device used for physics simulation.""" + return self._sim.cfg.device + + def _init_usd_physics_scene(self) -> None: + """Create and configure the USD physics scene.""" + stage = self._sim.stage + with sim_utils.use_stage(stage): + # Set stage conventions for metric units + UsdGeom.SetStageUpAxis(stage, "Z") + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + UsdPhysics.SetStageKilogramsPerUnit(stage, 1.0) + + # Find and delete any existing physics scene + for prim in stage.Traverse(): + if prim.GetTypeName() == "PhysicsScene": + sim_utils.delete_prim(prim.GetPath().pathString, stage=stage) + + # Create a new physics scene + if stage.GetPrimAtPath(self._sim.cfg.physics_prim_path).IsValid(): + raise RuntimeError(f"A prim already exists at path '{self._sim.cfg.physics_prim_path}'.") + + physics_scene = UsdPhysics.Scene.Define(stage, self._sim.cfg.physics_prim_path) + + # Pre-create gravity tensor to avoid torch heap corruption issues (torch 2.1+) + gravity = torch.tensor(self._sim.cfg.gravity, dtype=torch.float32, device=self.device) + gravity_magnitude = torch.norm(gravity).item() + + # Avoid division by zero + if gravity_magnitude == 0.0: + gravity_direction = [0.0, 0.0, -1.0] + else: + gravity_direction = (gravity / gravity_magnitude).tolist() + + physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) + physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) + + def reset(self, soft: bool = False) -> None: + """Reset all physics backends. + + Args: + soft: If True, skip full reinitialization. + """ + for backend in self._backends: + backend.reset(soft) + + def forward(self) -> None: + """Update articulation kinematics on all backends.""" + for backend in self._backends: + backend.forward() + + def step(self, render: bool = True) -> None: + """Step all physics backends (physics only). + + Args: + render: Unused, kept for interface compatibility. + """ + for backend in self._backends: + backend.step() + + def close(self) -> None: + """Clean up all physics backends.""" + for backend in self._backends: + backend.close() + self._backends.clear() diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 3ee466cf908..16e00347e41 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -21,7 +21,7 @@ from isaaclab.utils.logger import configure_logging from isaaclab.utils.version import get_isaac_sim_version -from .physx_backend import PhysXBackend +from .physics_interface import PhysicsInterface from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg from .visualizer_interface import RenderMode, VisualizerInterface @@ -131,8 +131,8 @@ def __init__(self, cfg: SimulationCfg | None = None): # define a global variable to store the exceptions raised in the callback stack builtins.ISAACLAB_CALLBACK_EXCEPTION = None - # initialize physics backend (handles scene creation, settings, fabric) - self._physics_backend = PhysXBackend(self) + # initialize physics interface (handles scene creation, delegates to backends) + self._physics_interface = PhysicsInterface(self) def __new__(cls, *args, **kwargs) -> SimulationContext: """Returns the instance of the simulation context. @@ -170,8 +170,8 @@ def clear_instance(cls) -> None: return # close visualizer (unsubscribes stop handle) cls._instance._visualizer.close() - # close physics backend (clears SimulationManager, detaches physx stage) - cls._instance._physics_backend.close() + # close physics interface (clears SimulationManager, detaches physx stage) + cls._instance._physics_interface.close() # detach the stage from the USD stage cache stage_cache = UsdUtils.StageCache.Get() stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() @@ -198,7 +198,7 @@ def device(self) -> str: In Omniverse, it is possible to configure multiple GPUs for rendering, while physics engine operates on a single GPU. This function returns the device that is used for physics simulation. """ - return self._physics_backend.device + return self._physics_interface.device @property def render_mode(self) -> RenderMode: @@ -394,13 +394,13 @@ 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. """ - self._physics_backend.reset(soft) + self._physics_interface.reset(soft) self._visualizer.reset(soft) self._check_for_callback_exceptions() def forward(self) -> None: """Updates articulation kinematics and fabric for rendering.""" - self._physics_backend.forward() + self._physics_interface.forward() def step(self, render: bool = True): """Steps the simulation. @@ -414,7 +414,7 @@ def step(self, render: bool = True): """ self._visualizer.step(render) if self.is_playing(): - self._physics_backend.step(render) + self._physics_interface.step(render) def render(self, mode: RenderMode | None = None): """Refreshes the rendering components including UI elements and view-ports depending on the render mode. From 0cb95dde7eff95e7740e08f96425ad7964e451cd Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 20:27:06 -0800 Subject: [PATCH 113/130] refactor step 9: rename simulation manager to physx manager --- .../benchmarks/benchmark_view_comparison.py | 4 +- .../assets/articulation/articulation.py | 4 +- .../assets/articulation/articulation_data.py | 4 +- source/isaaclab/isaaclab/assets/asset_base.py | 6 +-- .../deformable_object/deformable_object.py | 4 +- .../assets/rigid_object/rigid_object.py | 4 +- .../rigid_object_collection.py | 4 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 4 +- .../isaaclab/envs/manager_based_env.py | 4 +- .../sensors/contact_sensor/contact_sensor.py | 4 +- .../frame_transformer/frame_transformer.py | 4 +- source/isaaclab/isaaclab/sensors/imu/imu.py | 4 +- .../isaaclab/sensors/ray_caster/ray_caster.py | 4 +- .../isaaclab/isaaclab/sensors/sensor_base.py | 6 +-- source/isaaclab/isaaclab/sim/__init__.py | 8 ++-- .../isaaclab/isaaclab/sim/_impl/__init__.py | 3 ++ .../isaaclab/sim/_impl/physx_backend.py | 20 ++++----- .../physx_manager.py} | 8 ++-- .../isaaclab/sim/simulation_context.py | 2 +- source/isaaclab/isaaclab/sim/utils/stage.py | 6 +-- .../test/sim/test_simulation_context.py | 42 +++++++++---------- 21 files changed, 76 insertions(+), 73 deletions(-) rename source/isaaclab/isaaclab/sim/{simulation_manager.py => _impl/physx_manager.py} (98%) diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 44439d857f5..dd93aac035e 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -67,7 +67,7 @@ import time import torch -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.sim.views import XformPrimView @@ -128,7 +128,7 @@ def benchmark_view(view_type: str, num_iterations: int) -> tuple[dict[str, float num_prims = view.count view_name = "XformPrimView" else: # physx - physics_sim_view = SimulationManager.get_physics_sim_view() + physics_sim_view = PhysxManager.get_physics_sim_view() view = physics_sim_view.create_rigid_body_view(pattern) num_prims = view.count view_name = "PhysX RigidBodyView" diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index e791ce06b69..71ef003e5cc 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -15,7 +15,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import PhysxSchema, UsdPhysics import isaaclab.sim as sim_utils @@ -1475,7 +1475,7 @@ def write_spatial_tendon_properties_to_sim( def _initialize_impl(self): # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() if self.cfg.articulation_root_prim_path is not None: # The articulation root prim path is specified explicitly, so we can just use this. diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index ad5285d02be..75f3096e856 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -8,7 +8,7 @@ import weakref import omni.physics.tensors.impl.api as physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager import isaaclab.utils.math as math_utils from isaaclab.utils.buffers import TimestampedBuffer @@ -53,7 +53,7 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): self._sim_timestamp = 0.0 # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() gravity = self._physics_sim_view.get_gravity() # Convert to direction vector gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index 26aaf851c89..eb0a634e8a3 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -16,7 +16,7 @@ import omni.kit.app import omni.timeline -from isaaclab.sim import IsaacEvents, SimulationManager +from isaaclab.sim import IsaacEvents, PhysxManager import isaaclab.sim as sim_utils from isaaclab.sim.utils.stage import get_current_stage @@ -295,7 +295,7 @@ def safe_callback(callback_name, event, obj_ref): order=10, ) # register prim deletion callback - self._prim_deletion_callback_id = SimulationManager.register_callback( + self._prim_deletion_callback_id = PhysxManager.register_callback( lambda event, obj_ref=obj_ref: safe_callback("_on_prim_deletion", event, obj_ref), event=IsaacEvents.PRIM_DELETION, ) @@ -356,7 +356,7 @@ def _clear_callbacks(self, event: Any = None) -> None: event: Optional event that triggered the callback (unused but required for event handlers). """ if self._prim_deletion_callback_id: - SimulationManager.deregister_callback(self._prim_deletion_callback_id) + PhysxManager.deregister_callback(self._prim_deletion_callback_id) self._prim_deletion_callback_id = None if self._initialize_handle: self._initialize_handle.unsubscribe() diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py index 151db451ee2..5ff3cd9a8f4 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import PhysxSchema, UsdShade import isaaclab.sim as sim_utils @@ -266,7 +266,7 @@ def transform_nodal_pos( def _initialize_impl(self): # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() # obtain the first prim in the regex expression (all others are assumed to be a copy of this) template_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) if template_prim is None: diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py index 6e21517fe56..96d314f6fe0 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import UsdPhysics import isaaclab.sim as sim_utils @@ -458,7 +458,7 @@ def set_external_force_and_torque( def _initialize_impl(self): # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() # obtain the first prim in the regex expression (all others are assumed to be a copy of this) template_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) if template_prim is None: diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py index 74873065cf4..e92f0762c27 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py @@ -12,7 +12,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import UsdPhysics import isaaclab.sim as sim_utils @@ -594,7 +594,7 @@ def _initialize_impl(self): # clear object names list to prevent double counting on re-initialization self._object_names_list.clear() # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() root_prim_path_exprs = [] for name, rigid_object_cfg in self.cfg.rigid_objects.items(): # obtain the first prim in the regex expression (all others are assumed to be a copy of this) diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index d8fa1fac0b6..6947df280bb 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -20,7 +20,7 @@ import omni.kit.app import omni.physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from isaaclab.managers import EventManager from isaaclab.scene import InteractiveScene @@ -320,7 +320,7 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) self.sim.render() if self.cfg.wait_for_textures and self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors"): - while SimulationManager.assets_loading(): + while PhysxManager.assets_loading(): self.sim.render() # return observations diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 4b86181a009..5f905ca0c76 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -10,7 +10,7 @@ from typing import Any import omni.physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager from isaaclab.scene import InteractiveScene @@ -376,7 +376,7 @@ def reset( self.obs_buf = self.observation_manager.compute(update_history=True) if self.cfg.wait_for_textures and self.sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors"): - while SimulationManager.assets_loading(): + while PhysxManager.assets_loading(): self.sim.render() # return observations diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index b873c7df250..4d808934aa1 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -14,7 +14,7 @@ import carb import omni.physics.tensors.impl.api as physx -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import PhysxSchema import isaaclab.sim as sim_utils @@ -256,7 +256,7 @@ def compute_first_air(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: def _initialize_impl(self): super()._initialize_impl() # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() # check that only rigid bodies are selected leaf_pattern = self.cfg.prim_path.rsplit("/", 1)[-1] template_prim_path = self._parent_prims[0].GetPath().pathString diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index 2768c58924b..455345e56a9 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -11,7 +11,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import UsdPhysics import isaaclab.sim as sim_utils @@ -249,7 +249,7 @@ def _initialize_impl(self): body_names_regex = [tracked_prim_path.replace("env_0", "env_*") for tracked_prim_path in tracked_prim_paths] # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() # Create a prim view for all frames and initialize it # order of transforms coming out of view will be source frame followed by target frame(s) self._frame_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_regex) diff --git a/source/isaaclab/isaaclab/sensors/imu/imu.py b/source/isaaclab/isaaclab/sensors/imu/imu.py index a75683e80f1..09767d9f099 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu.py @@ -9,7 +9,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils @@ -135,7 +135,7 @@ def _initialize_impl(self): # Initialize parent class super()._initialize_impl() # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() # check if the prim at path is a rigid prim prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) if prim is None: diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 2d649b618a7..3de85177f5b 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -14,7 +14,7 @@ from typing import TYPE_CHECKING, ClassVar import omni -from isaaclab.sim.simulation_manager import SimulationManager +from isaaclab.sim._impl.physx_manager import PhysxManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils @@ -143,7 +143,7 @@ def _initialize_impl(self): super()._initialize_impl() # obtain global simulation view - self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._physics_sim_view = PhysxManager.get_physics_sim_view() prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) if prim is None: available_prims = ",".join([str(p.GetPath()) for p in sim_utils.get_current_stage().Traverse()]) diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 45c7ccdcee4..0d03e619910 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -22,7 +22,7 @@ import omni.kit.app import omni.timeline -from isaaclab.sim import IsaacEvents, SimulationManager +from isaaclab.sim import IsaacEvents, PhysxManager import isaaclab.sim as sim_utils from isaaclab.sim.utils.stage import get_current_stage @@ -284,7 +284,7 @@ def safe_callback(callback_name, event, obj_ref): order=10, ) # register prim deletion callback - self._prim_deletion_callback_id = SimulationManager.register_callback( + self._prim_deletion_callback_id = PhysxManager.register_callback( lambda event, obj_ref=obj_ref: safe_callback("_on_prim_deletion", event, obj_ref), event=IsaacEvents.PRIM_DELETION, ) @@ -333,7 +333,7 @@ def _on_prim_deletion(self, event) -> None: def _clear_callbacks(self) -> None: """Clears the callbacks.""" if self._prim_deletion_callback_id: - SimulationManager.deregister_callback(self._prim_deletion_callback_id) + PhysxManager.deregister_callback(self._prim_deletion_callback_id) self._prim_deletion_callback_id = None if self._initialize_handle: self._initialize_handle.unsubscribe() diff --git a/source/isaaclab/isaaclab/sim/__init__.py b/source/isaaclab/isaaclab/sim/__init__.py index ad741eba239..3130b187bb8 100644 --- a/source/isaaclab/isaaclab/sim/__init__.py +++ b/source/isaaclab/isaaclab/sim/__init__.py @@ -30,12 +30,12 @@ from .schemas import * # noqa: F401, F403 from .simulation_cfg import PhysxCfg, RenderCfg, SimulationCfg # noqa: F401, F403 from .simulation_context import SimulationContext, build_simulation_context # noqa: F401, F403 -from .simulation_manager import SimulationManager, IsaacEvents # noqa: F401, F403 +from ._impl.physx_manager import PhysxManager, IsaacEvents # noqa: F401, F403 from .spawners import * # noqa: F401, F403 from .utils import * # noqa: F401, F403 from .views import * # noqa: F401, F403 -# Monkey-patch Isaac Sim's SimulationManager to use Isaac Lab's implementation +# Monkey-patch Isaac Sim's PhysxManager to use Isaac Lab's implementation # This ensures all code (including Isaac Sim internals) uses our manager try: import isaacsim.core.simulation_manager as _isaacsim_sim_manager @@ -49,9 +49,9 @@ _OriginalSimManager.enable_all_default_callbacks(False) # Replace the class in both the module and impl module - _isaacsim_sim_manager.SimulationManager = SimulationManager + _isaacsim_sim_manager.PhysxManager = PhysxManager _isaacsim_sim_manager.IsaacEvents = IsaacEvents - _isaacsim_sim_manager_impl.SimulationManager = SimulationManager + _isaacsim_sim_manager_impl.SimulationManager = PhysxManager _isaacsim_sim_manager_impl.IsaacEvents = IsaacEvents except ImportError: pass # Isaac Sim extension not loaded yet, will be patched when available diff --git a/source/isaaclab/isaaclab/sim/_impl/__init__.py b/source/isaaclab/isaaclab/sim/_impl/__init__.py index d9f28e295c4..08267b819e2 100644 --- a/source/isaaclab/isaaclab/sim/_impl/__init__.py +++ b/source/isaaclab/isaaclab/sim/_impl/__init__.py @@ -8,9 +8,12 @@ # from .newton_backend import NewtonBackend from .physics_backend import PhysicsBackend from .physx_backend import PhysXBackend +from .physx_manager import PhysxManager, IsaacEvents __all__ = [ # "NewtonBackend", "PhysicsBackend", "PhysXBackend", + "PhysxManager", + "IsaacEvents", ] diff --git a/source/isaaclab/isaaclab/sim/_impl/physx_backend.py b/source/isaaclab/isaaclab/sim/_impl/physx_backend.py index 499080ce6e2..1a814ae085c 100644 --- a/source/isaaclab/isaaclab/sim/_impl/physx_backend.py +++ b/source/isaaclab/isaaclab/sim/_impl/physx_backend.py @@ -24,7 +24,7 @@ import isaaclab.sim as sim_utils from isaaclab.sim._impl.physics_backend import PhysicsBackend -from isaaclab.sim.simulation_manager import SimulationManager +from .physx_manager import PhysxManager if TYPE_CHECKING: from isaaclab.sim.simulation_context import SimulationContext @@ -217,7 +217,7 @@ def __init__(self, sim_context: "SimulationContext"): # Configure physics self._configure_simulation_dt() self._apply_physics_settings() - SimulationManager.initialize() + PhysxManager.initialize() # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes # when in headless mode @@ -244,7 +244,7 @@ def physics_dt(self) -> float: @property def physics_sim_view(self) -> "omni.physics.tensors.SimulationView": """Physics simulation view with torch backend.""" - return SimulationManager.get_physics_sim_view() + return PhysxManager.get_physics_sim_view() def is_fabric_enabled(self) -> bool: """Returns whether the fabric interface is enabled.""" @@ -258,22 +258,22 @@ def reset(self, soft: bool = False) -> None: """ if not soft: # initialize the physics simulation - if SimulationManager.get_physics_sim_view() is None: - SimulationManager.initialize_physics() + if PhysxManager.get_physics_sim_view() is None: + PhysxManager.initialize_physics() # app.update() may be changing the cuda device in reset, so we force it back to our desired device here if "cuda" in self._physics_device: torch.cuda.set_device(self._physics_device) # enable kinematic rendering with fabric - physics_sim_view = SimulationManager.get_physics_sim_view() + physics_sim_view = PhysxManager.get_physics_sim_view() if physics_sim_view is not None: physics_sim_view._backend.initialize_kinematic_bodies() def forward(self) -> None: """Update articulation kinematics and fabric for rendering.""" if self._fabric_iface is not None and self._update_fabric is not None: - physics_sim_view = SimulationManager.get_physics_sim_view() + physics_sim_view = PhysxManager.get_physics_sim_view() if physics_sim_view is not None and self._sim.is_playing(): # Update the articulations' link's poses before rendering physics_sim_view.update_articulations_kinematic() @@ -298,7 +298,7 @@ def step(self) -> None: def close(self) -> None: """Clean up physics resources.""" # clear the simulation manager state (notifies assets to cleanup) - SimulationManager.clear() + PhysxManager.clear() # detach the stage from physx if self._physx_sim_iface is not None: self._physx_sim_iface.detach_stage() @@ -396,8 +396,8 @@ def _apply_physics_settings(self): # Configure simulation manager backend # Isaac Lab always uses torch tensors for consistency, even on CPU - SimulationManager.set_backend("torch") - SimulationManager.set_physics_sim_device(self._physics_device) + PhysxManager.set_backend("torch") + PhysxManager.set_physics_sim_device(self._physics_device) # -------------------------- # USDPhysics API settings diff --git a/source/isaaclab/isaaclab/sim/simulation_manager.py b/source/isaaclab/isaaclab/sim/_impl/physx_manager.py similarity index 98% rename from source/isaaclab/isaaclab/sim/simulation_manager.py rename to source/isaaclab/isaaclab/sim/_impl/physx_manager.py index e980b478f14..2583f0a7a42 100644 --- a/source/isaaclab/isaaclab/sim/simulation_manager.py +++ b/source/isaaclab/isaaclab/sim/_impl/physx_manager.py @@ -23,7 +23,7 @@ import omni.usd from pxr import PhysxSchema -__all__ = ["IsaacEvents", "SimulationManager"] +__all__ = ["IsaacEvents", "PhysxManager"] class IsaacEvents(Enum): @@ -39,7 +39,7 @@ class IsaacEvents(Enum): TIMELINE_STOP = "isaac.timeline_stop" -class SimulationManager: +class PhysxManager: """Manages physics simulation lifecycle and callbacks. This is a class-level (singleton-like) manager for the simulation. @@ -69,7 +69,7 @@ class SimulationManager: _handles: dict = {} # Named internal handles # Compatibility stub for Isaac Sim code that calls _simulation_manager_interface - class _SimulationManagerInterfaceStub: + class _PhysxManagerInterfaceStub: """Minimal stub for Isaac Sim compatibility.""" @staticmethod @@ -109,7 +109,7 @@ def is_simulating() -> bool: get_sample_range = staticmethod(lambda: None) log_statistics = staticmethod(lambda: None) - _simulation_manager_interface = _SimulationManagerInterfaceStub() + _simulation_manager_interface = _PhysxManagerInterfaceStub() # ------------------------------------------------------------------ # Public API diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 16e00347e41..287232e49ad 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -170,7 +170,7 @@ def clear_instance(cls) -> None: return # close visualizer (unsubscribes stop handle) cls._instance._visualizer.close() - # close physics interface (clears SimulationManager, detaches physx stage) + # close physics interface (clears PhysxManager, detaches physx stage) cls._instance._physics_interface.close() # detach the stage from the USD stage cache stage_cache = UsdUtils.StageCache.Get() diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index bae5735c794..f805e474670 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -435,7 +435,7 @@ def attach_stage_to_usd_context(attaching_early: bool = False): import carb import omni.physx import omni.usd - from isaaclab.sim.simulation_manager import SimulationManager + from isaaclab.sim._impl.physx_manager import PhysxManager from isaaclab.sim.simulation_context import SimulationContext @@ -468,7 +468,7 @@ def attach_stage_to_usd_context(attaching_early: bool = False): ) # disable stage open callback to avoid clearing callbacks - SimulationManager.enable_stage_open_callback(False) + PhysxManager.enable_stage_open_callback(False) # enable physics fabric SimulationContext.instance().carb_settings.set_bool("/physics/fabricEnabled", True) @@ -481,4 +481,4 @@ def attach_stage_to_usd_context(attaching_early: bool = False): physx_sim_interface.attach_stage(stage_id) # re-enable stage open callback - SimulationManager.enable_stage_open_callback(True) + PhysxManager.enable_stage_open_callback(True) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 504469daff1..c070279dd9c 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -18,7 +18,7 @@ import weakref import omni.timeline -from isaaclab.sim import IsaacEvents, SimulationManager +from isaaclab.sim import IsaacEvents, PhysxManager import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -764,7 +764,7 @@ def on_event(event): callback_state["called"] = True # register callback for the event - callback_id = SimulationManager.register_callback(lambda event: on_event(event), event=event_type) + callback_id = PhysxManager.register_callback(lambda event: on_event(event), event=event_type) try: # verify callback hasn't been called yet @@ -779,7 +779,7 @@ def on_event(event): finally: # cleanup callback if callback_id is not None: - SimulationManager.deregister_callback(callback_id) + PhysxManager.deregister_callback(callback_id) @pytest.mark.isaacsim_ci @@ -804,7 +804,7 @@ def on_prim_deletion(event): callback_state["deleted_path"] = event.payload.get("prim_path") # register callback for PRIM_DELETION event - callback_id = SimulationManager.register_callback( + callback_id = PhysxManager.register_callback( lambda event: on_prim_deletion(event), event=IsaacEvents.PRIM_DELETION ) @@ -816,7 +816,7 @@ def on_prim_deletion(event): 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 + PhysxManager._message_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": "/World/Cube"}) # type: ignore # verify callback was triggered assert callback_state["prim_deleted"] @@ -824,7 +824,7 @@ def on_prim_deletion(event): finally: # cleanup callback if callback_id is not None: - SimulationManager.deregister_callback(callback_id) + PhysxManager.deregister_callback(callback_id) @pytest.mark.isaacsim_ci @@ -840,7 +840,7 @@ def on_timeline_stop(event): callback_state["timeline_stop_called"] = True # register callback for TIMELINE_STOP event - callback_id = SimulationManager.register_callback( + callback_id = PhysxManager.register_callback( lambda event: on_timeline_stop(event), event=IsaacEvents.TIMELINE_STOP ) @@ -858,7 +858,7 @@ def on_timeline_stop(event): sim.stop() # dispatch the event manually - SimulationManager._message_bus.dispatch_event(IsaacEvents.TIMELINE_STOP.value, payload={}) # type: ignore + PhysxManager._message_bus.dispatch_event(IsaacEvents.TIMELINE_STOP.value, payload={}) # type: ignore # verify callback was triggered assert callback_state["timeline_stop_called"] @@ -866,7 +866,7 @@ def on_timeline_stop(event): finally: # cleanup callback if callback_id is not None: - SimulationManager.deregister_callback(callback_id) + PhysxManager.deregister_callback(callback_id) @pytest.mark.isaacsim_ci @@ -907,11 +907,11 @@ def safe_callback(callback_name, event, obj_ref): # register callbacks with weakref obj_ref = weakref.ref(tracker) - warmup_id = SimulationManager.register_callback( + warmup_id = PhysxManager.register_callback( lambda event, obj_ref=obj_ref: safe_callback("on_warmup", event, obj_ref), event=IsaacEvents.PHYSICS_WARMUP, ) - ready_id = SimulationManager.register_callback( + ready_id = PhysxManager.register_callback( lambda event, obj_ref=obj_ref: safe_callback("on_ready", event, obj_ref), event=IsaacEvents.PHYSICS_READY ) @@ -938,9 +938,9 @@ def safe_callback(callback_name, event, obj_ref): finally: # cleanup callbacks if warmup_id is not None: - SimulationManager.deregister_callback(warmup_id) + PhysxManager.deregister_callback(warmup_id) if ready_id is not None: - SimulationManager.deregister_callback(ready_id) + PhysxManager.deregister_callback(ready_id) @pytest.mark.isaacsim_ci @@ -966,9 +966,9 @@ 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) + id1 = PhysxManager.register_callback(lambda event: callback1(event), event=IsaacEvents.PHYSICS_READY) + id2 = PhysxManager.register_callback(lambda event: callback2(event), event=IsaacEvents.PHYSICS_READY) + id3 = PhysxManager.register_callback(lambda event: callback3(event), event=IsaacEvents.PHYSICS_READY) try: # verify none have been called @@ -985,11 +985,11 @@ def callback3(event): finally: # cleanup all callbacks if id1 is not None: - SimulationManager.deregister_callback(id1) + PhysxManager.deregister_callback(id1) if id2 is not None: - SimulationManager.deregister_callback(id2) + PhysxManager.deregister_callback(id2) if id3 is not None: - SimulationManager.deregister_callback(id3) + PhysxManager.deregister_callback(id3) """ @@ -1016,7 +1016,7 @@ def on_physics_ready_with_exception(event): builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore # register callback that will raise an exception - callback_id = SimulationManager.register_callback( + callback_id = PhysxManager.register_callback( lambda event: on_physics_ready_with_exception(event), event=IsaacEvents.PHYSICS_READY ) @@ -1031,7 +1031,7 @@ def on_physics_ready_with_exception(event): finally: # cleanup callback if callback_id is not None: - SimulationManager.deregister_callback(callback_id) + PhysxManager.deregister_callback(callback_id) # ensure exception is cleared builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore From 4c238f5ad767675c16f1a47c71a83b4dd39d25c4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 20:50:05 -0800 Subject: [PATCH 114/130] refactor step 10: refactored ov_visualizer from visualizer_interfaces --- source/isaaclab/isaaclab/sim/ov_visualizer.py | 543 ++++++++++++++++++ .../isaaclab/sim/simulation_context.py | 4 - .../isaaclab/sim/visualizer_interface.py | 497 +++------------- 3 files changed, 609 insertions(+), 435 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/ov_visualizer.py diff --git a/source/isaaclab/isaaclab/sim/ov_visualizer.py b/source/isaaclab/isaaclab/sim/ov_visualizer.py new file mode 100644 index 00000000000..3b770331462 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/ov_visualizer.py @@ -0,0 +1,543 @@ +# 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 + +"""Omniverse visualizer helper for PhysX-based SimulationContext.""" + +from __future__ import annotations + +import enum +import flatdict +import logging +import os +import toml +import torch +import weakref +from typing import TYPE_CHECKING + +import omni.kit.app +import omni.timeline +from isaacsim.core.utils.viewports import set_camera_view + +from isaaclab.utils.version import get_isaac_sim_version + +if TYPE_CHECKING: + from .simulation_context import SimulationContext + +logger = logging.getLogger(__name__) + + +class RenderMode(enum.IntEnum): + """Different rendering modes for the simulation. + + Render modes correspond to how the viewport and other UI elements (such as listeners to keyboard or mouse + events) are updated. There are three main components that can be updated when the simulation is rendered: + + 1. **UI elements and other extensions**: These are UI elements (such as buttons, sliders, etc.) and other + extensions that are running in the background that need to be updated when the simulation is running. + 2. **Cameras**: These are typically based on Hydra textures and are used to render the scene from different + viewpoints. They can be attached to a viewport or be used independently to render the scene. + 3. **Viewports**: These are windows where you can see the rendered scene. + + Updating each of the above components has a different overhead. For example, updating the viewports is + computationally expensive compared to updating the UI elements. Therefore, it is useful to be able to + control what is updated when the simulation is rendered. This is where the render mode comes in. There are + four different render modes: + + * :attr:`NO_GUI_OR_RENDERING`: The simulation is running without a GUI and off-screen rendering flag is disabled, + so none of the above are updated. + * :attr:`NO_RENDERING`: No rendering, where only 1 is updated at a lower rate. + * :attr:`PARTIAL_RENDERING`: Partial rendering, where only 1 and 2 are updated. + * :attr:`FULL_RENDERING`: Full rendering, where everything (1, 2, 3) is updated. + + .. _Viewports: https://docs.omniverse.nvidia.com/extensions/latest/ext_viewport.html + """ + + NO_GUI_OR_RENDERING = -1 + """The simulation is running without a GUI and off-screen rendering is disabled.""" + NO_RENDERING = 0 + """No rendering, where only other UI elements are updated at a lower rate.""" + PARTIAL_RENDERING = 1 + """Partial rendering, where the simulation cameras and UI elements are updated.""" + FULL_RENDERING = 2 + """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" + + +class OVVisualizer: + """Omniverse visualizer helper managing viewport/rendering for PhysX workflow. + + This class handles: + - Viewport context and window management + - Render mode switching + - Camera view setup + - Render settings from configuration + - Throttled rendering for UI responsiveness + - Timeline control (play/pause/stop) + """ + + # Expose RenderMode for backwards compatibility + RenderMode = RenderMode + + def __init__(self, sim_context: "SimulationContext"): + """Initialize OV visualizer. + + Args: + sim_context: Parent simulation context. + """ + self._sim = sim_context + + # Acquire application interface + self._app_iface = omni.kit.app.get_app_interface() + + # Detect render flags + self._local_gui = self._sim.carb_settings.get("/app/window/enabled") + self._livestream_gui = self._sim.carb_settings.get("/app/livestream/enabled") + self._xr_gui = self._sim.carb_settings.get("/app/xr/enabled") + self._offscreen_render = bool(self._sim.carb_settings.get("/isaaclab/render/offscreen")) + self._render_viewport = bool(self._sim.carb_settings.get("/isaaclab/render/active_viewport")) + + # Flag for whether any GUI will be rendered (local, livestreamed or viewport) + has_gui = self._local_gui or self._livestream_gui or self._xr_gui + self._sim.carb_settings.set_bool("/isaaclab/has_gui", has_gui) + + # Apply render settings from config + self._apply_render_settings_from_cfg() + + # Initialize viewport state + self._viewport_context = None + self._viewport_window = None + self._render_throttle_counter = 0 + self._render_throttle_period = 5 + + # Store the default render mode + if not self._sim.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: + # set default render mode + # note: this is the terminal state: cannot exit from this render mode + self.render_mode = RenderMode.NO_GUI_OR_RENDERING + elif not self._sim.carb_settings.get("/isaaclab/has_gui") and self._offscreen_render: + # set default render mode + # note: this is the terminal state: cannot exit from this render mode + self.render_mode = RenderMode.PARTIAL_RENDERING + else: + # note: need to import here in case the UI is not available (ex. headless mode) + import omni.ui as ui + from omni.kit.viewport.utility import get_active_viewport + + # set default render mode + # note: this can be changed by calling the `set_render_mode` function + self.render_mode = RenderMode.FULL_RENDERING + # acquire viewport context + self._viewport_context = get_active_viewport() + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + # acquire viewport window + self._viewport_window = ui.Workspace.get_window("Viewport") + + # Check the case where we don't need to render the viewport + # since render_viewport can only be False in headless mode, we only need to check for offscreen_render + if not self._render_viewport and self._offscreen_render: + # disable the viewport if offscreen_render is enabled + from omni.kit.viewport.utility import get_active_viewport + + get_active_viewport().updates_enabled = False + + # Override enable scene querying if rendering is enabled + # this is needed for some GUI features + if self._sim.carb_settings.get("/isaaclab/has_gui"): + self._sim.cfg.enable_scene_query_support = True + + # Acquire timeline interface + self._timeline_iface = omni.timeline.get_timeline_interface() + + # Set timeline auto update to True + self._timeline_iface.set_auto_update(True) + + # Configure rendering/timeline rate + self._configure_rendering_dt() + + # Add callback to deal the simulation app when simulation is stopped. + # this is needed because physics views go invalid once we stop the simulation + timeline_event_stream = self._timeline_iface.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 + + # ------------------------------------------------------------------ + # Properties + # ------------------------------------------------------------------ + + @property + def has_gui(self) -> bool: + """Whether any GUI is available (local, livestreamed, or XR).""" + return bool(self._sim.carb_settings.get("/isaaclab/has_gui")) + + @property + def app(self) -> omni.kit.app.IApp: + """Omniverse Kit Application interface.""" + return self._app_iface + + @property + def offscreen_render(self) -> bool: + """Whether offscreen rendering is enabled.""" + return self._offscreen_render + + @property + def render_viewport(self) -> bool: + """Whether the default viewport should be rendered.""" + return self._render_viewport + + # ------------------------------------------------------------------ + # Timeline Control + # ------------------------------------------------------------------ + + def is_playing(self) -> bool: + """Check whether the simulation is playing.""" + return self._timeline_iface.is_playing() + + def is_stopped(self) -> bool: + """Check whether the simulation is stopped.""" + return self._timeline_iface.is_stopped() + + def play(self) -> None: + """Start playing the simulation.""" + self._timeline_iface.play() + self._timeline_iface.commit() + # perform one step to propagate all physics handles properly + self._sim.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self._sim.set_setting("/app/player/playSimulations", True) + + def pause(self) -> None: + """Pause the simulation.""" + self._timeline_iface.pause() + self._timeline_iface.commit() + + def stop(self) -> None: + """Stop the simulation.""" + self._timeline_iface.stop() + self._timeline_iface.commit() + # perform one step to propagate all physics handles properly + self._sim.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self._sim.set_setting("/app/player/playSimulations", True) + + # ------------------------------------------------------------------ + # Render Mode + # ------------------------------------------------------------------ + + def set_render_mode(self, mode: RenderMode): + """Change the current render mode of the simulation. + + Please see :class:`RenderMode` for more information on the different render modes. + + .. note:: + When no GUI is available (locally or livestreamed), we do not need to choose whether the viewport + needs to render or not (since there is no GUI). Thus, in this case, calling the function will not + change the render mode. + + Args: + mode: The rendering mode. If different than current rendering mode, + the mode is changed to the new mode. + + Raises: + ValueError: If the input mode is not supported. + """ + # check if mode change is possible -- not possible when no GUI is available + if not self._sim.carb_settings.get("/isaaclab/has_gui"): + self._sim.logger.warning( + f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." + ) + return + # check if there is a mode change + # note: this is mostly needed for GUI when we want to switch between full rendering and no rendering. + if mode != self.render_mode: + if mode == RenderMode.FULL_RENDERING: + # display the viewport and enable updates + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = True # pyright: ignore [reportOptionalMemberAccess] + elif mode == RenderMode.PARTIAL_RENDERING: + # hide the viewport and disable updates + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + elif mode == RenderMode.NO_RENDERING: + # hide the viewport and disable updates + if self._viewport_context is not None: + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + # reset the throttle counter + self._render_throttle_counter = 0 + else: + raise ValueError(f"Unsupported render mode: {mode}! Please check `RenderMode` for details.") + # update render mode + self.render_mode = mode + + # ------------------------------------------------------------------ + # Camera + # ------------------------------------------------------------------ + + def set_camera_view( + self, + eye: tuple[float, float, float], + target: tuple[float, float, float], + camera_prim_path: str = "/OmniverseKit_Persp", + ): + """Set the location and target of the viewport camera in the stage. + + Note: + This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. + It is provided here for convenience to reduce the amount of imports needed. + + Args: + eye: The location of the camera eye. + target: The location of the camera target. + camera_prim_path: The path to the camera primitive in the stage. Defaults to + "/OmniverseKit_Persp". + """ + # safe call only if we have a GUI or viewport rendering enabled + if self._sim.carb_settings.get("/isaaclab/has_gui") or self._offscreen_render or self._render_viewport: + set_camera_view(eye, target, camera_prim_path) + + # ------------------------------------------------------------------ + # Rendering + # ------------------------------------------------------------------ + + def render(self, mode: RenderMode | None = None): + """Refreshes the rendering components including UI elements and view-ports depending on the render mode. + + This function is used to refresh the rendering components of the simulation. This includes updating the + view-ports, UI elements, and other extensions (besides physics simulation) that are running in the + background. The rendering components are refreshed based on the render mode. + + Please see :class:`RenderMode` for more information on the different render modes. + + Args: + mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. + """ + # check if we need to change the render mode + if mode is not None: + self.set_render_mode(mode) + # render based on the render mode + if self.render_mode == RenderMode.NO_GUI_OR_RENDERING: + # we never want to render anything here (this is for complete headless mode) + pass + elif self.render_mode == RenderMode.NO_RENDERING: + # throttle the rendering frequency to keep the UI responsive + self._render_throttle_counter += 1 + 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 + self._sim.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self._sim.set_setting("/app/player/playSimulations", True) + else: + # manually flush the fabric data to update Hydra textures + self._sim.forward() + # render the simulation + # 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._sim.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self._sim.set_setting("/app/player/playSimulations", True) + + # app.update() may be changing the cuda device, so we force it back to our desired device here + if "cuda" in self._sim.device: + torch.cuda.set_device(self._sim.device) + + # ------------------------------------------------------------------ + # Lifecycle + # ------------------------------------------------------------------ + + def reset(self, soft: bool = False) -> None: + """Reset visualizer (timeline control + warmup renders on hard reset). + + Args: + soft: If True, skip timeline reset and warmup. + """ + if not soft: + # disable app control on stop handle during reset + self.set_stop_handle_enabled(False) + if not self.is_stopped(): + self.stop() + self.set_stop_handle_enabled(True) + # play the simulation + self.play() + # warmup renders to initialize replicator buffers + for _ in range(2): + self.render() + + def set_stop_handle_enabled(self, enabled: bool) -> None: + """Enable or disable the app control on stop handle. + + Args: + enabled: If True, the stop handle callback is active. + """ + self._disable_app_control_on_stop_handle = not enabled + + def close(self) -> None: + """Clean up visualizer resources.""" + # Unsubscribe from stop handle + if self._app_control_on_stop_handle is not None: + self._app_control_on_stop_handle.unsubscribe() + self._app_control_on_stop_handle = None + + # ------------------------------------------------------------------ + # Private Methods + # ------------------------------------------------------------------ + + 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 + resume the simulation from the last state. This leaves the app in an inconsistent state, where + two possible actions can be taken: + + 1. **Keep the app rendering**: In this case, the simulation is kept running and the app is not shutdown. + However, the physics is not updated and the script cannot be resumed from the last state. The + user has to manually close the app to stop the simulation. + 2. **Shutdown the app**: This is the default behavior. In this case, the app is shutdown and + the simulation is stopped. + + Note: + 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. + """ + pass + # if not self._disable_app_control_on_stop_handle: + # while not omni.timeline.get_timeline_interface().is_playing(): + # self.render() + + def _apply_render_settings_from_cfg(self): # noqa: C901 + """Sets rtx settings specified in the RenderCfg.""" + + # define mapping of user-friendly RenderCfg names to native carb names + rendering_setting_name_mapping = { + "enable_translucency": "/rtx/translucency/enabled", + "enable_reflections": "/rtx/reflections/enabled", + "enable_global_illumination": "/rtx/indirectDiffuse/enabled", + "enable_dlssg": "/rtx-transient/dlssg/enabled", + "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", + "dlss_mode": "/rtx/post/dlss/execMode", + "enable_direct_lighting": "/rtx/directLighting/enabled", + "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", + "enable_shadows": "/rtx/shadows/enabled", + "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", + "dome_light_upper_lower_strategy": "/rtx/domeLight/upperLowerStrategy", + } + + not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] + + # grab the rendering mode using the following priority: + # 1. command line argument --rendering_mode, if provided + # 2. rendering_mode from Render Config, if set + # 3. lastly, default to "balanced" mode, if neither is specified + rendering_mode = self._sim.carb_settings.get("/isaaclab/rendering/rendering_mode") + if not rendering_mode: + rendering_mode = self._sim.cfg.render.rendering_mode + if not rendering_mode: + rendering_mode = "balanced" + + # set preset settings (same behavior as the CLI arg --rendering_mode) + if rendering_mode is not None: + # check if preset is supported + supported_rendering_modes = ["performance", "balanced", "quality"] + if rendering_mode not in supported_rendering_modes: + raise ValueError( + f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." + ) + + # grab isaac lab apps path + isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") + # for Isaac Sim 4.5 compatibility, we use the 4.5 rendering mode app files in a different folder + if get_isaac_sim_version().major < 5: + isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") + + # grab preset settings + preset_filename = os.path.join(isaaclab_app_exp_path, f"rendering_modes/{rendering_mode}.kit") + with open(preset_filename) as file: + preset_dict = toml.load(file) + preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) + + # set presets + for key, value in preset_dict.items(): + key = "/" + key.replace(".", "/") # convert to carb setting format + self._sim.set_setting(key, value) + + # set user-friendly named settings + for key, value in vars(self._sim.cfg.render).items(): + if value is None or key in not_carb_settings: + # skip unset settings and non-carb settings + continue + if key not in rendering_setting_name_mapping: + raise ValueError( + f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" + " need to be updated." + ) + key = rendering_setting_name_mapping[key] + self._sim.set_setting(key, value) + + # set general carb settings + carb_settings = self._sim.cfg.render.carb_settings + if carb_settings is not None: + for key, value in carb_settings.items(): + if "_" in key: + key = "/" + key.replace("_", "/") # convert from python variable style string + elif "." in key: + key = "/" + key.replace(".", "/") # convert from .kit file style string + if self._sim.get_setting(key) is None: + raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") + self._sim.set_setting(key, value) + + # set denoiser mode + if self._sim.cfg.render.antialiasing_mode is not None: + try: + import omni.replicator.core as rep + + rep.settings.set_render_rtx_realtime(antialiasing=self._sim.cfg.render.antialiasing_mode) + except Exception: + pass + + # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. + if self._sim.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": + self._sim.carb_settings.set_string("/rtx/rendermode", "RaytracedLighting") + + def _configure_rendering_dt(self): + """Configures the rendering/timeline rate based on physics dt and render interval.""" + from pxr import Usd + + cfg = self._sim.cfg + stage = self._sim.stage + carb_settings = self._sim.carb_settings + + # compute rendering frequency + render_interval = max(cfg.render_interval, 1) + rendering_hz = int(1.0 / (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 carb_settings.get("/app/runLoops/main/rateLimitEnabled"): + carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) + + # set stage time codes per second + with Usd.EditContext(stage, stage.GetRootLayer()): + 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(cfg.dt * render_interval) + _loop_runner.set_manual_mode(True) + except Exception: + self._sim.logger.warning( + "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" + ) + carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) + carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 287232e49ad..d4985e9294c 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -460,15 +460,11 @@ 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._visualizer.set_stop_handle_enabled(False) # check if we need to raise an exception that was raised in a callback if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # 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._visualizer.set_stop_handle_enabled(True) - @contextmanager def build_simulation_context( diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index abe2d6b1c61..60b746c3918 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -7,72 +7,23 @@ from __future__ import annotations -import enum -import flatdict -import logging -import os -import toml -import torch -import weakref from typing import TYPE_CHECKING -import omni.kit.app -import omni.timeline -from isaacsim.core.utils.viewports import set_camera_view - -from isaaclab.utils.version import get_isaac_sim_version +from .interface import Interface +from .ov_visualizer import OVVisualizer, RenderMode if TYPE_CHECKING: from .simulation_context import SimulationContext -logger = logging.getLogger(__name__) - - -class RenderMode(enum.IntEnum): - """Different rendering modes for the simulation. - - Render modes correspond to how the viewport and other UI elements (such as listeners to keyboard or mouse - events) are updated. There are three main components that can be updated when the simulation is rendered: - - 1. **UI elements and other extensions**: These are UI elements (such as buttons, sliders, etc.) and other - extensions that are running in the background that need to be updated when the simulation is running. - 2. **Cameras**: These are typically based on Hydra textures and are used to render the scene from different - viewpoints. They can be attached to a viewport or be used independently to render the scene. - 3. **Viewports**: These are windows where you can see the rendered scene. - - Updating each of the above components has a different overhead. For example, updating the viewports is - computationally expensive compared to updating the UI elements. Therefore, it is useful to be able to - control what is updated when the simulation is rendered. This is where the render mode comes in. There are - four different render modes: +# Re-export RenderMode for backwards compatibility +__all__ = ["RenderMode", "VisualizerInterface"] - * :attr:`NO_GUI_OR_RENDERING`: The simulation is running without a GUI and off-screen rendering flag is disabled, - so none of the above are updated. - * :attr:`NO_RENDERING`: No rendering, where only 1 is updated at a lower rate. - * :attr:`PARTIAL_RENDERING`: Partial rendering, where only 1 and 2 are updated. - * :attr:`FULL_RENDERING`: Full rendering, where everything (1, 2, 3) is updated. - .. _Viewports: https://docs.omniverse.nvidia.com/extensions/latest/ext_viewport.html - """ - - NO_GUI_OR_RENDERING = -1 - """The simulation is running without a GUI and off-screen rendering is disabled.""" - NO_RENDERING = 0 - """No rendering, where only other UI elements are updated at a lower rate.""" - PARTIAL_RENDERING = 1 - """Partial rendering, where the simulation cameras and UI elements are updated.""" - FULL_RENDERING = 2 - """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" - - -class VisualizerInterface: +class VisualizerInterface(Interface): """Manages viewport/rendering for SimulationContext (Omniverse workflow). - This class handles: - - Viewport context and window management - - Render mode switching - - Camera view setup - - Render settings from configuration - - Throttled rendering for UI responsiveness + This interface delegates rendering operations to an internal OVVisualizer helper, + while managing the lifecycle integration with SimulationContext. """ # Expose RenderMode as class attribute for backwards compatibility @@ -84,182 +35,81 @@ def __init__(self, sim_context: "SimulationContext"): Args: sim_context: Parent simulation context. """ - self._sim = sim_context - - # acquire application interface - self._app_iface = omni.kit.app.get_app_interface() - - # Detect render flags - self._local_gui = self._sim.carb_settings.get("/app/window/enabled") - self._livestream_gui = self._sim.carb_settings.get("/app/livestream/enabled") - self._xr_gui = self._sim.carb_settings.get("/app/xr/enabled") - self._offscreen_render = bool(self._sim.carb_settings.get("/isaaclab/render/offscreen")) - self._render_viewport = bool(self._sim.carb_settings.get("/isaaclab/render/active_viewport")) - - # Flag for whether any GUI will be rendered (local, livestreamed or viewport) - has_gui = self._local_gui or self._livestream_gui or self._xr_gui - self._sim.carb_settings.set_bool("/isaaclab/has_gui", has_gui) - - # Apply render settings from config - self._apply_render_settings_from_cfg() - - # Initialize viewport state - self._viewport_context = None - self._viewport_window = None - self._render_throttle_counter = 0 - self._render_throttle_period = 5 - - # Store the default render mode - if not self._sim.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: - # set default render mode - # note: this is the terminal state: cannot exit from this render mode - self.render_mode = RenderMode.NO_GUI_OR_RENDERING - elif not self._sim.carb_settings.get("/isaaclab/has_gui") and self._offscreen_render: - # set default render mode - # note: this is the terminal state: cannot exit from this render mode - self.render_mode = RenderMode.PARTIAL_RENDERING - else: - # note: need to import here in case the UI is not available (ex. headless mode) - import omni.ui as ui - from omni.kit.viewport.utility import get_active_viewport - - # set default render mode - # note: this can be changed by calling the `set_render_mode` function - self.render_mode = RenderMode.FULL_RENDERING - # acquire viewport context - self._viewport_context = get_active_viewport() - self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] - # acquire viewport window - self._viewport_window = ui.Workspace.get_window("Viewport") - - # Check the case where we don't need to render the viewport - # since render_viewport can only be False in headless mode, we only need to check for offscreen_render - if not self._render_viewport and self._offscreen_render: - # disable the viewport if offscreen_render is enabled - from omni.kit.viewport.utility import get_active_viewport - - get_active_viewport().updates_enabled = False - - # Override enable scene querying if rendering is enabled - # this is needed for some GUI features - if self._sim.carb_settings.get("/isaaclab/has_gui"): - self._sim.cfg.enable_scene_query_support = True - - # acquire timeline interface - self._timeline_iface = omni.timeline.get_timeline_interface() - - # set timeline auto update to True - self._timeline_iface.set_auto_update(True) - - # configure rendering/timeline rate - self._configure_rendering_dt() - - # add callback to deal the simulation app when simulation is stopped. - # this is needed because physics views go invalid once we stop the simulation - timeline_event_stream = self._timeline_iface.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 + super().__init__(sim_context) - @property - def has_gui(self) -> bool: - """Whether any GUI is available (local, livestreamed, or XR).""" - return bool(self._sim.carb_settings.get("/isaaclab/has_gui")) + # Create OV visualizer helper + self._ov_visualizer = OVVisualizer(sim_context) + # ------------------------------------------------------------------ + # Properties (delegate to OVVisualizer) + # ------------------------------------------------------------------ @property - def app(self) -> omni.kit.app.IApp: + def app(self): """Omniverse Kit Application interface.""" - return self._app_iface + return self._ov_visualizer.app + + @property + def offscreen_render(self) -> bool: + """Whether offscreen rendering is enabled.""" + return self._ov_visualizer.offscreen_render + + @property + def render_viewport(self) -> bool: + """Whether the default viewport should be rendered.""" + return self._ov_visualizer.render_viewport + + @property + def render_mode(self) -> RenderMode: + """Current render mode.""" + return self._ov_visualizer.render_mode + + @render_mode.setter + def render_mode(self, value: RenderMode) -> None: + """Set render mode.""" + self._ov_visualizer.render_mode = value + + # ------------------------------------------------------------------ + # Timeline Control (delegate to OVVisualizer) + # ------------------------------------------------------------------ def is_playing(self) -> bool: """Check whether the simulation is playing.""" - return self._timeline_iface.is_playing() + return self._ov_visualizer.is_playing() def is_stopped(self) -> bool: """Check whether the simulation is stopped.""" - return self._timeline_iface.is_stopped() + return self._ov_visualizer.is_stopped() def play(self) -> None: """Start playing the simulation.""" - self._timeline_iface.play() - self._timeline_iface.commit() - # perform one step to propagate all physics handles properly - self._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) + self._ov_visualizer.play() def pause(self) -> None: """Pause the simulation.""" - self._timeline_iface.pause() - self._timeline_iface.commit() + self._ov_visualizer.pause() def stop(self) -> None: """Stop the simulation.""" - self._timeline_iface.stop() - self._timeline_iface.commit() - # perform one step to propagate all physics handles properly - self._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) - - @property - def offscreen_render(self) -> bool: - """Whether offscreen rendering is enabled.""" - return self._offscreen_render + self._ov_visualizer.stop() - @property - def render_viewport(self) -> bool: - """Whether the default viewport should be rendered.""" - return self._render_viewport + # ------------------------------------------------------------------ + # Render Mode (delegate to OVVisualizer) + # ------------------------------------------------------------------ def set_render_mode(self, mode: RenderMode): """Change the current render mode of the simulation. Please see :class:`RenderMode` for more information on the different render modes. - .. note:: - When no GUI is available (locally or livestreamed), we do not need to choose whether the viewport - needs to render or not (since there is no GUI). Thus, in this case, calling the function will not - change the render mode. - Args: mode: The rendering mode. If different than current rendering mode, the mode is changed to the new mode. - - Raises: - ValueError: If the input mode is not supported. """ - # check if mode change is possible -- not possible when no GUI is available - if not self._sim.carb_settings.get("/isaaclab/has_gui"): - self._sim.logger.warning( - f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." - ) - return - # check if there is a mode change - # note: this is mostly needed for GUI when we want to switch between full rendering and no rendering. - if mode != self.render_mode: - if mode == RenderMode.FULL_RENDERING: - # display the viewport and enable updates - self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] - self._viewport_window.visible = True # pyright: ignore [reportOptionalMemberAccess] - elif mode == RenderMode.PARTIAL_RENDERING: - # hide the viewport and disable updates - self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] - self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] - elif mode == RenderMode.NO_RENDERING: - # hide the viewport and disable updates - if self._viewport_context is not None: - self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] - self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] - # reset the throttle counter - self._render_throttle_counter = 0 - else: - raise ValueError(f"Unsupported render mode: {mode}! Please check `RenderMode` for details.") - # update render mode - self.render_mode = mode + self._ov_visualizer.set_render_mode(mode) + + # ------------------------------------------------------------------ + # Camera (delegate to OVVisualizer) + # ------------------------------------------------------------------ def set_camera_view( self, @@ -269,19 +119,17 @@ def set_camera_view( ): """Set the location and target of the viewport camera in the stage. - Note: - This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. - It is provided here for convenience to reduce the amount of imports needed. - Args: eye: The location of the camera eye. target: The location of the camera target. camera_prim_path: The path to the camera primitive in the stage. Defaults to "/OmniverseKit_Persp". """ - # safe call only if we have a GUI or viewport rendering enabled - if self._sim.carb_settings.get("/isaaclab/has_gui") or self._offscreen_render or self._render_viewport: - set_camera_view(eye, target, camera_prim_path) + self._ov_visualizer.set_camera_view(eye, target, camera_prim_path) + + # ------------------------------------------------------------------ + # Interface Methods + # ------------------------------------------------------------------ def render(self, mode: RenderMode | None = None): """Refreshes the rendering components including UI elements and view-ports depending on the render mode. @@ -295,35 +143,7 @@ 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 change the render mode - if mode is not None: - self.set_render_mode(mode) - # render based on the render mode - if self.render_mode == RenderMode.NO_GUI_OR_RENDERING: - # we never want to render anything here (this is for complete headless mode) - pass - elif self.render_mode == RenderMode.NO_RENDERING: - # throttle the rendering frequency to keep the UI responsive - self._render_throttle_counter += 1 - 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 - self._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) - else: - # manually flush the fabric data to update Hydra textures - self._sim.forward() - # render the simulation - # 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._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) - - # app.update() may be changing the cuda device, so we force it back to our desired device here - if "cuda" in self._sim.device: - torch.cuda.set_device(self._sim.device) + self._ov_visualizer.render(mode) def reset(self, soft: bool = False) -> None: """Reset visualizer (timeline control + warmup renders on hard reset). @@ -331,212 +151,27 @@ def reset(self, soft: bool = False) -> None: Args: soft: If True, skip timeline reset and warmup. """ - if not soft: - # disable app control on stop handle - self.set_stop_handle_enabled(False) - if not self.is_stopped(): - self.stop() - self.set_stop_handle_enabled(True) - # play the simulation - self.play() - # warmup renders to initialize replicator buffers - for _ in range(2): - self.render() + self._ov_visualizer.reset(soft) def forward(self) -> None: """No-op for visualizer (rendering happens in render()).""" pass - def step(self, render: bool = True) -> bool: - """Handle pause loop and prepare for physics step. - - This method blocks while the timeline is paused, keeping the UI responsive. - It returns whether physics should proceed (True if playing, False if stopped). + def step(self, render: bool = True) -> None: + """Step visualizers and optionally render. Args: - render: Whether to render while waiting. - - Returns: - True if physics should step (timeline is playing), False if stopped. + render: Whether to render after stepping. """ # if paused, keep rendering until playing or stopped - was_paused = not self.is_playing() while not self.is_playing() and not self.is_stopped(): self.render() - # refresh app after resuming from pause (physics needs to re-parse the scene) - if was_paused and self.is_playing(): - self._app_iface.update() + self.forward() - # return whether physics should proceed - return self.is_playing() + if render: + self.render() def close(self) -> None: """Clean up visualizer resources.""" - # Unsubscribe from stop handle - if self._app_control_on_stop_handle is not None: - self._app_control_on_stop_handle.unsubscribe() - self._app_control_on_stop_handle = None - - def set_stop_handle_enabled(self, enabled: bool) -> None: - """Enable or disable the app control on stop handle. - - Args: - enabled: If True, the stop handle callback is active. - """ - self._disable_app_control_on_stop_handle = not enabled - - 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 - resume the simulation from the last state. This leaves the app in an inconsistent state, where - two possible actions can be taken: - - 1. **Keep the app rendering**: In this case, the simulation is kept running and the app is not shutdown. - However, the physics is not updated and the script cannot be resumed from the last state. The - user has to manually close the app to stop the simulation. - 2. **Shutdown the app**: This is the default behavior. In this case, the app is shutdown and - the simulation is stopped. - - Note: - 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. - """ - pass - # if not self._disable_app_control_on_stop_handle: - # while not omni.timeline.get_timeline_interface().is_playing(): - # self.render() - - def _apply_render_settings_from_cfg(self): # noqa: C901 - """Sets rtx settings specified in the RenderCfg.""" - - # define mapping of user-friendly RenderCfg names to native carb names - rendering_setting_name_mapping = { - "enable_translucency": "/rtx/translucency/enabled", - "enable_reflections": "/rtx/reflections/enabled", - "enable_global_illumination": "/rtx/indirectDiffuse/enabled", - "enable_dlssg": "/rtx-transient/dlssg/enabled", - "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", - "dlss_mode": "/rtx/post/dlss/execMode", - "enable_direct_lighting": "/rtx/directLighting/enabled", - "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", - "enable_shadows": "/rtx/shadows/enabled", - "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", - "dome_light_upper_lower_strategy": "/rtx/domeLight/upperLowerStrategy", - } - - not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] - - # grab the rendering mode using the following priority: - # 1. command line argument --rendering_mode, if provided - # 2. rendering_mode from Render Config, if set - # 3. lastly, default to "balanced" mode, if neither is specified - rendering_mode = self._sim.carb_settings.get("/isaaclab/rendering/rendering_mode") - if not rendering_mode: - rendering_mode = self._sim.cfg.render.rendering_mode - if not rendering_mode: - rendering_mode = "balanced" - - # set preset settings (same behavior as the CLI arg --rendering_mode) - if rendering_mode is not None: - # check if preset is supported - supported_rendering_modes = ["performance", "balanced", "quality"] - if rendering_mode not in supported_rendering_modes: - raise ValueError( - f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." - ) - - # grab isaac lab apps path - isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") - # for Isaac Sim 4.5 compatibility, we use the 4.5 rendering mode app files in a different folder - if get_isaac_sim_version().major < 5: - isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") - - # grab preset settings - preset_filename = os.path.join(isaaclab_app_exp_path, f"rendering_modes/{rendering_mode}.kit") - with open(preset_filename) as file: - preset_dict = toml.load(file) - preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) - - # set presets - for key, value in preset_dict.items(): - key = "/" + key.replace(".", "/") # convert to carb setting format - self._sim.set_setting(key, value) - - # set user-friendly named settings - for key, value in vars(self._sim.cfg.render).items(): - if value is None or key in not_carb_settings: - # skip unset settings and non-carb settings - continue - if key not in rendering_setting_name_mapping: - raise ValueError( - f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" - " need to be updated." - ) - key = rendering_setting_name_mapping[key] - self._sim.set_setting(key, value) - - # set general carb settings - carb_settings = self._sim.cfg.render.carb_settings - if carb_settings is not None: - for key, value in carb_settings.items(): - if "_" in key: - key = "/" + key.replace("_", "/") # convert from python variable style string - elif "." in key: - key = "/" + key.replace(".", "/") # convert from .kit file style string - if self._sim.get_setting(key) is None: - raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") - self._sim.set_setting(key, value) - - # set denoiser mode - if self._sim.cfg.render.antialiasing_mode is not None: - try: - import omni.replicator.core as rep - - rep.settings.set_render_rtx_realtime(antialiasing=self._sim.cfg.render.antialiasing_mode) - except Exception: - pass - - # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. - if self._sim.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": - self._sim.carb_settings.set_string("/rtx/rendermode", "RaytracedLighting") - - def _configure_rendering_dt(self): - """Configures the rendering/timeline rate based on physics dt and render interval.""" - from pxr import Usd - - cfg = self._sim.cfg - stage = self._sim.stage - carb_settings = self._sim.carb_settings - - # compute rendering frequency - render_interval = max(cfg.render_interval, 1) - rendering_hz = int(1.0 / (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 carb_settings.get("/app/runLoops/main/rateLimitEnabled"): - carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) - self._timeline_iface.set_target_framerate(rendering_hz) - - # set stage time codes per second - with Usd.EditContext(stage, stage.GetRootLayer()): - 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(cfg.dt * render_interval) - _loop_runner.set_manual_mode(True) - except Exception: - self._sim.logger.warning( - "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" - ) - carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) - carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) - self._timeline_iface.set_target_framerate(rendering_hz) + self._ov_visualizer.close() From a82e8c799b706305f90dbffecbaa605b8d28cb9c Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 23:01:30 -0800 Subject: [PATCH 115/130] refactor step 11: refactor timeline out from ov visualizer --- source/isaaclab/isaaclab/sim/ov_visualizer.py | 254 ++++++++++++------ .../isaaclab/sim/simulation_context.py | 1 + .../isaaclab/sim/visualizer_interface.py | 14 +- 3 files changed, 192 insertions(+), 77 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/ov_visualizer.py b/source/isaaclab/isaaclab/sim/ov_visualizer.py index 3b770331462..fafe47b2743 100644 --- a/source/isaaclab/isaaclab/sim/ov_visualizer.py +++ b/source/isaaclab/isaaclab/sim/ov_visualizer.py @@ -14,7 +14,7 @@ import toml import torch import weakref -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Callable import omni.kit.app import omni.timeline @@ -23,6 +23,8 @@ from isaaclab.utils.version import get_isaac_sim_version if TYPE_CHECKING: + import carb + from .simulation_context import SimulationContext logger = logging.getLogger(__name__) @@ -64,6 +66,149 @@ class RenderMode(enum.IntEnum): """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" +class TimelineControl: + """Helper class for managing timeline lifecycle (play/pause/stop). + + This class wraps the omni.timeline interface and provides a clean API + for controlling simulation playback. It can be composed by visualizers + or simulation contexts that need timeline control. + + Features: + - Play/pause/stop control with proper physics handle propagation + - Stop event callback subscription + - Timeline state queries (is_playing, is_stopped) + """ + + def __init__( + self, + app_interface: omni.kit.app.IApp, + carb_settings: "carb.settings.ISettings", + on_stop_callback: Callable[[], None] | None = None, + ): + """Initialize timeline control. + + Args: + app_interface: Omniverse Kit application interface. + carb_settings: Carb settings interface for controlling playSimulations. + on_stop_callback: Optional callback to invoke when simulation stops. + """ + self._app_iface = app_interface + self._carb_settings = carb_settings + self._on_stop_callback = on_stop_callback + + # Acquire timeline interface + self._timeline_iface = omni.timeline.get_timeline_interface() + self._timeline_iface.set_auto_update(True) + + # Setup stop handle callback + self._disable_stop_handle = False + timeline_event_stream = self._timeline_iface.get_timeline_event_stream() + self._stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda *args, obj=weakref.proxy(self): obj._on_stop_event(*args), + order=15, + ) + + @property + def timeline_interface(self) -> omni.timeline.ITimeline: + """Get the underlying timeline interface.""" + return self._timeline_iface + + def is_playing(self) -> bool: + """Check whether the simulation is playing. + + Returns: + True if simulation is currently playing, False otherwise. + """ + return self._timeline_iface.is_playing() + + def is_stopped(self) -> bool: + """Check whether the simulation is stopped. + + Returns: + True if simulation is stopped, False otherwise. + """ + return self._timeline_iface.is_stopped() + + def play(self) -> None: + """Start playing the simulation. + + This commits the timeline state and performs one app update step + to propagate all physics handles properly. + """ + self._timeline_iface.play() + self._timeline_iface.commit() + # Perform one step to propagate all physics handles properly + self._carb_settings.set_bool("/app/player/playSimulations", False) + self._app_iface.update() + self._carb_settings.set_bool("/app/player/playSimulations", True) + + def pause(self) -> None: + """Pause the simulation. + + This commits the timeline state to ensure the pause takes effect. + """ + self._timeline_iface.pause() + self._timeline_iface.commit() + + def stop(self) -> None: + """Stop the simulation. + + This commits the timeline state and performs one app update step + to propagate all physics handles properly. + """ + self._timeline_iface.stop() + self._timeline_iface.commit() + # Perform one step to propagate all physics handles properly + self._carb_settings.set_bool("/app/player/playSimulations", False) + self._app_iface.update() + self._carb_settings.set_bool("/app/player/playSimulations", True) + + def set_stop_handle_enabled(self, enabled: bool) -> None: + """Enable or disable the stop handle callback. + + When disabled, the on_stop_callback will not be invoked when + the simulation stops. + + Args: + enabled: If True, the stop handle callback is active. + """ + self._disable_stop_handle = not enabled + + def set_target_framerate(self, hz: int) -> None: + """Set the target framerate for the timeline. + + Args: + hz: Target framerate in Hz. + """ + self._timeline_iface.set_target_framerate(hz) + + def set_time_codes_per_second(self, time_codes_per_second: float) -> None: + """Set the time codes per second for the timeline. + + Args: + time_codes_per_second: Number of time codes per second. + """ + self._timeline_iface.set_time_codes_per_second(time_codes_per_second) + + def close(self) -> None: + """Clean up timeline resources. + + Unsubscribes from the stop event and releases the timeline handle. + """ + if self._stop_handle is not None: + self._stop_handle.unsubscribe() + self._stop_handle = None + + def _on_stop_event(self, _) -> None: + """Internal callback when simulation stops. + + Invokes the on_stop_callback if the stop handle is enabled. + """ + if not self._disable_stop_handle and self._on_stop_callback is not None: + self._on_stop_callback() + + class OVVisualizer: """Omniverse visualizer helper managing viewport/rendering for PhysX workflow. @@ -146,25 +291,16 @@ def __init__(self, sim_context: "SimulationContext"): if self._sim.carb_settings.get("/isaaclab/has_gui"): self._sim.cfg.enable_scene_query_support = True - # Acquire timeline interface - self._timeline_iface = omni.timeline.get_timeline_interface() - - # Set timeline auto update to True - self._timeline_iface.set_auto_update(True) + # Initialize timeline control (manages play/pause/stop and stop callbacks) + self._timeline = TimelineControl( + app_interface=self._app_iface, + carb_settings=self._sim.carb_settings, + on_stop_callback=self._on_timeline_stop, + ) # Configure rendering/timeline rate self._configure_rendering_dt() - # Add callback to deal the simulation app when simulation is stopped. - # this is needed because physics views go invalid once we stop the simulation - timeline_event_stream = self._timeline_iface.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 - # ------------------------------------------------------------------ # Properties # ------------------------------------------------------------------ @@ -193,36 +329,30 @@ def render_viewport(self) -> bool: # Timeline Control # ------------------------------------------------------------------ + @property + def timeline(self) -> TimelineControl: + """Get the timeline control instance.""" + return self._timeline + def is_playing(self) -> bool: """Check whether the simulation is playing.""" - return self._timeline_iface.is_playing() + return self._timeline.is_playing() def is_stopped(self) -> bool: """Check whether the simulation is stopped.""" - return self._timeline_iface.is_stopped() + return self._timeline.is_stopped() def play(self) -> None: """Start playing the simulation.""" - self._timeline_iface.play() - self._timeline_iface.commit() - # perform one step to propagate all physics handles properly - self._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) + self._timeline.play() def pause(self) -> None: """Pause the simulation.""" - self._timeline_iface.pause() - self._timeline_iface.commit() + self._timeline.pause() def stop(self) -> None: """Stop the simulation.""" - self._timeline_iface.stop() - self._timeline_iface.commit() - # perform one step to propagate all physics handles properly - self._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) + self._timeline.stop() # ------------------------------------------------------------------ # Render Mode @@ -319,28 +449,11 @@ def render(self, mode: RenderMode | None = None): # check if we need to change the render mode if mode is not None: self.set_render_mode(mode) - # render based on the render mode - if self.render_mode == RenderMode.NO_GUI_OR_RENDERING: - # we never want to render anything here (this is for complete headless mode) - pass - elif self.render_mode == RenderMode.NO_RENDERING: - # throttle the rendering frequency to keep the UI responsive - self._render_throttle_counter += 1 - 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 - self._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) - else: - # manually flush the fabric data to update Hydra textures - self._sim.forward() - # render the simulation - # 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._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) + # 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._sim.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self._sim.set_setting("/app/player/playSimulations", True) # app.update() may be changing the cuda device, so we force it back to our desired device here if "cuda" in self._sim.device: @@ -358,37 +471,28 @@ def reset(self, soft: bool = False) -> None: """ if not soft: # disable app control on stop handle during reset - self.set_stop_handle_enabled(False) + self._timeline.set_stop_handle_enabled(False) if not self.is_stopped(): self.stop() - self.set_stop_handle_enabled(True) + self._timeline.set_stop_handle_enabled(True) # play the simulation self.play() # warmup renders to initialize replicator buffers for _ in range(2): self.render() - def set_stop_handle_enabled(self, enabled: bool) -> None: - """Enable or disable the app control on stop handle. - - Args: - enabled: If True, the stop handle callback is active. - """ - self._disable_app_control_on_stop_handle = not enabled - def close(self) -> None: """Clean up visualizer resources.""" - # Unsubscribe from stop handle - if self._app_control_on_stop_handle is not None: - self._app_control_on_stop_handle.unsubscribe() - self._app_control_on_stop_handle = None + # Clean up timeline control + if self._timeline is not None: + self._timeline.close() # ------------------------------------------------------------------ # Private Methods # ------------------------------------------------------------------ - def _app_control_on_stop_handle_fn(self, _): - """Callback to deal with the app when the simulation is stopped. + def _on_timeline_stop(self) -> None: + """Callback invoked when the simulation timeline is stopped. Once the simulation is stopped, the physics handles go invalid. After that, it is not possible to resume the simulation from the last state. This leaves the app in an inconsistent state, where @@ -404,10 +508,8 @@ def _app_control_on_stop_handle_fn(self, _): 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. """ + # Currently a no-op, but can be extended to handle stop events pass - # if not self._disable_app_control_on_stop_handle: - # while not omni.timeline.get_timeline_interface().is_playing(): - # self.render() def _apply_render_settings_from_cfg(self): # noqa: C901 """Sets rtx settings specified in the RenderCfg.""" @@ -519,12 +621,12 @@ def _configure_rendering_dt(self): # Otherwise run the app as fast as possible and do not specify the target rate if carb_settings.get("/app/runLoops/main/rateLimitEnabled"): carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) - self._timeline_iface.set_target_framerate(rendering_hz) + self._timeline.set_target_framerate(rendering_hz) # set stage time codes per second with Usd.EditContext(stage, stage.GetRootLayer()): stage.SetTimeCodesPerSecond(rendering_hz) - self._timeline_iface.set_time_codes_per_second(rendering_hz) + self._timeline.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 @@ -540,4 +642,4 @@ def _configure_rendering_dt(self): ) carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) - self._timeline_iface.set_target_framerate(rendering_hz) + self._timeline.set_target_framerate(rendering_hz) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index d4985e9294c..450d71aa7e3 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -428,6 +428,7 @@ def render(self, mode: RenderMode | None = None): Args: mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. """ + self._physics_interface.forward() self._visualizer.render(mode) @classmethod diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index 60b746c3918..c6c1e6146e6 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -40,6 +40,9 @@ def __init__(self, sim_context: "SimulationContext"): # Create OV visualizer helper self._ov_visualizer = OVVisualizer(sim_context) + # Track previous playing state to detect transitions + self._was_playing = False + # ------------------------------------------------------------------ # Properties (delegate to OVVisualizer) # ------------------------------------------------------------------ @@ -163,9 +166,18 @@ def step(self, render: bool = True) -> None: Args: render: Whether to render after stepping. """ - # if paused, keep rendering until playing or stopped + # If paused, keep rendering until playing or stopped while not self.is_playing() and not self.is_stopped(): self.render() + self._was_playing = False + + # # Detect transition: was not playing → now playing (resume from pause) + is_playing = self.is_playing() + if not self._was_playing and is_playing: + self.reset(soft=True) # TODO: it is currently buggy + + # Update state tracking + self._was_playing = is_playing self.forward() From fadd8e7dd88dd96baf326ca21e67a261f9488597 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 23:17:39 -0800 Subject: [PATCH 116/130] remove app public api from visualizer interface --- source/isaaclab/isaaclab/sim/visualizer_interface.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index c6c1e6146e6..af889057df1 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -36,6 +36,7 @@ def __init__(self, sim_context: "SimulationContext"): sim_context: Parent simulation context. """ super().__init__(sim_context) + self.dt = self._sim.cfg.dt * self._sim.cfg.render_interval # Create OV visualizer helper self._ov_visualizer = OVVisualizer(sim_context) @@ -46,10 +47,6 @@ def __init__(self, sim_context: "SimulationContext"): # ------------------------------------------------------------------ # Properties (delegate to OVVisualizer) # ------------------------------------------------------------------ - @property - def app(self): - """Omniverse Kit Application interface.""" - return self._ov_visualizer.app @property def offscreen_render(self) -> bool: From 13920ef6d6e24d503fa63a9927ec77221db15098 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 1 Feb 2026 23:55:25 -0800 Subject: [PATCH 117/130] refactor step 12: update visualizer_interface to be in accordance with newton version --- .../isaaclab/sim/visualizer_interface.py | 125 +++++++----------- 1 file changed, 51 insertions(+), 74 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index af889057df1..8cfdfb4be08 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Visualizer interface for SimulationContext (Omniverse/PhysX workflow).""" +"""Visualizer interface for SimulationContext.""" from __future__ import annotations @@ -20,11 +20,7 @@ class VisualizerInterface(Interface): - """Manages viewport/rendering for SimulationContext (Omniverse workflow). - - This interface delegates rendering operations to an internal OVVisualizer helper, - while managing the lifecycle integration with SimulationContext. - """ + """Manages visualizer lifecycle and rendering for SimulationContext.""" # Expose RenderMode as class attribute for backwards compatibility RenderMode = RenderMode @@ -37,16 +33,24 @@ def __init__(self, sim_context: "SimulationContext"): """ super().__init__(sim_context) self.dt = self._sim.cfg.dt * self._sim.cfg.render_interval - # Create OV visualizer helper self._ov_visualizer = OVVisualizer(sim_context) - # Track previous playing state to detect transitions self._was_playing = False - # ------------------------------------------------------------------ - # Properties (delegate to OVVisualizer) - # ------------------------------------------------------------------ + # -- Properties -- + + @property + def settings(self): + return self._sim.carb_settings + + @property + def device(self) -> str: + return self._sim.device + + @property + def stage(self): + return self._sim.stage @property def offscreen_render(self) -> bool: @@ -62,12 +66,6 @@ def render_viewport(self) -> bool: def render_mode(self) -> RenderMode: """Current render mode.""" return self._ov_visualizer.render_mode - - @render_mode.setter - def render_mode(self, value: RenderMode) -> None: - """Set render mode.""" - self._ov_visualizer.render_mode = value - # ------------------------------------------------------------------ # Timeline Control (delegate to OVVisualizer) # ------------------------------------------------------------------ @@ -80,18 +78,6 @@ def is_stopped(self) -> bool: """Check whether the simulation is stopped.""" return self._ov_visualizer.is_stopped() - def play(self) -> None: - """Start playing the simulation.""" - self._ov_visualizer.play() - - def pause(self) -> None: - """Pause the simulation.""" - self._ov_visualizer.pause() - - def stop(self) -> None: - """Stop the simulation.""" - self._ov_visualizer.stop() - # ------------------------------------------------------------------ # Render Mode (delegate to OVVisualizer) # ------------------------------------------------------------------ @@ -107,54 +93,13 @@ def set_render_mode(self, mode: RenderMode): """ self._ov_visualizer.set_render_mode(mode) - # ------------------------------------------------------------------ - # Camera (delegate to OVVisualizer) - # ------------------------------------------------------------------ - - def set_camera_view( - self, - eye: tuple[float, float, float], - target: tuple[float, float, float], - camera_prim_path: str = "/OmniverseKit_Persp", - ): - """Set the location and target of the viewport camera in the stage. - - Args: - eye: The location of the camera eye. - target: The location of the camera target. - camera_prim_path: The path to the camera primitive in the stage. Defaults to - "/OmniverseKit_Persp". - """ - self._ov_visualizer.set_camera_view(eye, target, camera_prim_path) - - # ------------------------------------------------------------------ - # Interface Methods - # ------------------------------------------------------------------ - - def render(self, mode: RenderMode | None = None): - """Refreshes the rendering components including UI elements and view-ports depending on the render mode. - - This function is used to refresh the rendering components of the simulation. This includes updating the - view-ports, UI elements, and other extensions (besides physics simulation) that are running in the - background. The rendering components are refreshed based on the render mode. - Please see :class:`RenderMode` for more information on the different render modes. - - Args: - mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. - """ - self._ov_visualizer.render(mode) - - def reset(self, soft: bool = False) -> None: - """Reset visualizer (timeline control + warmup renders on hard reset). + def forward(self) -> None: + """Sync scene data and step all active visualizers. Args: - soft: If True, skip timeline reset and warmup. + dt: Time step in seconds (0.0 for kinematics-only). """ - self._ov_visualizer.reset(soft) - - def forward(self) -> None: - """No-op for visualizer (rendering happens in render()).""" pass def step(self, render: bool = True) -> None: @@ -181,6 +126,38 @@ def step(self, render: bool = True) -> None: if render: self.render() + def reset(self, soft: bool) -> None: + """Reset visualizers (warmup renders on hard reset).""" + self._ov_visualizer.reset(soft) + def close(self) -> None: - """Clean up visualizer resources.""" + """Close all visualizers and clean up.""" self._ov_visualizer.close() + + def play(self) -> None: + """Handle simulation start.""" + self._ov_visualizer.play() + + def stop(self) -> None: + """Handle simulation stop.""" + self._ov_visualizer.stop() + + def pause(self) -> None: + """Pause the simulation.""" + self._ov_visualizer.pause() + + def render(self, mode: RenderMode | None = None): + """Render the scene (OV mode only). + + Args: + mode: Render mode to set, or None to keep current. + + Returns: + True if rendered, False if not in OV mode. + """ + self._ov_visualizer.render(mode) + + + def set_camera_view(self, eye: tuple, target: tuple) -> None: + """Set camera view on all visualizers that support it.""" + self._ov_visualizer.set_camera_view(eye, target, "/OmniverseKit_Persp") From 6ac6ed403be9ccdcc4b0833e9e16f37535edc1ac Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 2 Feb 2026 18:00:57 -0800 Subject: [PATCH 118/130] refactor step 13: refactored visualizers to be newton consistent --- .../isaaclab/isaaclab/sim/simulation_cfg.py | 5 +- .../isaaclab/sim/simulation_context.py | 4 +- .../isaaclab/sim/visualizer_interface.py | 217 +++++++++--- .../isaaclab/isaaclab/visualizers/__init__.py | 103 ++++++ .../physx_ov_visualizer.py} | 313 ++++++++++++------ .../visualizers/physx_ov_visualizer_cfg.py | 83 +++++ .../isaaclab/visualizers/visualizer.py | 86 +++++ .../isaaclab/visualizers/visualizer_cfg.py | 82 +++++ 8 files changed, 747 insertions(+), 146 deletions(-) create mode 100644 source/isaaclab/isaaclab/visualizers/__init__.py rename source/isaaclab/isaaclab/{sim/ov_visualizer.py => visualizers/physx_ov_visualizer.py} (79%) create mode 100644 source/isaaclab/isaaclab/visualizers/physx_ov_visualizer_cfg.py create mode 100644 source/isaaclab/isaaclab/visualizers/visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/visualizer_cfg.py diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index ac19ca22b32..355cda5711c 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -14,7 +14,7 @@ from isaaclab.utils import configclass from .spawners.materials import RigidBodyMaterialCfg - +from isaaclab.visualizers import VisualizerCfg @configclass class PhysxCfg: @@ -436,3 +436,6 @@ class SimulationCfg: If :attr:`save_logs_to_file` is True, the logs will be saved to the directory specified by :attr:`log_dir`. If None, the logs will be saved to the temp directory. """ + + visualizer_cfgs: list[VisualizerCfg] | VisualizerCfg | None = None + """The list of visualizer configurations. Default is None.""" \ No newline at end of file diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 450d71aa7e3..af1cc534822 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -275,7 +275,7 @@ def set_camera_view( camera_prim_path: The path to the camera primitive in the stage. Defaults to "/OmniverseKit_Persp". """ - self._visualizer.set_camera_view(eye, target, camera_prim_path) + self._visualizer.set_camera_view(eye, target) def set_render_mode(self, mode: RenderMode): """Change the current render mode of the simulation. @@ -429,7 +429,7 @@ def render(self, mode: RenderMode | None = None): mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. """ self._physics_interface.forward() - self._visualizer.render(mode) + self._visualizer.render() @classmethod def clear(cls): diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index 8cfdfb4be08..01bf6d4ae96 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -7,24 +7,24 @@ from __future__ import annotations +import logging from typing import TYPE_CHECKING +from isaaclab.visualizers import Visualizer + from .interface import Interface -from .ov_visualizer import OVVisualizer, RenderMode +from isaaclab.visualizers.physx_ov_visualizer import PhysxOVVisualizer, RenderMode +from isaaclab.visualizers.physx_ov_visualizer_cfg import PhysxOVVisualizerCfg if TYPE_CHECKING: from .simulation_context import SimulationContext -# Re-export RenderMode for backwards compatibility -__all__ = ["RenderMode", "VisualizerInterface"] +logger = logging.getLogger(__name__) class VisualizerInterface(Interface): """Manages visualizer lifecycle and rendering for SimulationContext.""" - # Expose RenderMode as class attribute for backwards compatibility - RenderMode = RenderMode - def __init__(self, sim_context: "SimulationContext"): """Initialize visualizer interface. @@ -33,11 +33,18 @@ def __init__(self, sim_context: "SimulationContext"): """ super().__init__(sim_context) self.dt = self._sim.cfg.dt * self._sim.cfg.render_interval - # Create OV visualizer helper - self._ov_visualizer = OVVisualizer(sim_context) - # Track previous playing state to detect transitions + + # Visualizer state + visualizers = "omniverse" + self._visualizers_str = [v.strip() for v in visualizers.split(",") if v.strip()] + self._visualizers: list[Visualizer] = [] + self._visualizer_step_counter = 0 + self._scene_data_provider: SceneDataProvider | None = None self._was_playing = False + # Initialize visualizers immediately + self.initialize_visualizers() + # -- Properties -- @property @@ -53,34 +60,110 @@ def stage(self): return self._sim.stage @property - def offscreen_render(self) -> bool: - """Whether offscreen rendering is enabled.""" - return self._ov_visualizer.offscreen_render + def visualizers(self) -> list[Visualizer]: + return self._visualizers @property - def render_viewport(self) -> bool: - """Whether the default viewport should be rendered.""" - return self._ov_visualizer.render_viewport + def scene_data_provider(self) -> SceneDataProvider | None: + return self._scene_data_provider @property def render_mode(self) -> RenderMode: - """Current render mode.""" - return self._ov_visualizer.render_mode - # ------------------------------------------------------------------ - # Timeline Control (delegate to OVVisualizer) - # ------------------------------------------------------------------ + """Current render mode from the first PhysxOVVisualizer.""" + for viz in self._visualizers: + if isinstance(viz, PhysxOVVisualizer): + return viz.render_mode + return RenderMode.NO_GUI_OR_RENDERING + + # -- Visualizer Initialization -- + + def _create_default_visualizer_configs(self, requested: list[str]) -> list: + """Create default configs for requested visualizer types.""" + configs = [] + type_map = {"omniverse": PhysxOVVisualizerCfg} + + for viz_type in requested: + if viz_type in type_map: + try: + configs.append(type_map[viz_type]()) + except Exception as e: + logger.error(f"Failed to create default config for '{viz_type}': {e}") + else: + logger.warning(f"Unknown visualizer type '{viz_type}'. Valid: {list(type_map.keys())}") + + return configs + + def initialize_visualizers(self) -> None: + """Initialize visualizers based on --visualizer flag.""" + if not self._visualizers_str: + if bool(self.settings.get("/isaaclab/visualizer")) or bool(self.settings.get("/isaaclab/render/offscreen")): + logger.info("No visualizers specified via --visualizer flag.") + return + + # Get or create visualizer configs + cfg_list = self._sim.cfg.visualizer_cfgs + if cfg_list is None: + visualizer_cfgs = self._create_default_visualizer_configs(self._visualizers_str) + else: + visualizer_cfgs = cfg_list if isinstance(cfg_list, list) else [cfg_list] + visualizer_cfgs = [c for c in visualizer_cfgs if c.visualizer_type in self._visualizers_str] + + if not visualizer_cfgs: + logger.info(f"Creating default configs for: {self._visualizers_str}") + visualizer_cfgs = self._create_default_visualizer_configs(self._visualizers_str) + + if not visualizer_cfgs: + return + + # Create scene data provider + self._scene_data_provider = None # SceneDataProvider(visualizer_cfgs) + + # Initialize each visualizer + for cfg in visualizer_cfgs: + try: + visualizer = cfg.create_visualizer() + scene_data = self._build_scene_data(cfg) + visualizer.initialize(scene_data) + self._visualizers.append(visualizer) + logger.info(f"Initialized: {type(visualizer).__name__} ({cfg.visualizer_type})") + except Exception as e: + logger.error(f"Failed to init '{cfg.visualizer_type}': {e}") + + def _build_scene_data(self, cfg) -> dict: + """Build scene data dict for visualizer initialization.""" + if cfg.visualizer_type in ("newton", "rerun"): + return {"scene_data_provider": self._scene_data_provider} + elif cfg.visualizer_type == "omniverse": + return {"usd_stage": self._sim.stage, "simulation_context": self._sim} + return {} + + # -- Unified Interface Methods -- def is_playing(self) -> bool: """Check whether the simulation is playing.""" - return self._ov_visualizer.is_playing() + for viz in self.visualizers: + if viz.is_playing(): + return True + return False def is_stopped(self) -> bool: """Check whether the simulation is stopped.""" - return self._ov_visualizer.is_stopped() + for viz in self.visualizers: + if viz.is_stopped(): + return True + return False + + def forward(self) -> None: + """Sync scene data and step all active visualizers. + + Args: + dt: Time step in seconds (0.0 for kinematics-only). + """ + if self._scene_data_provider: + self._scene_data_provider.update() - # ------------------------------------------------------------------ - # Render Mode (delegate to OVVisualizer) - # ------------------------------------------------------------------ + if not self._visualizers: + return def set_render_mode(self, mode: RenderMode): """Change the current render mode of the simulation. @@ -91,16 +174,9 @@ def set_render_mode(self, mode: RenderMode): mode: The rendering mode. If different than current rendering mode, the mode is changed to the new mode. """ - self._ov_visualizer.set_render_mode(mode) - - - def forward(self) -> None: - """Sync scene data and step all active visualizers. - - Args: - dt: Time step in seconds (0.0 for kinematics-only). - """ - pass + for viz in self._visualizers: + if isinstance(viz, PhysxOVVisualizer): + viz.set_render_mode(mode) def step(self, render: bool = True) -> None: """Step visualizers and optionally render. @@ -113,10 +189,10 @@ def step(self, render: bool = True) -> None: self.render() self._was_playing = False - # # Detect transition: was not playing → now playing (resume from pause) + # Detect transition: was not playing → now playing (resume from pause) is_playing = self.is_playing() if not self._was_playing and is_playing: - self.reset(soft=True) # TODO: it is currently buggy + self.reset(soft=True) # Update state tracking self._was_playing = is_playing @@ -128,36 +204,77 @@ def step(self, render: bool = True) -> None: def reset(self, soft: bool) -> None: """Reset visualizers (warmup renders on hard reset).""" - self._ov_visualizer.reset(soft) + for viz in self._visualizers: + if isinstance(viz, PhysxOVVisualizer): + viz.reset(soft) def close(self) -> None: """Close all visualizers and clean up.""" - self._ov_visualizer.close() + for viz in self._visualizers: + try: + viz.close() + except Exception as e: + logger.error(f"Error closing {type(viz).__name__}: {e}") + + self._visualizers.clear() + logger.info("All visualizers closed") def play(self) -> None: """Handle simulation start.""" - self._ov_visualizer.play() + for viz in self._visualizers: + viz.play() def stop(self) -> None: """Handle simulation stop.""" - self._ov_visualizer.stop() + for viz in self._visualizers: + viz.stop() def pause(self) -> None: """Pause the simulation.""" - self._ov_visualizer.pause() + for viz in self._visualizers: + viz.pause() - def render(self, mode: RenderMode | None = None): - """Render the scene (OV mode only). + def render(self) -> None: + """Render the scene. Args: mode: Render mode to set, or None to keep current. - - Returns: - True if rendered, False if not in OV mode. """ - self._ov_visualizer.render(mode) - + self._visualizer_step_counter += 1 + to_remove = [] + + for viz in self._visualizers: + try: + if not viz.is_running(): + to_remove.append(viz) + continue + + # Block while training paused + while viz.is_training_paused() and viz.is_running(): + viz.step(0.0, state=None) + + viz.step(self.get_rendering_dt() or self.dt, state=None) + except Exception as e: + logger.error(f"Error stepping {type(viz).__name__}: {e}") + to_remove.append(viz) + + for viz in to_remove: + try: + viz.close() + self._visualizers.remove(viz) + logger.info(f"Removed: {type(viz).__name__}") + except Exception as e: + logger.error(f"Error closing visualizer: {e}") + + def get_rendering_dt(self) -> float: + """Get rendering dt from visualizers, or fall back to physics dt.""" + for viz in self._visualizers: + dt = viz.get_rendering_dt() + if dt is not None: + return dt + return self.dt def set_camera_view(self, eye: tuple, target: tuple) -> None: """Set camera view on all visualizers that support it.""" - self._ov_visualizer.set_camera_view(eye, target, "/OmniverseKit_Persp") + for viz in self._visualizers: + viz.set_camera_view(eye, target) diff --git a/source/isaaclab/isaaclab/visualizers/__init__.py b/source/isaaclab/isaaclab/visualizers/__init__.py new file mode 100644 index 00000000000..762874f3e19 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/__init__.py @@ -0,0 +1,103 @@ +# # 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 + +# """Sub-package for visualizer configurations and implementations. + +# This sub-package contains configuration classes and implementations for different +# visualizer backends that can be used with Isaac Lab. The visualizers are used for +# debug visualization and monitoring of the simulation, separate from rendering for sensors. + +# Supported visualizers: +# - Newton OpenGL Visualizer: Lightweight OpenGL-based visualizer +# - Omniverse Visualizer: High-fidelity Omniverse-based visualizer using Isaac Sim viewport +# - Rerun Visualizer: Web-based Rerun visualizer with recording and timeline scrubbing + +# Visualizer Registry +# ------------------- +# This module uses a registry pattern to decouple visualizer instantiation from specific types. +# Visualizer implementations can register themselves using the `register_visualizer` decorator, +# and configs can create visualizers via the `create_visualizer()` factory method. +# """ + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +# # Import config classes (no circular dependency) +# from .newton_visualizer_cfg import NewtonVisualizerCfg +# from .ov_visualizer_cfg import OVVisualizerCfg +# from .rerun_visualizer_cfg import RerunVisualizerCfg + +# # Import base classes first +from .visualizer import Visualizer +from .visualizer_cfg import VisualizerCfg + +# if TYPE_CHECKING: +# from typing import Type + +# from .newton_visualizer import NewtonVisualizer +# from .ov_visualizer import OVVisualizer +# from .rerun_visualizer import RerunVisualizer + +# # Global registry for visualizer types (lazy-loaded) +# _VISUALIZER_REGISTRY: dict[str, Any] = {} + +# __all__ = [ +# "Visualizer", +# "VisualizerCfg", +# "NewtonVisualizerCfg", +# "OVVisualizerCfg", +# "RerunVisualizerCfg", +# "get_visualizer_class", +# ] + + +# # Register only selected visualizers to reduce unnecessary imports +# def get_visualizer_class(name: str) -> type[Visualizer] | None: +# """Get a visualizer class by name (lazy-loaded). + +# Visualizer classes are imported only when requested to avoid loading +# unnecessary dependencies. + +# Args: +# name: Visualizer type name (e.g., 'newton', 'rerun', 'omniverse'). + +# Returns: +# Visualizer class if found, None otherwise. + +# Example: +# >>> visualizer_cls = get_visualizer_class('newton') +# >>> if visualizer_cls: +# >>> visualizer = visualizer_cls(cfg) +# """ +# # Check if already loaded +# if name in _VISUALIZER_REGISTRY: +# return _VISUALIZER_REGISTRY[name] + +# # Lazy-load visualizer on first access +# try: +# if name == "newton": +# from .newton_visualizer import NewtonVisualizer + +# _VISUALIZER_REGISTRY["newton"] = NewtonVisualizer +# return NewtonVisualizer +# elif name == "omniverse": +# from .ov_visualizer import OVVisualizer + +# _VISUALIZER_REGISTRY["omniverse"] = OVVisualizer +# return OVVisualizer +# elif name == "rerun": +# from .rerun_visualizer import RerunVisualizer + +# _VISUALIZER_REGISTRY["rerun"] = RerunVisualizer +# return RerunVisualizer +# else: +# return None +# except ImportError as e: +# # Log import error but don't crash - visualizer just won't be available +# import warnings + +# warnings.warn(f"Failed to load visualizer '{name}': {e}", ImportWarning) +# return None diff --git a/source/isaaclab/isaaclab/sim/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py similarity index 79% rename from source/isaaclab/isaaclab/sim/ov_visualizer.py rename to source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py index fafe47b2743..be6857a09cc 100644 --- a/source/isaaclab/isaaclab/sim/ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Omniverse visualizer helper for PhysX-based SimulationContext.""" +"""Omniverse visualizer for PhysX-based SimulationContext.""" from __future__ import annotations @@ -14,18 +14,18 @@ import toml import torch import weakref -from typing import TYPE_CHECKING, Callable +from typing import TYPE_CHECKING, Any, Callable import omni.kit.app import omni.timeline -from isaacsim.core.utils.viewports import set_camera_view - from isaaclab.utils.version import get_isaac_sim_version +from .visualizers import Visualizer if TYPE_CHECKING: import carb - from .simulation_context import SimulationContext + from .physx_ov_visualizer_cfg import PhysxOVVisualizerCfg + from isaaclab.sim.simulation_context import SimulationContext logger = logging.getLogger(__name__) @@ -209,28 +209,68 @@ def _on_stop_event(self, _) -> None: self._on_stop_callback() -class OVVisualizer: - """Omniverse visualizer helper managing viewport/rendering for PhysX workflow. +class PhysxOVVisualizer(Visualizer): + """Omniverse visualizer managing viewport/rendering for PhysX workflow. - This class handles: + This class extends the base :class:`Visualizer` and handles: - Viewport context and window management - Render mode switching - Camera view setup - Render settings from configuration - Throttled rendering for UI responsiveness - Timeline control (play/pause/stop) + + Lifecycle: __init__(cfg) -> initialize(scene_data) -> step() (repeated) -> close() """ - # Expose RenderMode for backwards compatibility - RenderMode = RenderMode + def __init__(self, cfg: "PhysxOVVisualizerCfg"): + """Initialize PhysX OV visualizer with configuration. + + Args: + cfg: Configuration for the visualizer. + """ + super().__init__(cfg) + self.cfg: "PhysxOVVisualizerCfg" = cfg + + # Will be set during initialize() + self._sim: "SimulationContext | None" = None + self._app_iface = None + self._timeline: TimelineControl | None = None + + # Render state + self._local_gui = False + self._livestream_gui = False + self._xr_gui = False + self._offscreen_render = False + self._render_viewport = False + + # Viewport state + self._viewport_context = None + self._viewport_window = None + self._render_throttle_counter = 0 + self._render_throttle_period = cfg.render_throttle_period + + # Render mode + self.render_mode = RenderMode.NO_GUI_OR_RENDERING - def __init__(self, sim_context: "SimulationContext"): - """Initialize OV visualizer. + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + """Initialize visualizer with simulation context. Args: - sim_context: Parent simulation context. + scene_data: Dictionary containing: + - 'simulation_context': The SimulationContext instance (required) + - 'usd_stage': The USD stage (optional, can get from sim context) """ - self._sim = sim_context + if self._is_initialized: + logger.warning("[PhysxOVVisualizer] Already initialized.") + return + + if scene_data is None: + raise ValueError("PhysxOVVisualizer requires scene_data with 'simulation_context'") + + self._sim = scene_data.get("simulation_context") + if self._sim is None: + raise ValueError("PhysxOVVisualizer requires 'simulation_context' in scene_data") # Acquire application interface self._app_iface = omni.kit.app.get_app_interface() @@ -249,14 +289,10 @@ def __init__(self, sim_context: "SimulationContext"): # Apply render settings from config self._apply_render_settings_from_cfg() - # Initialize viewport state - self._viewport_context = None - self._viewport_window = None - self._render_throttle_counter = 0 - self._render_throttle_period = 5 - # Store the default render mode - if not self._sim.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: + if self.cfg.default_render_mode is not None: + self.render_mode = self.cfg.default_render_mode + elif not self._sim.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: # set default render mode # note: this is the terminal state: cannot exit from this render mode self.render_mode = RenderMode.NO_GUI_OR_RENDERING @@ -276,7 +312,7 @@ def __init__(self, sim_context: "SimulationContext"): self._viewport_context = get_active_viewport() self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] # acquire viewport window - self._viewport_window = ui.Workspace.get_window("Viewport") + self._viewport_window = ui.Workspace.get_window(self.cfg.viewport_name or "Viewport") # Check the case where we don't need to render the viewport # since render_viewport can only be False in headless mode, we only need to check for offscreen_render @@ -301,13 +337,57 @@ def __init__(self, sim_context: "SimulationContext"): # Configure rendering/timeline rate self._configure_rendering_dt() - # ------------------------------------------------------------------ - # Properties - # ------------------------------------------------------------------ + # Set initial camera view + self.set_camera_view(self.cfg.camera_position, self.cfg.camera_target) + + self._is_initialized = True + logger.info("[PhysxOVVisualizer] Initialized") + + def step(self, dt: float, state: Any | None = None) -> None: + """Update visualization for one step (render the scene). + + Args: + dt: Time step in seconds. + state: Updated physics state (unused for OV - USD stage is auto-synced). + """ + if not self._is_initialized: + return + + self.render() + + def render(self, mode: RenderMode | None = None) -> None: + """Refreshes the rendering components including UI elements and view-ports depending on the render mode. + + This function is used to refresh the rendering components of the simulation. This includes updating the + view-ports, UI elements, and other extensions (besides physics simulation) that are running in the + background. The rendering components are refreshed based on the render mode. + + Please see :class:`RenderMode` for more information on the different render modes. + + Args: + mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. + """ + if self._sim is None or self._app_iface is None: + return + + # check if we need to change the render mode + if mode is not None: + self.set_render_mode(mode) + # 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._sim.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self._sim.set_setting("/app/player/playSimulations", True) + + # app.update() may be changing the cuda device, so we force it back to our desired device here + if "cuda" in self._sim.device: + torch.cuda.set_device(self._sim.device) @property def has_gui(self) -> bool: """Whether any GUI is available (local, livestreamed, or XR).""" + if self._sim is None: + return False return bool(self._sim.carb_settings.get("/isaaclab/has_gui")) @property @@ -330,35 +410,121 @@ def render_viewport(self) -> bool: # ------------------------------------------------------------------ @property - def timeline(self) -> TimelineControl: + def timeline(self) -> TimelineControl | None: """Get the timeline control instance.""" return self._timeline def is_playing(self) -> bool: """Check whether the simulation is playing.""" + if self._timeline is None: + return False return self._timeline.is_playing() def is_stopped(self) -> bool: """Check whether the simulation is stopped.""" + if self._timeline is None: + return True return self._timeline.is_stopped() def play(self) -> None: """Start playing the simulation.""" - self._timeline.play() + if self._timeline is not None: + self._timeline.play() def pause(self) -> None: """Pause the simulation.""" - self._timeline.pause() + if self._timeline is not None: + self._timeline.pause() def stop(self) -> None: """Stop the simulation.""" - self._timeline.stop() + if self._timeline is not None: + self._timeline.stop() # ------------------------------------------------------------------ - # Render Mode + # Visualizer Interface Implementation # ------------------------------------------------------------------ - def set_render_mode(self, mode: RenderMode): + def is_running(self) -> bool: + """Check if visualizer is still running.""" + if self._app_iface is None: + return False + try: + return self._app_iface.is_running() + except Exception: + return False + + def is_training_paused(self) -> bool: + """Check if training is paused (always False for OV).""" + return False + + def supports_markers(self) -> bool: + """Supports markers via USD prims.""" + return True + + def supports_live_plots(self) -> bool: + """Supports live plots via Isaac Lab UI.""" + return True + + def get_rendering_dt(self) -> float | None: + """Get rendering dt based on OV rate limiting settings.""" + if self._sim is None: + return None + + settings = self._sim.carb_settings + + def _from_frequency(): + freq = settings.get("/app/runLoops/main/rateLimitFrequency") + return 1.0 / freq if freq else None + + if settings.get("/app/runLoops/main/rateLimitEnabled"): + return _from_frequency() + + try: + import omni.kit.loop._loop as omni_loop + + runner = omni_loop.acquire_loop_interface() + return runner.get_manual_step_size() if runner.get_manual_mode() else _from_frequency() + except Exception: + return _from_frequency() + + def set_camera_view( + self, eye: tuple[float, float, float] | list[float], target: tuple[float, float, float] | list[float] + ) -> None: + """Set the location and target of the viewport camera in the stage. + + Note: + This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. + It is provided here for convenience to reduce the amount of imports needed. + + Args: + eye: The location of the camera eye. + target: The location of the camera target. + camera_prim_path: The path to the camera primitive in the stage. Defaults to + the value from config or "/OmniverseKit_Persp". + """ + if not self._is_initialized: + logger.warning("[PhysxOVVisualizer] Cannot set camera view - visualizer not initialized.") + return + + try: + # Import Isaac Sim viewport utilities + import isaacsim.core.utils.viewports as vp_utils + + # Get the camera prim path - use viewport context or config default + camera_path = self.cfg.camera_prim_path + + # Use Isaac Sim utility to set camera view + vp_utils.set_camera_view( + eye=list(eye), target=list(target), camera_prim_path=camera_path + ) + + logger.info(f"[PhysxOVVisualizer] Camera set: pos={eye}, target={target}, camera={camera_path}") + + except Exception as e: + logger.warning(f"[PhysxOVVisualizer] Could not set camera: {e}") + + def set_render_mode(self, mode: RenderMode) -> None: """Change the current render mode of the simulation. Please see :class:`RenderMode` for more information on the different render modes. @@ -375,9 +541,12 @@ def set_render_mode(self, mode: RenderMode): Raises: ValueError: If the input mode is not supported. """ + if self._sim is None: + return + # check if mode change is possible -- not possible when no GUI is available if not self._sim.carb_settings.get("/isaaclab/has_gui"): - self._sim.logger.warning( + logger.warning( f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." ) return @@ -404,71 +573,15 @@ def set_render_mode(self, mode: RenderMode): # update render mode self.render_mode = mode - # ------------------------------------------------------------------ - # Camera - # ------------------------------------------------------------------ - - def set_camera_view( - self, - eye: tuple[float, float, float], - target: tuple[float, float, float], - camera_prim_path: str = "/OmniverseKit_Persp", - ): - """Set the location and target of the viewport camera in the stage. - - Note: - This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. - It is provided here for convenience to reduce the amount of imports needed. - - Args: - eye: The location of the camera eye. - target: The location of the camera target. - camera_prim_path: The path to the camera primitive in the stage. Defaults to - "/OmniverseKit_Persp". - """ - # safe call only if we have a GUI or viewport rendering enabled - if self._sim.carb_settings.get("/isaaclab/has_gui") or self._offscreen_render or self._render_viewport: - set_camera_view(eye, target, camera_prim_path) - - # ------------------------------------------------------------------ - # Rendering - # ------------------------------------------------------------------ - - def render(self, mode: RenderMode | None = None): - """Refreshes the rendering components including UI elements and view-ports depending on the render mode. - - This function is used to refresh the rendering components of the simulation. This includes updating the - view-ports, UI elements, and other extensions (besides physics simulation) that are running in the - background. The rendering components are refreshed based on the render mode. - - Please see :class:`RenderMode` for more information on the different render modes. - - Args: - mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. - """ - # check if we need to change the render mode - if mode is not None: - self.set_render_mode(mode) - # 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._sim.set_setting("/app/player/playSimulations", False) - self._app_iface.update() - self._sim.set_setting("/app/player/playSimulations", True) - - # app.update() may be changing the cuda device, so we force it back to our desired device here - if "cuda" in self._sim.device: - torch.cuda.set_device(self._sim.device) - - # ------------------------------------------------------------------ - # Lifecycle - # ------------------------------------------------------------------ - def reset(self, soft: bool = False) -> None: """Reset visualizer (timeline control + warmup renders on hard reset). Args: soft: If True, skip timeline reset and warmup. """ + if self._timeline is None: + return + if not soft: # disable app control on stop handle during reset self._timeline.set_stop_handle_enabled(False) @@ -478,7 +591,8 @@ def reset(self, soft: bool = False) -> None: # play the simulation self.play() # warmup renders to initialize replicator buffers - for _ in range(2): + warmup_count = self.cfg.warmup_renders if hasattr(self.cfg, "warmup_renders") else 2 + for _ in range(warmup_count): self.render() def close(self) -> None: @@ -486,6 +600,14 @@ def close(self) -> None: # Clean up timeline control if self._timeline is not None: self._timeline.close() + self._timeline = None + + self._sim = None + self._app_iface = None + self._viewport_context = None + self._viewport_window = None + self._is_initialized = False + self._is_closed = True # ------------------------------------------------------------------ # Private Methods @@ -513,6 +635,8 @@ def _on_timeline_stop(self) -> None: def _apply_render_settings_from_cfg(self): # noqa: C901 """Sets rtx settings specified in the RenderCfg.""" + if self._sim is None: + return # define mapping of user-friendly RenderCfg names to native carb names rendering_setting_name_mapping = { @@ -607,6 +731,9 @@ def _apply_render_settings_from_cfg(self): # noqa: C901 def _configure_rendering_dt(self): """Configures the rendering/timeline rate based on physics dt and render interval.""" + if self._sim is None or self._timeline is None: + return + from pxr import Usd cfg = self._sim.cfg @@ -637,7 +764,7 @@ def _configure_rendering_dt(self): _loop_runner.set_manual_step_size(cfg.dt * render_interval) _loop_runner.set_manual_mode(True) except Exception: - self._sim.logger.warning( + logger.warning( "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" ) carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) diff --git a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer_cfg.py new file mode 100644 index 00000000000..729e782dfbd --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer_cfg.py @@ -0,0 +1,83 @@ +# 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 + +"""Configuration for Omniverse visualizer in PhysX-based SimulationContext.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass +from .visualizer_cfg import VisualizerCfg + +from .physx_ov_visualizer import RenderMode + +if TYPE_CHECKING: + from .physx_ov_visualizer import PhysxOVVisualizer + + +@configclass +class PhysxOVVisualizerCfg(VisualizerCfg): + """Configuration for Omniverse visualizer in PhysX-based SimulationContext. + + This configuration extends :class:`VisualizerCfg` and is used by the + :class:`PhysxOVVisualizer` class which manages viewport/rendering for + PhysX-based SimulationContext workflows. + """ + + visualizer_type: str = "omniverse" + """Type identifier for Omniverse visualizer.""" + + default_render_mode: RenderMode | None = None + """Default render mode for the visualizer. + + If None, the render mode will be automatically determined based on GUI availability: + - NO_GUI_OR_RENDERING: When no GUI and offscreen rendering is disabled + - PARTIAL_RENDERING: When no GUI but offscreen rendering is enabled + - FULL_RENDERING: When GUI is available (local, livestreamed, or XR) + + See :class:`RenderMode` for available options. + """ + + render_throttle_period: int = 5 + """Throttle period for rendering updates. + + This controls how frequently UI elements are updated when in NO_RENDERING mode. + A higher value means less frequent UI updates, improving performance. + """ + + camera_prim_path: str = "/OmniverseKit_Persp" + """Path to the camera primitive in the USD stage.""" + + warmup_renders: int = 2 + """Number of warmup renders to perform on hard reset. + + This is used to initialize replicator buffers before the simulation starts. + """ + + viewport_name: str | None = "Viewport" + """Viewport name to use. If None, uses active viewport.""" + + create_viewport: bool = False + """Create new viewport with specified name and camera pose.""" + + dock_position: str = "SAME" + """Dock position for new viewport. Options: 'LEFT', 'RIGHT', 'BOTTOM', 'SAME' (tabs with existing).""" + + window_width: int = 1280 + """Viewport width in pixels.""" + + window_height: int = 720 + """Viewport height in pixels.""" + + def create_visualizer(self) -> "PhysxOVVisualizer": + """Create PhysxOVVisualizer instance from this config. + + Returns: + PhysxOVVisualizer instance configured with this config. + """ + from .physx_ov_visualizer import PhysxOVVisualizer + + return PhysxOVVisualizer(self) diff --git a/source/isaaclab/isaaclab/visualizers/visualizer.py b/source/isaaclab/isaaclab/visualizers/visualizer.py new file mode 100644 index 00000000000..46c1c299dd0 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/visualizer.py @@ -0,0 +1,86 @@ +# 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 + +"""Base class for visualizers.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from .visualizer_cfg import VisualizerCfg + + +class Visualizer(ABC): + """Base class for all visualizer backends. + + Lifecycle: __init__() -> initialize() -> step() (repeated) -> close() + """ + + def __init__(self, cfg: VisualizerCfg): + """Initialize visualizer with config.""" + self.cfg = cfg + self._is_initialized = False + self._is_closed = False + + @abstractmethod + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + """Initialize visualizer with scene data (model, state, usd_stage, etc.).""" + pass + + @abstractmethod + def step(self, dt: float, state: Any | None = None) -> None: + """Update visualization for one step. + + Args: + dt: Time step in seconds. + state: Updated physics state (e.g., newton.State). + """ + pass + + @abstractmethod + def close(self) -> None: + """Clean up resources.""" + pass + + @abstractmethod + def is_running(self) -> bool: + """Check if visualizer is still running (e.g., window not closed).""" + pass + + def is_training_paused(self) -> bool: + """Check if training is paused by visualizer controls.""" + return False + + def is_rendering_paused(self) -> bool: + """Check if rendering is paused by visualizer controls.""" + return False + + @property + def is_initialized(self) -> bool: + """Check if initialize() has been called.""" + return self._is_initialized + + @property + def is_closed(self) -> bool: + """Check if close() has been called.""" + return self._is_closed + + def supports_markers(self) -> bool: + """Check if visualizer supports VisualizationMarkers.""" + return False + + def supports_live_plots(self) -> bool: + """Check if visualizer supports LivePlots.""" + return False + + def get_rendering_dt(self) -> float | None: + """Get rendering time step. Returns None to use interface default.""" + return None + + def set_camera_view(self, eye: tuple, target: tuple) -> None: + """Set camera view position. No-op by default.""" + pass diff --git a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py new file mode 100644 index 00000000000..370d1ad94e1 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py @@ -0,0 +1,82 @@ +# 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 + +"""Base configuration for visualizers.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass + +if TYPE_CHECKING: + from .visualizer import Visualizer + + +@configclass +class VisualizerCfg: + """Base configuration for all visualizer backends. + + Note: + This is an abstract base class and should not be instantiated directly. + Use specific visualizer configs like NewtonVisualizerCfg, RerunVisualizerCfg, or OVVisualizerCfg. + """ + + visualizer_type: str | None = None + """Type identifier (e.g., 'newton', 'rerun', 'omniverse'). Must be overridden by subclasses.""" + + # Note: Partial environment visualization will come later + # env_ids: list[Integer] = [] + + enable_markers: bool = True + """Enable visualization markers (debug drawing).""" + + enable_live_plots: bool = True + """Enable live plotting of data. + + When set to True for OVVisualizer: + - Automatically checks the checkboxes for all manager visualizers (Actions, Observations, Rewards, etc.) + - Keeps the plot frames expanded by default (not collapsed) + - Makes the live plots visible immediately in the IsaacLab window (docked to the right of the viewport) + + This provides a better out-of-the-box experience when you want to monitor training metrics. + """ + + camera_position: tuple[float, float, float] = (8.0, 8.0, 3.0) + """Initial camera position (x, y, z) in world coordinates.""" + + camera_target: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Initial camera target/look-at point (x, y, z) in world coordinates.""" + + def get_visualizer_type(self) -> str | None: + """Get the visualizer type identifier. + + Returns: + The visualizer type string, or None if not set (base class). + """ + return self.visualizer_type + + def create_visualizer(self) -> Visualizer: + """Create visualizer instance from this config using factory pattern. + + Raises: + ValueError: If visualizer_type is None (base class used directly) or not registered. + """ + from . import get_visualizer_class + + if self.visualizer_type is None: + raise ValueError( + "Cannot create visualizer from base VisualizerCfg class. " + "Use a specific visualizer config: NewtonVisualizerCfg, RerunVisualizerCfg, or OVVisualizerCfg." + ) + + visualizer_class = get_visualizer_class(self.visualizer_type) + if visualizer_class is None: + raise ValueError( + f"Visualizer type '{self.visualizer_type}' is not registered. " + "Valid types: 'newton', 'rerun', 'omniverse'." + ) + + return visualizer_class(self) From 8e24e522cf484ebfaf94ecd4193cef3a0c3631cc Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 05:23:44 -0800 Subject: [PATCH 119/130] refactor step 14: refactored render mode out --- .../isaaclab/isaaclab/envs/direct_marl_env.py | 6 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 6 +- .../isaaclab/envs/manager_based_env.py | 2 +- .../isaaclab/envs/manager_based_rl_env.py | 4 +- .../isaaclab/envs/ui/base_env_window.py | 10 ++- .../isaaclab/sim/physx_ov_visualizer_cfg.py | 83 +++++++++++++++++++ .../isaaclab/sim/visualizer_interface.py | 32 +------ .../visualizers/physx_ov_visualizer.py | 9 +- .../test/envs/test_env_rendering_logic.py | 2 +- .../test/sim/test_simulation_context.py | 3 +- 10 files changed, 109 insertions(+), 48 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 807cedd0ee2..41c58ee390b 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -131,7 +131,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # viewport is not available in other rendering modes so the function will throw a warning # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for # non-rendering modes. - if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + if self.sim.carb_settings.get("/isaaclab/has_gui"): self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) else: self.viewport_camera_controller = None @@ -495,10 +495,10 @@ def render(self, recompute: bool = False) -> np.ndarray | None: return None elif self.render_mode == "rgb_array": # check that if any render could have happened - if self.sim.render_mode.value < self.sim.RenderMode.PARTIAL_RENDERING.value: + if not self.sim.carb_settings.get("/isaaclab/has_gui") and not bool(self.sim.carb_settings.get("/isaaclab/render/offscreen")): raise RuntimeError( f"Cannot render '{self.render_mode}' when the simulation render mode is" - f" '{self.sim.render_mode.name}'. Please set the simulation render mode to:" + f" is headless and offscreen rendering is disabled. Please set the simulation render mode to:" f"'{self.sim.RenderMode.PARTIAL_RENDERING.name}' or '{self.sim.RenderMode.FULL_RENDERING.name}'." " If running headless, make sure --enable_cameras is set." ) diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 6947df280bb..e6590e33dd8 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -138,7 +138,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # viewport is not available in other rendering modes so the function will throw a warning # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for # non-rendering modes. - if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + if self.sim.carb_settings.get("/isaaclab/has_gui"): self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) else: self.viewport_camera_controller = None @@ -464,10 +464,10 @@ def render(self, recompute: bool = False) -> np.ndarray | None: return None elif self.render_mode == "rgb_array": # check that if any render could have happened - if self.sim.render_mode.value < self.sim.RenderMode.PARTIAL_RENDERING.value: + if not self.sim.carb_settings.get("/isaaclab/has_gui") and not bool(self.sim.carb_settings.get("/isaaclab/render/offscreen")): raise RuntimeError( f"Cannot render '{self.render_mode}' when the simulation render mode is" - f" '{self.sim.render_mode.name}'. Please set the simulation render mode to:" + f" is headless and offscreen rendering is disabled. Please set the simulation render mode to:" f"'{self.sim.RenderMode.PARTIAL_RENDERING.name}' or '{self.sim.RenderMode.FULL_RENDERING.name}'." " If running headless, make sure --enable_cameras is set." ) diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 5f905ca0c76..85a4d7fafa0 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -140,7 +140,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # viewport is not available in other rendering modes so the function will throw a warning # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for # non-rendering modes. - if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + if self.sim.carb_settings.get("/isaaclab/has_gui"): self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) else: self.viewport_camera_controller = None diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py index 22bfdfc21dc..5fee4871f00 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py @@ -269,10 +269,10 @@ def render(self, recompute: bool = False) -> np.ndarray | None: return None elif self.render_mode == "rgb_array": # check that if any render could have happened - if self.sim.render_mode.value < self.sim.RenderMode.PARTIAL_RENDERING.value: + if not self.sim.carb_settings.get("/isaaclab/has_gui") and not bool(self.sim.carb_settings.get("/isaaclab/render/offscreen")): raise RuntimeError( f"Cannot render '{self.render_mode}' when the simulation render mode is" - f" '{self.sim.render_mode.name}'. Please set the simulation render mode to:" + f" is headless and offscreen rendering is disabled. Please set the simulation render mode to:" f"'{self.sim.RenderMode.PARTIAL_RENDERING.name}' or '{self.sim.RenderMode.FULL_RENDERING.name}'." " If running headless, make sure --enable_cameras is set." ) diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 3958dcf4959..333bd61c37e 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -122,12 +122,20 @@ def _build_sim_frame(self): with self.ui_window_elements["sim_frame"]: # create stack for controls self.ui_window_elements["sim_vstack"] = omni.ui.VStack(spacing=5, height=0) + if not self.env.sim.carb_settings.get("/isaaclab/has_gui"): + if not self.env.sim.carb_settings.get_as_bool("/isaaclab/render/offscreen"): + render_value = self.env.sim.RenderMode.NO_GUI_OR_RENDERING + else: + render_value = self.env.sim.RenderMode.PARTIAL_RENDERING + else: + render_value = self.env.sim.RenderMode.FULL_RENDERING + with self.ui_window_elements["sim_vstack"]: # create rendering mode dropdown render_mode_cfg = { "label": "Rendering Mode", "type": "dropdown", - "default_val": self.env.sim.render_mode.value, + "default_val": render_value, "items": [member.name for member in self.env.sim.RenderMode if member.value >= 0], "tooltip": "Select a rendering mode\n" + self.env.sim.RenderMode.__doc__, "on_clicked_fn": lambda value: self.env.sim.set_render_mode(self.env.sim.RenderMode[value]), diff --git a/source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py b/source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py new file mode 100644 index 00000000000..7243118e0e5 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py @@ -0,0 +1,83 @@ +# 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 + +"""Configuration for Omniverse visualizer in PhysX-based SimulationContext.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass +from isaaclab.visualizers import VisualizerCfg + +from .physx_ov_visualizer import RenderMode + +if TYPE_CHECKING: + from .physx_ov_visualizer import PhysxOVVisualizer + + +@configclass +class PhysxOVVisualizerCfg(VisualizerCfg): + """Configuration for Omniverse visualizer in PhysX-based SimulationContext. + + This configuration extends :class:`VisualizerCfg` and is used by the + :class:`PhysxOVVisualizer` class which manages viewport/rendering for + PhysX-based SimulationContext workflows. + """ + + visualizer_type: str = "omniverse" + """Type identifier for Omniverse visualizer.""" + + default_render_mode: RenderMode | None = None + """Default render mode for the visualizer. + + If None, the render mode will be automatically determined based on GUI availability: + - NO_GUI_OR_RENDERING: When no GUI and offscreen rendering is disabled + - PARTIAL_RENDERING: When no GUI but offscreen rendering is enabled + - FULL_RENDERING: When GUI is available (local, livestreamed, or XR) + + See :class:`RenderMode` for available options. + """ + + render_throttle_period: int = 5 + """Throttle period for rendering updates. + + This controls how frequently UI elements are updated when in NO_RENDERING mode. + A higher value means less frequent UI updates, improving performance. + """ + + camera_prim_path: str = "/OmniverseKit_Persp" + """Path to the camera primitive in the USD stage.""" + + warmup_renders: int = 2 + """Number of warmup renders to perform on hard reset. + + This is used to initialize replicator buffers before the simulation starts. + """ + + viewport_name: str | None = "Viewport" + """Viewport name to use. If None, uses active viewport.""" + + create_viewport: bool = False + """Create new viewport with specified name and camera pose.""" + + dock_position: str = "SAME" + """Dock position for new viewport. Options: 'LEFT', 'RIGHT', 'BOTTOM', 'SAME' (tabs with existing).""" + + window_width: int = 1280 + """Viewport width in pixels.""" + + window_height: int = 720 + """Viewport height in pixels.""" + + def create_visualizer(self) -> "PhysxOVVisualizer": + """Create PhysxOVVisualizer instance from this config. + + Returns: + PhysxOVVisualizer instance configured with this config. + """ + from .physx_ov_visualizer import PhysxOVVisualizer + + return PhysxOVVisualizer(self) diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index 01bf6d4ae96..ab7cf338398 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -67,14 +67,6 @@ def visualizers(self) -> list[Visualizer]: def scene_data_provider(self) -> SceneDataProvider | None: return self._scene_data_provider - @property - def render_mode(self) -> RenderMode: - """Current render mode from the first PhysxOVVisualizer.""" - for viz in self._visualizers: - if isinstance(viz, PhysxOVVisualizer): - return viz.render_mode - return RenderMode.NO_GUI_OR_RENDERING - # -- Visualizer Initialization -- def _create_default_visualizer_configs(self, requested: list[str]) -> list: @@ -142,15 +134,13 @@ def _build_scene_data(self, cfg) -> dict: def is_playing(self) -> bool: """Check whether the simulation is playing.""" for viz in self.visualizers: - if viz.is_playing(): - return True - return False + return viz.is_running() + return True # physics is always playing when there is no visualizer, basically headless mode def is_stopped(self) -> bool: """Check whether the simulation is stopped.""" for viz in self.visualizers: - if viz.is_stopped(): - return True + return not viz.is_running() return False def forward(self) -> None: @@ -165,19 +155,6 @@ def forward(self) -> None: if not self._visualizers: return - def set_render_mode(self, mode: RenderMode): - """Change the current render mode of the simulation. - - Please see :class:`RenderMode` for more information on the different render modes. - - Args: - mode: The rendering mode. If different than current rendering mode, - the mode is changed to the new mode. - """ - for viz in self._visualizers: - if isinstance(viz, PhysxOVVisualizer): - viz.set_render_mode(mode) - def step(self, render: bool = True) -> None: """Step visualizers and optionally render. @@ -205,8 +182,7 @@ def step(self, render: bool = True) -> None: def reset(self, soft: bool) -> None: """Reset visualizers (warmup renders on hard reset).""" for viz in self._visualizers: - if isinstance(viz, PhysxOVVisualizer): - viz.reset(soft) + viz.reset(soft) def close(self) -> None: """Close all visualizers and clean up.""" diff --git a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py index be6857a09cc..cd50e370dd0 100644 --- a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py @@ -19,7 +19,7 @@ import omni.kit.app import omni.timeline from isaaclab.utils.version import get_isaac_sim_version -from .visualizers import Visualizer +from .visualizer import Visualizer if TYPE_CHECKING: import carb @@ -447,12 +447,7 @@ def stop(self) -> None: def is_running(self) -> bool: """Check if visualizer is still running.""" - if self._app_iface is None: - return False - try: - return self._app_iface.is_running() - except Exception: - return False + return self.is_playing() def is_training_paused(self) -> bool: """Check if training is paused (always False for OV).""" diff --git a/source/isaaclab/test/envs/test_env_rendering_logic.py b/source/isaaclab/test/envs/test_env_rendering_logic.py index a70ea672db6..f37ee535273 100644 --- a/source/isaaclab/test/envs/test_env_rendering_logic.py +++ b/source/isaaclab/test/envs/test_env_rendering_logic.py @@ -174,7 +174,7 @@ def test_env_rendering_logic(env_type, render_interval, physics_callback, render # check that we are in partial rendering mode for the environment # this is enabled due to app launcher setting "enable_cameras=True" - assert env.sim.render_mode == SimulationContext.RenderMode.PARTIAL_RENDERING + assert env.sim.carb_settings.get("/isaaclab/has_gui") and bool(env.sim.carb_settings.get("/isaaclab/render/offscreen")) # add physics and render callbacks env.sim.add_physics_callback("physics_step", physics_cb) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index c070279dd9c..82c9e2b0157 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -144,8 +144,7 @@ def test_headless_mode(): """Test that render mode is headless since we are running in headless mode.""" sim = SimulationContext() # check default render mode - assert sim.render_mode == sim.RenderMode.NO_GUI_OR_RENDERING - assert not sim.carb_settings.get("/isaaclab/has_gui") + assert not sim.carb_settings.get("/isaaclab/has_gui") and not bool(sim.carb_settings.get("/isaaclab/render/offscreen")) """ From 5315495abd2e8f895b72a18a65627179d3de9a6e Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 05:57:01 -0800 Subject: [PATCH 120/130] refactor step 15: refactored physics to lab.physics --- .../benchmarks/benchmark_view_comparison.py | 2 +- .../assets/articulation/articulation.py | 2 +- .../assets/articulation/articulation_data.py | 2 +- .../deformable_object/deformable_object.py | 2 +- .../assets/rigid_object/rigid_object.py | 2 +- .../rigid_object_collection.py | 2 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 2 +- .../isaaclab/envs/manager_based_env.py | 2 +- .../isaaclab/envs/ui/base_env_window.py | 17 +- .../{sim/_impl => physics}/__init__.py | 0 .../isaaclab/physics/newton_backend.py | 66 +++ .../isaaclab/physics/newton_manager.py | 435 ++++++++++++++++++ .../isaaclab/physics/newton_manager_cfg.py | 30 ++ .../{sim/_impl => physics}/physics_backend.py | 0 .../{sim/_impl => physics}/physx_backend.py | 2 +- .../{sim/_impl => physics}/physx_manager.py | 0 .../isaaclab/isaaclab/physics/solvers_cfg.py | 177 +++++++ .../sensors/contact_sensor/contact_sensor.py | 2 +- .../frame_transformer/frame_transformer.py | 2 +- source/isaaclab/isaaclab/sensors/imu/imu.py | 2 +- .../isaaclab/sensors/ray_caster/ray_caster.py | 2 +- source/isaaclab/isaaclab/sim/__init__.py | 2 +- .../isaaclab/sim/physics_interface.py | 4 +- .../isaaclab/sim/simulation_context.py | 30 +- source/isaaclab/isaaclab/sim/utils/stage.py | 2 +- .../isaaclab/sim/visualizer_interface.py | 1 - .../visualizers/physx_ov_visualizer.py | 4 +- 27 files changed, 740 insertions(+), 54 deletions(-) rename source/isaaclab/isaaclab/{sim/_impl => physics}/__init__.py (100%) create mode 100644 source/isaaclab/isaaclab/physics/newton_backend.py create mode 100644 source/isaaclab/isaaclab/physics/newton_manager.py create mode 100644 source/isaaclab/isaaclab/physics/newton_manager_cfg.py rename source/isaaclab/isaaclab/{sim/_impl => physics}/physics_backend.py (100%) rename source/isaaclab/isaaclab/{sim/_impl => physics}/physx_backend.py (99%) rename source/isaaclab/isaaclab/{sim/_impl => physics}/physx_manager.py (100%) create mode 100644 source/isaaclab/isaaclab/physics/solvers_cfg.py diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index dd93aac035e..6db3b540190 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -67,7 +67,7 @@ import time import torch -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.sim.views import XformPrimView diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index 71ef003e5cc..f4da9e3b3ad 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -15,7 +15,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import PhysxSchema, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index 75f3096e856..9707c15d192 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -8,7 +8,7 @@ import weakref import omni.physics.tensors.impl.api as physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager import isaaclab.utils.math as math_utils from isaaclab.utils.buffers import TimestampedBuffer diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py index 5ff3cd9a8f4..84a504ada14 100644 --- a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import PhysxSchema, UsdShade import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py index 96d314f6fe0..e505f17ca29 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py index e92f0762c27..75d483276b1 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py @@ -12,7 +12,7 @@ from typing import TYPE_CHECKING import omni.physics.tensors.impl.api as physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index e6590e33dd8..9e7e0280bd4 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -20,7 +20,7 @@ import omni.kit.app import omni.physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from isaaclab.managers import EventManager from isaaclab.scene import InteractiveScene diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 85a4d7fafa0..95c5e0d3ffa 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -10,7 +10,7 @@ from typing import Any import omni.physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager from isaaclab.scene import InteractiveScene diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 333bd61c37e..0ca18b50143 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -19,6 +19,7 @@ from isaaclab.sim.utils.stage import get_current_stage from isaaclab.ui.widgets import ManagerLiveVisualizer +from isaaclab.visualizers.ov_visualizer import RenderMode if TYPE_CHECKING: import omni.ui @@ -124,11 +125,15 @@ def _build_sim_frame(self): self.ui_window_elements["sim_vstack"] = omni.ui.VStack(spacing=5, height=0) if not self.env.sim.carb_settings.get("/isaaclab/has_gui"): if not self.env.sim.carb_settings.get_as_bool("/isaaclab/render/offscreen"): - render_value = self.env.sim.RenderMode.NO_GUI_OR_RENDERING + render_value = RenderMode.NO_GUI_OR_RENDERING else: - render_value = self.env.sim.RenderMode.PARTIAL_RENDERING + render_value = RenderMode.PARTIAL_RENDERING else: - render_value = self.env.sim.RenderMode.FULL_RENDERING + render_value = RenderMode.FULL_RENDERING + + for visualizer in self.env.sim._visualizer._visualizers: + if hasattr(visualizer, "set_render_mode"): + set_render_mode_fn = lambda value: visualizer.set_render_mode(RenderMode[value]) with self.ui_window_elements["sim_vstack"]: # create rendering mode dropdown @@ -136,9 +141,9 @@ def _build_sim_frame(self): "label": "Rendering Mode", "type": "dropdown", "default_val": render_value, - "items": [member.name for member in self.env.sim.RenderMode if member.value >= 0], - "tooltip": "Select a rendering mode\n" + self.env.sim.RenderMode.__doc__, - "on_clicked_fn": lambda value: self.env.sim.set_render_mode(self.env.sim.RenderMode[value]), + "items": [member.name for member in RenderMode if member.value >= 0], + "tooltip": "Select a rendering mode\n" + RenderMode.__doc__, + "on_clicked_fn": lambda value: set_render_mode_fn(RenderMode[value]), } self.ui_window_elements["render_dropdown"] = isaacsim.gui.components.ui_utils.dropdown_builder( **render_mode_cfg diff --git a/source/isaaclab/isaaclab/sim/_impl/__init__.py b/source/isaaclab/isaaclab/physics/__init__.py similarity index 100% rename from source/isaaclab/isaaclab/sim/_impl/__init__.py rename to source/isaaclab/isaaclab/physics/__init__.py diff --git a/source/isaaclab/isaaclab/physics/newton_backend.py b/source/isaaclab/isaaclab/physics/newton_backend.py new file mode 100644 index 00000000000..3b5538b6012 --- /dev/null +++ b/source/isaaclab/isaaclab/physics/newton_backend.py @@ -0,0 +1,66 @@ +# 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 + +"""Newton physics backend implementation.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from .newton_manager import NewtonManager +from .physics_backend import PhysicsBackend + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext + + +class NewtonBackend(PhysicsBackend): + """Newton physics backend wrapping NewtonManager.""" + + def __init__(self, sim_context: "SimulationContext"): + """Initialize and configure Newton backend. + + Args: + sim_context: Parent simulation context. + """ + super().__init__(sim_context) + cfg = self._sim.cfg + + # Set simulation parameters + NewtonManager.set_simulation_dt(cfg.dt) + NewtonManager._gravity_vector = cfg.gravity + + # Extract and apply solver settings from config + to_dict = getattr(cfg, "to_dict", None) + params = to_dict() if callable(to_dict) else {} + newton_cfg = dict(params.get("newton_cfg", {})) if isinstance(params, dict) else {} + NewtonManager.set_solver_settings(newton_cfg) + + # USD fabric sync only needed for OV rendering + NewtonManager._clone_physics_only = "omniverse" not in self._sim._visualizer_interface._visualizers_str + + def reset(self, soft: bool = False) -> None: + """Reset physics simulation. + + Args: + soft: If True, skip full reinitialization. + """ + if not soft: + NewtonManager.start_simulation() + NewtonManager.initialize_solver() + + def forward(self) -> None: + """Update articulation kinematics without stepping physics.""" + NewtonManager.forward_kinematics() + + def step(self) -> None: + """Step physics simulation.""" + if self._sim.is_playing(): + NewtonManager.step() + + def close(self) -> None: + """Clean up Newton physics resources.""" + NewtonManager.clear() + self._initialized = False diff --git a/source/isaaclab/isaaclab/physics/newton_manager.py b/source/isaaclab/isaaclab/physics/newton_manager.py new file mode 100644 index 00000000000..83496da575e --- /dev/null +++ b/source/isaaclab/isaaclab/physics/newton_manager.py @@ -0,0 +1,435 @@ +# 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 numpy as np +import re + +import warp as wp +from newton import Axis, Contacts, Control, Model, ModelBuilder, State, eval_fk +from newton.examples import create_collision_pipeline +from newton.sensors import ContactSensor as NewtonContactSensor +from newton.sensors import populate_contacts +from newton.solvers import SolverBase, SolverFeatherstone, SolverMuJoCo, SolverNotifyFlags, SolverXPBD + +from isaaclab.physics.newton_manager_cfg import NewtonCfg +from isaaclab.sim.utils.stage import get_current_stage +from isaaclab.utils.timer import Timer + + +def flipped_match(x: str, y: str) -> re.Match | None: + """Flipped match function. + + This function is used to match the contact partners' body/shape names with the body/shape names in the simulation. + + Args: + x: The body/shape name in the simulation. + y: The body/shape name in the contact view. + + Returns: + The match object if the body/shape name is found in the contact view, otherwise None. + """ + return re.match(y, x) + + +class NewtonManager: + _builder: ModelBuilder = None + _model: Model = None + _device: str = "cuda:0" + _dt: float = 1.0 / 200.0 + _solver_dt: float = 1.0 / 200.0 + _num_substeps: int = 1 + _solver = None + _state_0: State = None + _state_1: State = None + _state_temp: State = None + _control: Control = None + _on_init_callbacks: list = [] + _on_start_callbacks: list = [] + _contacts: Contacts = None + _needs_collision_pipeline: bool = False + _collision_pipeline = None + _newton_contact_sensor: NewtonContactSensor = None # TODO: allow several contact sensors + _report_contacts: bool = False + _graph = None + _newton_stage_path = None + _sim_time = 0.0 + _usdrt_stage = None + _newton_index_attr = "newton:index" + _clone_physics_only = False + _cfg: NewtonCfg = NewtonCfg() + _solver_type: str = "mujoco_warp" + _gravity_vector: tuple[float, float, float] = (0.0, 0.0, -9.81) + _up_axis: str = "Z" + _num_envs: int = None + _model_changes: set[int] = set() + + @classmethod + def clear(cls): + NewtonManager._builder = None + NewtonManager._model = None + NewtonManager._solver = None + NewtonManager._state_0 = None + NewtonManager._state_1 = None + NewtonManager._state_temp = None + NewtonManager._control = None + NewtonManager._contacts = None + NewtonManager._needs_collision_pipeline = False + NewtonManager._collision_pipeline = None + NewtonManager._newton_contact_sensor = None + NewtonManager._report_contacts = False + NewtonManager._graph = None + NewtonManager._newton_stage_path = None + NewtonManager._sim_time = 0.0 + NewtonManager._on_init_callbacks = [] + NewtonManager._on_start_callbacks = [] + NewtonManager._usdrt_stage = None + # Only create new config if not during Python shutdown + try: + NewtonManager._cfg = NewtonCfg() + except (ImportError, AttributeError, TypeError): + NewtonManager._cfg = None + NewtonManager._up_axis = "Z" + NewtonManager._first_call = True + NewtonManager._model_changes = set() + + @classmethod + def set_builder(cls, builder): + NewtonManager._builder = builder + + @classmethod + def add_on_init_callback(cls, callback) -> None: + NewtonManager._on_init_callbacks.append(callback) + + @classmethod + def add_on_start_callback(cls, callback) -> None: + NewtonManager._on_start_callbacks.append(callback) + + @classmethod + def add_model_change(cls, change: SolverNotifyFlags) -> None: + NewtonManager._model_changes.add(change) + + @classmethod + def start_simulation(cls) -> None: + """Starts the simulation. + + This function finalizes the model and initializes the simulation state. + """ + + print(f"[INFO] Builder: {NewtonManager._builder}") + if NewtonManager._builder is None: + NewtonManager.instantiate_builder_from_stage() + print("[INFO] Running on init callbacks") + for callback in NewtonManager._on_init_callbacks: + callback() + print(f"[INFO] Finalizing model on device: {NewtonManager._device}") + NewtonManager._builder.gravity = np.array(NewtonManager._gravity_vector)[-1] + NewtonManager._builder.up_axis = Axis.from_string(NewtonManager._up_axis) + with Timer(name="newton_finalize_builder", msg="Finalize builder took:", enable=True, format="ms"): + NewtonManager._model = NewtonManager._builder.finalize(device=NewtonManager._device) + NewtonManager._model.num_envs = NewtonManager._num_envs + NewtonManager._state_0 = NewtonManager._model.state() + NewtonManager._state_1 = NewtonManager._model.state() + NewtonManager._state_temp = NewtonManager._model.state() + NewtonManager._control = NewtonManager._model.control() + NewtonManager.forward_kinematics() + if NewtonManager._needs_collision_pipeline: + NewtonManager._collision_pipeline = create_collision_pipeline(NewtonManager._model) + NewtonManager._contacts = NewtonManager._model.collide( + NewtonManager._state_0, collision_pipeline=NewtonManager._collision_pipeline + ) + else: + NewtonManager._contacts = Contacts(0, 0) + print("[INFO] Running on start callbacks") + for callback in NewtonManager._on_start_callbacks: + callback() + if not NewtonManager._clone_physics_only: + import usdrt + + NewtonManager._usdrt_stage = get_current_stage(fabric=True) + for i, prim_path in enumerate(NewtonManager._model.body_key): + prim = NewtonManager._usdrt_stage.GetPrimAtPath(prim_path) + prim.CreateAttribute(NewtonManager._newton_index_attr, usdrt.Sdf.ValueTypeNames.UInt, True) + prim.GetAttribute(NewtonManager._newton_index_attr).Set(i) + xformable_prim = usdrt.Rt.Xformable(prim) + if not xformable_prim.HasWorldXform(): + xformable_prim.SetWorldXformFromUsd() + + @classmethod + def instantiate_builder_from_stage(cls): + from pxr import UsdGeom + + stage = get_current_stage() + up_axis = UsdGeom.GetStageUpAxis(stage) + builder = ModelBuilder(up_axis=up_axis) + builder.add_usd(stage) + NewtonManager.set_builder(builder) + + @classmethod + def set_solver_settings(cls, newton_params: dict): + NewtonManager._cfg = NewtonCfg(**newton_params) + + @classmethod + def initialize_solver(cls): + """Initializes the solver. + + This function initializes the solver based on the specified solver type. Currently, only XPBD and MuJoCoWarp + are supported. The graphing of the simulation is performed in this function if the simulation is ran using + a CUDA enabled device. + + .. warning:: + When using a CUDA enabled device, the simulation will be graphed. This means that this function steps the + simulation once to capture the graph. Hence, this function should only be called after everything else in + the simulation is initialized. + """ + with Timer(name="newton_initialize_solver", msg="Initialize solver took:", enable=True, format="ms"): + NewtonManager._num_substeps = NewtonManager._cfg.num_substeps + NewtonManager._solver_dt = NewtonManager._dt / NewtonManager._num_substeps + print(NewtonManager._model.gravity) + NewtonManager._solver = NewtonManager._get_solver(NewtonManager._model, NewtonManager._cfg.solver_cfg) + if isinstance(NewtonManager._solver, SolverMuJoCo): + NewtonManager._needs_collision_pipeline = not NewtonManager._cfg.solver_cfg.get( + "use_mujoco_contacts", False + ) + else: + NewtonManager._needs_collision_pipeline = True + + # Ensure we are using a CUDA enabled device + assert NewtonManager._device.startswith("cuda"), "NewtonManager only supports CUDA enabled devices" + + # Capture the graph if CUDA is enabled + with Timer(name="newton_cuda_graph", msg="CUDA graph took:", enable=True, format="ms"): + if NewtonManager._cfg.use_cuda_graph: + with wp.ScopedCapture() as capture: + NewtonManager.simulate() + NewtonManager._graph = capture.graph + + @classmethod + def simulate(cls) -> None: + """Simulates the simulation. + + Performs one simulation step with the specified number of substeps. Depending on the solver type, this function + may need to explicitly compute the collisions. This function also aggregates the contacts and evaluates the + contact sensors. Finally, it performs the state swapping for Newton. + """ + state_0_dict = NewtonManager._state_0.__dict__ + state_1_dict = NewtonManager._state_1.__dict__ + state_temp_dict = NewtonManager._state_temp.__dict__ + contacts = None + + # MJWarp computes its own collisions. + if NewtonManager._needs_collision_pipeline: + contacts = NewtonManager._model.collide( + NewtonManager._state_0, collision_pipeline=NewtonManager._collision_pipeline + ) + + if NewtonManager._num_substeps % 2 == 0: + for i in range(NewtonManager._num_substeps): + NewtonManager._solver.step( + NewtonManager._state_0, + NewtonManager._state_1, + NewtonManager._control, + contacts, + NewtonManager._solver_dt, + ) + NewtonManager._state_0, NewtonManager._state_1 = NewtonManager._state_1, NewtonManager._state_0 + NewtonManager._state_0.clear_forces() + else: + for i in range(NewtonManager._num_substeps): + NewtonManager._solver.step( + NewtonManager._state_0, + NewtonManager._state_1, + NewtonManager._control, + contacts, + NewtonManager._solver_dt, + ) + + # FIXME: Ask Lukasz help to deal with non-even number of substeps. This should not be needed. + if i < NewtonManager._num_substeps - 1 or not NewtonManager._cfg.use_cuda_graph: + # we can just swap the state references + NewtonManager._state_0, NewtonManager._state_1 = NewtonManager._state_1, NewtonManager._state_0 + elif NewtonManager._cfg.use_cuda_graph: + # swap states by actually copying the state arrays to make sure the graph capture works + for key, value in state_0_dict.items(): + if isinstance(value, wp.array): + if key not in state_temp_dict: + state_temp_dict[key] = wp.empty_like(value) + state_temp_dict[key].assign(value) + state_0_dict[key].assign(state_1_dict[key]) + state_1_dict[key].assign(state_temp_dict[key]) + NewtonManager._state_0.clear_forces() + + if NewtonManager._report_contacts: + populate_contacts(NewtonManager._contacts, NewtonManager._solver) + NewtonManager._newton_contact_sensor.eval(NewtonManager._contacts) + + @classmethod + def set_device(cls, device: str) -> None: + """Sets the device to use for the Newton simulation. + + Args: + device (str): The device to use for the Newton simulation. + """ + NewtonManager._device = device + + @classmethod + def step(cls) -> None: + """Steps the simulation. + + This function steps the simulation by the specified time step in the simulation configuration. + """ + if NewtonManager._model_changes: + for change in NewtonManager._model_changes: + NewtonManager._solver.notify_model_changed(change) + NewtonManager._model_changes = set() + + if NewtonManager._cfg.use_cuda_graph: + wp.capture_launch(NewtonManager._graph) + else: + NewtonManager.simulate() + + if NewtonManager._cfg.debug_mode: + convergence_data = NewtonManager.get_solver_convergence_steps() + # print(f"solver niter: {convergence_data}") + if convergence_data["max"] == NewtonManager._solver.mjw_model.opt.iterations: + print("solver didn't converge!", convergence_data["max"]) + + NewtonManager._sim_time += NewtonManager._solver_dt * NewtonManager._num_substeps + + @classmethod + def get_solver_convergence_steps(cls) -> dict[str, float | int]: + niter = NewtonManager._solver.mjw_data.solver_niter.numpy() + max_niter = np.max(niter) + mean_niter = np.mean(niter) + min_niter = np.min(niter) + std_niter = np.std(niter) + return {"max": max_niter, "mean": mean_niter, "min": min_niter, "std": std_niter} + + @classmethod + def set_simulation_dt(cls, dt: float) -> None: + """Sets the simulation time step and the number of substeps. + + Args: + dt (float): The simulation time step. + """ + NewtonManager._dt = dt + + @classmethod + def get_model(cls): + return NewtonManager._model + + @classmethod + def get_state_0(cls): + return NewtonManager._state_0 + + @classmethod + def get_state_1(cls): + return NewtonManager._state_1 + + @classmethod + def get_control(cls): + return NewtonManager._control + + @classmethod + def get_dt(cls): + return NewtonManager._dt + + @classmethod + def get_solver_dt(cls): + return NewtonManager._solver_dt + + @classmethod + def forward_kinematics(cls, mask: wp.array | None = None) -> None: + """Evaluates the forward kinematics for the selected articulations. + + This function evaluates the forward kinematics for the selected articulations. + """ + eval_fk( + NewtonManager._model, + NewtonManager._state_0.joint_q, + NewtonManager._state_0.joint_qd, + NewtonManager._state_0, + None, + ) + + @classmethod + def _get_solver(cls, model: Model, solver_cfg: dict) -> SolverBase: + NewtonManager._solver_type = solver_cfg.pop("solver_type") + + if NewtonManager._solver_type == "mujoco_warp": + return SolverMuJoCo(model, **solver_cfg) + elif NewtonManager._solver_type == "xpbd": + return SolverXPBD(model, **solver_cfg) + elif NewtonManager._solver_type == "featherstone": + return SolverFeatherstone(model, **solver_cfg) + else: + raise ValueError(f"Invalid solver type: {NewtonManager._solver_type}") + + @classmethod + def add_contact_sensor( + cls, + body_names_expr: str | list[str] | None = None, + shape_names_expr: str | list[str] | None = None, + contact_partners_body_expr: str | list[str] | None = None, + contact_partners_shape_expr: str | list[str] | None = None, + prune_noncolliding: bool = False, + verbose: bool = False, + ): + """Adds a contact view. + + Adds a contact view to the simulation allowing to report contacts between the specified bodies/shapes and the + contact partners. As of now, only one body/shape name expression can be provided. Similarly, only one contact + partner body/shape expression can be provided. If no contact partner expression is provided, the contact view + will report contacts with all bodies/shapes. + + Note that we make an explicit difference between a body and a shape. A body is a rigid body, while a shape + is a collision shape. A body can have multiple shapes. The shape option allows a more fine-grained control + over the contact reporting. + + Args: + body_names_expr (str | None): The expression for the body names. + shape_names_expr (str | None): The expression for the shape names. + contact_partners_body_expr (str | None): The expression for the contact partners' body names. + contact_partners_shape_expr (str | None): The expression for the contact partners' shape names. + prune_noncolliding (bool): Make the force matrix sparse using the collision pairs in the model. + verbose (bool): Whether to print verbose information. + """ + if body_names_expr is None and shape_names_expr is None: + raise ValueError("At least one of body_names_expr or shape_names_expr must be provided") + if body_names_expr is not None and shape_names_expr is not None: + raise ValueError("Only one of body_names_expr or shape_names_expr must be provided") + if contact_partners_body_expr is not None and contact_partners_shape_expr is not None: + raise ValueError("Only one of contact_partners_body_expr or contact_partners_shape_expr must be provided") + if contact_partners_body_expr is None and contact_partners_shape_expr is None: + print(f"[INFO] Adding contact view for {body_names_expr}. It will report contacts with all bodies/shapes.") + else: + if body_names_expr is not None: + if contact_partners_body_expr is not None: + print(f"[INFO] Adding contact view for {body_names_expr} with filter {contact_partners_body_expr}.") + else: + print(f"[INFO] Adding contact view for {body_names_expr} with filter {shape_names_expr}.") + else: + if contact_partners_body_expr is not None: + print( + f"[INFO] Adding contact view for {shape_names_expr} with filter {contact_partners_body_expr}." + ) + else: + print( + f"[INFO] Adding contact view for {shape_names_expr} with filter {contact_partners_shape_expr}." + ) + + NewtonManager._newton_contact_sensor = NewtonContactSensor( + NewtonManager._model, + sensing_obj_bodies=body_names_expr, + sensing_obj_shapes=shape_names_expr, + counterpart_bodies=contact_partners_body_expr, + counterpart_shapes=contact_partners_shape_expr, + match_fn=flipped_match, + include_total=True, + prune_noncolliding=prune_noncolliding, + verbose=verbose, + ) + NewtonManager._report_contacts = True diff --git a/source/isaaclab/isaaclab/physics/newton_manager_cfg.py b/source/isaaclab/isaaclab/physics/newton_manager_cfg.py new file mode 100644 index 00000000000..c9c6dc99098 --- /dev/null +++ b/source/isaaclab/isaaclab/physics/newton_manager_cfg.py @@ -0,0 +1,30 @@ +# 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 isaaclab.utils import configclass + +from .solvers_cfg import MJWarpSolverCfg, NewtonSolverCfg + + +@configclass +class NewtonCfg: + """Configuration for Newton-related parameters. + + These parameters are used to configure the Newton physics simulation. + """ + + num_substeps: int = 1 + """Number of substeps to use for the solver.""" + + debug_mode: bool = False + """Whether to enable debug mode for the solver.""" + + use_cuda_graph: bool = True + """Whether to use CUDA graphing when simulating. + + If set to False, the simulation performance will be severely degraded. + """ + + solver_cfg: NewtonSolverCfg = MJWarpSolverCfg() diff --git a/source/isaaclab/isaaclab/sim/_impl/physics_backend.py b/source/isaaclab/isaaclab/physics/physics_backend.py similarity index 100% rename from source/isaaclab/isaaclab/sim/_impl/physics_backend.py rename to source/isaaclab/isaaclab/physics/physics_backend.py diff --git a/source/isaaclab/isaaclab/sim/_impl/physx_backend.py b/source/isaaclab/isaaclab/physics/physx_backend.py similarity index 99% rename from source/isaaclab/isaaclab/sim/_impl/physx_backend.py rename to source/isaaclab/isaaclab/physics/physx_backend.py index 1a814ae085c..eb3163b5248 100644 --- a/source/isaaclab/isaaclab/sim/_impl/physx_backend.py +++ b/source/isaaclab/isaaclab/physics/physx_backend.py @@ -23,7 +23,7 @@ from pxr import PhysxSchema, Sdf import isaaclab.sim as sim_utils -from isaaclab.sim._impl.physics_backend import PhysicsBackend +from .physics_backend import PhysicsBackend from .physx_manager import PhysxManager if TYPE_CHECKING: diff --git a/source/isaaclab/isaaclab/sim/_impl/physx_manager.py b/source/isaaclab/isaaclab/physics/physx_manager.py similarity index 100% rename from source/isaaclab/isaaclab/sim/_impl/physx_manager.py rename to source/isaaclab/isaaclab/physics/physx_manager.py diff --git a/source/isaaclab/isaaclab/physics/solvers_cfg.py b/source/isaaclab/isaaclab/physics/solvers_cfg.py new file mode 100644 index 00000000000..e9db3b2ee67 --- /dev/null +++ b/source/isaaclab/isaaclab/physics/solvers_cfg.py @@ -0,0 +1,177 @@ +# 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 isaaclab.utils import configclass + + +@configclass +class NewtonSolverCfg: + """Configuration for Newton solver-related parameters. + + These parameters are used to configure the Newton solver. For more information, see the `Newton documentation`_. + + .. _Newton documentation: https://newton.readthedocs.io/en/latest/ + """ + + solver_type: str = "None" + """Solver type. + + Used to select the right solver class. + """ + + +@configclass +class MJWarpSolverCfg(NewtonSolverCfg): + """Configuration for MuJoCo Warp solver-related parameters. + + These parameters are used to configure the MuJoCo Warp solver. For more information, see the + `MuJoCo Warp documentation`_. + + .. _MuJoCo Warp documentation: https://github.com/google-deepmind/mujoco_warp + """ + + solver_type: str = "mujoco_warp" + """Solver type. Can be "mujoco_warp".""" + + njmax: int = 300 + """Number of constraints per environment (world).""" + + nconmax: int | None = None + """Number of contact points per environment (world).""" + + iterations: int = 100 + """Number of solver iterations.""" + + ls_iterations: int = 50 + """Number of line search iterations for the solver.""" + + solver: str = "newton" + """Solver type. Can be "cg" or "newton", or their corresponding MuJoCo integer constants.""" + + integrator: str = "euler" + """Integrator type. Can be "euler", "rk4", or "implicit", or their corresponding MuJoCo integer constants.""" + + use_mujoco_cpu: bool = False + """Whether to use the pure MuJoCo backend instead of `mujoco_warp`.""" + + disable_contacts: bool = False + """Whether to disable contact computation in MuJoCo.""" + + default_actuator_gear: float | None = None + """Default gear ratio for all actuators.""" + + actuator_gears: dict[str, float] | None = None + """Dictionary mapping joint names to specific gear ratios, overriding the `default_actuator_gear`.""" + + update_data_interval: int = 1 + """Frequency (in simulation steps) at which to update the MuJoCo Data object from the Newton state. + + If 0, Data is never updated after initialization. + """ + + save_to_mjcf: str | None = None + """Optional path to save the generated MJCF model file. + + If None, the MJCF model is not saved. + """ + + impratio: float = 1.0 + """Frictional-to-normal constraint impedance ratio.""" + + cone: str = "pyramidal" + """The type of contact friction cone. Can be "pyramidal" or "elliptic".""" + + ls_parallel: bool = False + """Whether to use parallel line search.""" + + use_mujoco_contacts: bool = True + """Whether to use MuJoCo's contact solver.""" + + +@configclass +class XPBOSolverCfg(NewtonSolverCfg): + """An implicit integrator using eXtended Position-Based Dynamics (XPBD) for rigid and soft body simulation. + + References: + - Miles Macklin, Matthias Müller, and Nuttapong Chentanez. 2016. XPBD: position-based simulation of compliant + constrained dynamics. In Proceedings of the 9th International Conference on Motion in Games (MIG '16). + Association for Computing Machinery, New York, NY, USA, 49-54. https://doi.org/10.1145/2994258.2994272 + - Matthias Müller, Miles Macklin, Nuttapong Chentanez, Stefan Jeschke, and Tae-Yong Kim. 2020. Detailed rigid + body simulation with extended position based dynamics. In Proceedings of the ACM SIGGRAPH/Eurographics + Symposium on Computer Animation (SCA '20). Eurographics Association, Goslar, DEU, + Article 10, 1-12. https://doi.org/10.1111/cgf.14105 + + """ + + solver_type: str = "xpbd" + """Solver type. Can be "xpbd".""" + + iterations: int = 2 + """Number of solver iterations.""" + + soft_body_relaxation: float = 0.9 + """Relaxation parameter for soft body simulation.""" + + soft_contact_relaxation: float = 0.9 + """Relaxation parameter for soft contact simulation.""" + + joint_linear_relaxation: float = 0.7 + """Relaxation parameter for joint linear simulation.""" + + joint_angular_relaxation: float = 0.4 + """Relaxation parameter for joint angular simulation.""" + + joint_linear_compliance: float = 0.0 + """Compliance parameter for joint linear simulation.""" + + joint_angular_compliance: float = 0.0 + """Compliance parameter for joint angular simulation.""" + + rigid_contact_relaxation: float = 0.8 + """Relaxation parameter for rigid contact simulation.""" + + rigid_contact_con_weighting: bool = True + """Whether to use contact constraint weighting for rigid contact simulation.""" + + angular_damping: float = 0.0 + """Angular damping parameter for rigid contact simulation.""" + + enable_restitution: bool = False + """Whether to enable restitution for rigid contact simulation.""" + + +@configclass +class FeatherstoneSolverCfg(NewtonSolverCfg): + """A semi-implicit integrator using symplectic Euler. + + It operates on reduced (also called generalized) coordinates to simulate articulated rigid body dynamics + based on Featherstone's composite rigid body algorithm (CRBA). + + See: Featherstone, Roy. Rigid Body Dynamics Algorithms. Springer US, 2014. + + Semi-implicit time integration is a variational integrator that + preserves energy, however it not unconditionally stable, and requires a time-step + small enough to support the required stiffness and damping forces. + + See: https://en.wikipedia.org/wiki/Semi-implicit_Euler_method + """ + + solver_type: str = "featherstone" + """Solver type. Can be "featherstone".""" + + angular_damping: float = 0.05 + """Angular damping parameter for rigid contact simulation.""" + + update_mass_matrix_interval: int = 1 + """Frequency (in simulation steps) at which to update the mass matrix.""" + + friction_smoothing: float = 1.0 + """Friction smoothing parameter.""" + + use_tile_gemm: bool = False + """Whether to use tile-based GEMM for the mass matrix.""" + + fuse_cholesky: bool = True + """Whether to fuse the Cholesky decomposition.""" diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 4d808934aa1..dc4081b25ad 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -14,7 +14,7 @@ import carb import omni.physics.tensors.impl.api as physx -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import PhysxSchema import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index 455345e56a9..9ec17337a06 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -11,7 +11,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/imu/imu.py b/source/isaaclab/isaaclab/sensors/imu/imu.py index 09767d9f099..fdc9a9a8452 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu.py @@ -9,7 +9,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 3de85177f5b..5bf4ca99af7 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -14,7 +14,7 @@ from typing import TYPE_CHECKING, ClassVar import omni -from isaaclab.sim._impl.physx_manager import PhysxManager +from isaaclab.physics.physx_manager import PhysxManager from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sim/__init__.py b/source/isaaclab/isaaclab/sim/__init__.py index 3130b187bb8..16b32df44de 100644 --- a/source/isaaclab/isaaclab/sim/__init__.py +++ b/source/isaaclab/isaaclab/sim/__init__.py @@ -30,7 +30,7 @@ from .schemas import * # noqa: F401, F403 from .simulation_cfg import PhysxCfg, RenderCfg, SimulationCfg # noqa: F401, F403 from .simulation_context import SimulationContext, build_simulation_context # noqa: F401, F403 -from ._impl.physx_manager import PhysxManager, IsaacEvents # noqa: F401, F403 +from isaaclab.physics.physx_manager import PhysxManager, IsaacEvents # noqa: F401, F403 from .spawners import * # noqa: F401, F403 from .utils import * # noqa: F401, F403 from .views import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sim/physics_interface.py b/source/isaaclab/isaaclab/sim/physics_interface.py index 98268ec0b67..a9e76b4299d 100644 --- a/source/isaaclab/isaaclab/sim/physics_interface.py +++ b/source/isaaclab/isaaclab/sim/physics_interface.py @@ -14,8 +14,8 @@ from pxr import Gf, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils -from ._impl.physx_backend import PhysXBackend -from ._impl.physics_backend import PhysicsBackend +from isaaclab.physics.physx_backend import PhysXBackend +from isaaclab.physics.physics_backend import PhysicsBackend from .interface import Interface if TYPE_CHECKING: diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index af1cc534822..906f6ea872b 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -24,7 +24,7 @@ from .physics_interface import PhysicsInterface from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg -from .visualizer_interface import RenderMode, VisualizerInterface +from .visualizer_interface import VisualizerInterface logger = logging.getLogger(__name__) @@ -76,9 +76,6 @@ class SimulationContext: _is_initialized: ClassVar[bool] = False """Whether the simulation context is initialized.""" - # Expose RenderMode as class attribute for backwards compatibility - RenderMode = RenderMode - def __init__(self, cfg: SimulationCfg | None = None): """Creates a simulation context to control the simulator. @@ -200,11 +197,6 @@ def device(self) -> str: """ return self._physics_interface.device - @property - def render_mode(self) -> RenderMode: - """Current render mode.""" - return self._visualizer.render_mode - """ Operations - Simulation Information. """ @@ -277,25 +269,6 @@ def set_camera_view( """ self._visualizer.set_camera_view(eye, target) - def set_render_mode(self, mode: RenderMode): - """Change the current render mode of the simulation. - - Please see :class:`RenderMode` for more information on the different render modes. - - .. note:: - When no GUI is available (locally or livestreamed), we do not need to choose whether the viewport - needs to render or not (since there is no GUI). Thus, in this case, calling the function will not - change the render mode. - - Args: - 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. - """ - self._visualizer.set_render_mode(mode) - def set_setting(self, name: str, value: Any): """Set simulation settings using the Carbonite SDK. @@ -467,6 +440,7 @@ def _check_for_callback_exceptions(self): builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore raise exception_to_raise + @contextmanager def build_simulation_context( create_new_stage: bool = True, diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index f805e474670..ce7fa6b9ca5 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -435,7 +435,7 @@ def attach_stage_to_usd_context(attaching_early: bool = False): import carb import omni.physx import omni.usd - from isaaclab.sim._impl.physx_manager import PhysxManager + from isaaclab.physics.physx_manager import PhysxManager from isaaclab.sim.simulation_context import SimulationContext diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index ab7cf338398..9fab9486c41 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -13,7 +13,6 @@ from isaaclab.visualizers import Visualizer from .interface import Interface -from isaaclab.visualizers.physx_ov_visualizer import PhysxOVVisualizer, RenderMode from isaaclab.visualizers.physx_ov_visualizer_cfg import PhysxOVVisualizerCfg if TYPE_CHECKING: diff --git a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py index cd50e370dd0..cf68bbd6feb 100644 --- a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py @@ -292,11 +292,11 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: # Store the default render mode if self.cfg.default_render_mode is not None: self.render_mode = self.cfg.default_render_mode - elif not self._sim.carb_settings.get("/isaaclab/has_gui") and not self._offscreen_render: + elif not has_gui and not self._offscreen_render: # set default render mode # note: this is the terminal state: cannot exit from this render mode self.render_mode = RenderMode.NO_GUI_OR_RENDERING - elif not self._sim.carb_settings.get("/isaaclab/has_gui") and self._offscreen_render: + elif not has_gui and self._offscreen_render: # set default render mode # note: this is the terminal state: cannot exit from this render mode self.render_mode = RenderMode.PARTIAL_RENDERING From 6be6ac8ebef05d35f63776eb84f80298c072a5f6 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 06:21:34 -0800 Subject: [PATCH 121/130] refactor step 16: refactored physics_manager --- source/isaaclab/isaaclab/physics/__init__.py | 10 +- .../isaaclab/physics/physics_backend.py | 53 -- .../isaaclab/physics/physics_manager.py | 86 +++ .../isaaclab/physics/physics_manager_cfg.py | 35 ++ .../isaaclab/physics/physx_backend.py | 509 ----------------- .../isaaclab/physics/physx_manager.py | 519 +++++++++++++++++- .../isaaclab/physics/physx_manager_cfg.py | 215 ++++++++ source/isaaclab/isaaclab/sim/__init__.py | 2 +- .../isaaclab/sim/physics_interface.py | 57 +- .../isaaclab/isaaclab/sim/simulation_cfg.py | 191 +------ .../manager_based/classic/ant/ant_env_cfg.py | 2 +- .../classic/humanoid/humanoid_env_cfg.py | 2 +- .../locomotion/velocity/velocity_env_cfg.py | 2 +- .../manipulation/cabinet/cabinet_env_cfg.py | 6 +- .../config/openarm/cabinet_openarm_env_cfg.py | 4 +- .../manipulation/dexsuite/dexsuite_env_cfg.py | 6 +- .../config/openarm/lift_openarm_env_cfg.py | 8 +- .../manipulation/lift/lift_env_cfg.py | 10 +- .../agibot/place_toy2box_rmp_rel_env_cfg.py | 10 +- .../config/galbot/stack_rmp_rel_env_cfg.py | 2 +- .../manipulation/stack/stack_env_cfg.py | 10 +- .../stack/stack_instance_randomize_env_cfg.py | 10 +- 22 files changed, 923 insertions(+), 826 deletions(-) delete mode 100644 source/isaaclab/isaaclab/physics/physics_backend.py create mode 100644 source/isaaclab/isaaclab/physics/physics_manager.py create mode 100644 source/isaaclab/isaaclab/physics/physics_manager_cfg.py delete mode 100644 source/isaaclab/isaaclab/physics/physx_backend.py create mode 100644 source/isaaclab/isaaclab/physics/physx_manager_cfg.py diff --git a/source/isaaclab/isaaclab/physics/__init__.py b/source/isaaclab/isaaclab/physics/__init__.py index 08267b819e2..4af76ff4d3a 100644 --- a/source/isaaclab/isaaclab/physics/__init__.py +++ b/source/isaaclab/isaaclab/physics/__init__.py @@ -6,14 +6,16 @@ """Implementation backends for simulation interfaces.""" # from .newton_backend import NewtonBackend -from .physics_backend import PhysicsBackend -from .physx_backend import PhysXBackend +from .physics_manager import PhysicsManager +from .physics_manager_cfg import PhysicsManagerCfg from .physx_manager import PhysxManager, IsaacEvents +from .physx_manager_cfg import PhysxManagerCfg __all__ = [ # "NewtonBackend", - "PhysicsBackend", - "PhysXBackend", + "PhysicsManager", + "PhysicsManagerCfg", "PhysxManager", + "PhysxManagerCfg", "IsaacEvents", ] diff --git a/source/isaaclab/isaaclab/physics/physics_backend.py b/source/isaaclab/isaaclab/physics/physics_backend.py deleted file mode 100644 index e46028bfda0..00000000000 --- a/source/isaaclab/isaaclab/physics/physics_backend.py +++ /dev/null @@ -1,53 +0,0 @@ -# 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 - -"""Base class for physics backends.""" - -from __future__ import annotations - -from abc import ABC, abstractmethod -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from isaaclab.sim.simulation_context import SimulationContext - - -class PhysicsBackend(ABC): - """Base class for physics simulation backends. - - Lifecycle: __init__() -> initialize() -> step() (repeated) -> close() - """ - - def __init__(self, sim_context: "SimulationContext"): - """Initialize backend with simulation context. - - Args: - sim_context: Parent simulation context. - """ - self._sim = sim_context - - @abstractmethod - def reset(self, soft: bool = False) -> None: - """Reset physics simulation. - - Args: - soft: If True, skip full reinitialization. - """ - pass - - @abstractmethod - def forward(self) -> None: - """Update kinematics without stepping physics.""" - pass - - @abstractmethod - def step(self) -> None: - """Step physics simulation by one timestep (physics only, no rendering).""" - pass - - @abstractmethod - def close(self) -> None: - """Clean up physics resources.""" - pass diff --git a/source/isaaclab/isaaclab/physics/physics_manager.py b/source/isaaclab/isaaclab/physics/physics_manager.py new file mode 100644 index 00000000000..a8382149e02 --- /dev/null +++ b/source/isaaclab/isaaclab/physics/physics_manager.py @@ -0,0 +1,86 @@ +# 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 + +"""Base class for physics managers.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext + + +class PhysicsManager(ABC): + """Abstract base class for physics simulation managers. + + Physics managers handle the lifecycle of a physics simulation backend, + including initialization, stepping, and cleanup. + + Lifecycle: __init__() -> reset() -> step() (repeated) -> close() + """ + + @classmethod + @abstractmethod + def initialize(cls, sim_context: "SimulationContext") -> None: + """Initialize the physics manager with simulation context. + + Args: + sim_context: Parent simulation context. + """ + pass + + @classmethod + @abstractmethod + def reset(cls, soft: bool = False) -> None: + """Reset physics simulation. + + Args: + soft: If True, skip full reinitialization. + """ + pass + + @classmethod + @abstractmethod + def forward(cls) -> None: + """Update kinematics without stepping physics (for rendering).""" + pass + + @classmethod + @abstractmethod + def step(cls) -> None: + """Step physics simulation by one timestep (physics only, no rendering).""" + pass + + @classmethod + @abstractmethod + def close(cls) -> None: + """Clean up physics resources.""" + pass + + @classmethod + @abstractmethod + def get_physics_dt(cls) -> float: + """Get the physics timestep in seconds.""" + pass + + @classmethod + @abstractmethod + def get_device(cls) -> str: + """Get the physics simulation device.""" + pass + + @classmethod + @abstractmethod + def get_physics_sim_view(cls): + """Get the physics simulation view.""" + pass + + @classmethod + @abstractmethod + def is_fabric_enabled(cls) -> bool: + """Check if fabric interface is enabled.""" + pass diff --git a/source/isaaclab/isaaclab/physics/physics_manager_cfg.py b/source/isaaclab/isaaclab/physics/physics_manager_cfg.py new file mode 100644 index 00000000000..f30d0fa24f8 --- /dev/null +++ b/source/isaaclab/isaaclab/physics/physics_manager_cfg.py @@ -0,0 +1,35 @@ +# 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 + +"""Base configuration for physics managers.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass + +if TYPE_CHECKING: + from .physics_manager import PhysicsManager + + +@configclass +class PhysicsManagerCfg: + """Abstract base configuration for physics managers. + + Subclasses should override :meth:`create_manager` to return the appropriate + physics manager class. + """ + + def create_manager(self) -> type["PhysicsManager"]: + """Create and return the physics manager class for this configuration. + + Returns: + The physics manager class (not an instance). + + Raises: + NotImplementedError: If not overridden by subclass. + """ + raise NotImplementedError("Subclasses must implement create_manager()") diff --git a/source/isaaclab/isaaclab/physics/physx_backend.py b/source/isaaclab/isaaclab/physics/physx_backend.py deleted file mode 100644 index eb3163b5248..00000000000 --- a/source/isaaclab/isaaclab/physics/physx_backend.py +++ /dev/null @@ -1,509 +0,0 @@ -# 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 - -"""PhysX physics backend for SimulationContext.""" - -from __future__ import annotations - -import glob -import logging -import os -import re -import time -import torch -from datetime import datetime -from typing import TYPE_CHECKING - -import carb -import omni.kit.app -import omni.physx -import omni.physics.tensors -from pxr import PhysxSchema, Sdf - -import isaaclab.sim as sim_utils -from .physics_backend import PhysicsBackend -from .physx_manager import PhysxManager - -if TYPE_CHECKING: - from isaaclab.sim.simulation_context import SimulationContext - -logger = logging.getLogger(__name__) - - -class AnimationRecorder: - """Handles animation recording for the simulation. - - This class manages the recording of physics animations using the PhysX PVD - (Physics Visual Debugger) interface. It handles the setup, update, and - finalization of animation recordings. - """ - - def __init__(self, carb_settings: carb.settings.ISettings, app_iface: omni.kit.app.IApp): - """Initialize the animation recorder. - - Args: - carb_settings: The Carbonite settings interface. - app_iface: The Omniverse Kit application interface. - """ - self._carb_settings = carb_settings - self._app_iface = app_iface - self._enabled = False - self._start_time: float = 0.0 - self._stop_time: float = 0.0 - self._started_timestamp: float | None = None - self._output_dir: str = "" - self._timestamp: str = "" - self._physx_pvd_interface = None - - self._setup() - - @property - def enabled(self) -> bool: - """Whether animation recording is enabled.""" - return self._enabled - - def _setup(self) -> None: - """Sets up animation recording settings and initializes the recording.""" - self._enabled = bool(self._carb_settings.get("/isaaclab/anim_recording/enabled")) - if not self._enabled: - return - - # Import omni.physx.pvd.bindings here since it is not available by default - from omni.physxpvd.bindings import _physxPvd - - # Init anim recording settings - self._start_time = self._carb_settings.get("/isaaclab/anim_recording/start_time") - self._stop_time = self._carb_settings.get("/isaaclab/anim_recording/stop_time") - self._started_timestamp = None - - # Make output path relative to repo path - repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") - self._timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") - self._output_dir = ( - os.path.join(repo_path, "anim_recordings", self._timestamp).replace("\\", "/").rstrip("/") + "/" - ) - os.makedirs(self._output_dir, exist_ok=True) - - # Acquire physx pvd interface and set output directory - self._physx_pvd_interface = _physxPvd.acquire_physx_pvd_interface() - - # Set carb settings for the output path and enabling pvd recording - self._carb_settings.set_string("/persistent/physics/omniPvdOvdRecordingDirectory", self._output_dir) - self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) - - def update(self) -> bool: - """Tracks timestamps and triggers finish if total time has elapsed. - - Returns: - True if animation recording has finished, False otherwise. - """ - if not self._enabled: - return False - - if self._started_timestamp is None: - self._started_timestamp = time.time() - - total_time = time.time() - self._started_timestamp - if total_time > self._stop_time: - self._finish() - return True - return False - - def _finish(self) -> bool: - """Finishes the animation recording and outputs the baked animation recording. - - Returns: - True if the recording was successfully finished. - """ - logger.warning( - "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." - ) - - # Detaching the stage will also close it and force the serialization of the OVD file - physx = omni.physx.get_physx_simulation_interface() - physx.detach_stage() - - # Save stage to disk - stage_path = os.path.join(self._output_dir, "stage_simulation.usdc") - sim_utils.save_stage(stage_path, save_and_reload_in_place=False) - - # Find the latest ovd file not named tmp.ovd - ovd_files = [f for f in glob.glob(os.path.join(self._output_dir, "*.ovd")) if not f.endswith("tmp.ovd")] - input_ovd_path = max(ovd_files, key=os.path.getctime) - - # Invoke pvd interface to create recording - stage_filename = "baked_animation_recording.usda" - result = self._physx_pvd_interface.ovd_to_usd_over_with_layer_creation( - input_ovd_path, - stage_path, - self._output_dir, - stage_filename, - self._start_time, - self._stop_time, - True, # True: ASCII layers / False : USDC layers - False, # True: verify over layer - ) - - # Workaround for manually setting the truncated start time in the baked animation recording - self._update_usda_start_time(os.path.join(self._output_dir, stage_filename), self._start_time) - - # Disable recording - self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) - - return result - - @staticmethod - def _update_usda_start_time(file_path: str, start_time: float) -> None: - """Updates the start time of the USDA baked animation recording file. - - Args: - file_path: Path to the USDA file. - start_time: The new start time to set. - """ - # Read the USDA file - with open(file_path) as file: - content = file.read() - - # Extract the timeCodesPerSecond value - time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) - if not time_code_match: - raise ValueError("timeCodesPerSecond not found in the file.") - time_codes_per_second = int(time_code_match.group(1)) - - # Compute the new start time code - new_start_time_code = int(start_time * time_codes_per_second) - - # Replace the startTimeCode in the file - content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) - - # Write the updated content back to the file - with open(file_path, "w") as file: - file.write(content) - - -class PhysXBackend(PhysicsBackend): - """PhysX physics backend. - - This class manages the PhysX physics simulation, including: - - Device settings (CPU/GPU) - - Timestep and solver configuration - - Fabric interface for fast data access - - Physics stepping and reset - - Lifecycle: __init__() -> reset() -> step() (repeated) -> close() - """ - - def __init__(self, sim_context: "SimulationContext"): - """Initialize the PhysX backend. - - Args: - sim_context: Parent simulation context. - """ - super().__init__(sim_context) - - # acquire physics interfaces - self._physx_iface = omni.physx.get_physx_interface() - self._physx_sim_iface = omni.physx.get_physx_simulation_interface() - - # Initialize physics device (will be set in _apply_physics_settings) - self._physics_device: str = "cpu" - - # Fabric interface (will be set in _load_fabric_interface) - self._fabric_iface = None - self._update_fabric = None - - # Configure physics - self._configure_simulation_dt() - self._apply_physics_settings() - PhysxManager.initialize() - - # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes - # when in headless mode - self._sim.set_setting("/app/player/playSimulations", False) - omni.kit.app.get_app().update() - self._sim.set_setting("/app/player/playSimulations", True) - - # load flatcache/fabric interface - self._load_fabric_interface() - - # initialize animation recorder - self._anim_recorder = AnimationRecorder(self._sim.carb_settings, omni.kit.app.get_app()) - - @property - def device(self) -> str: - """Device used for physics simulation.""" - return self._physics_device - - @property - def physics_dt(self) -> float: - """Physics timestep.""" - return self._sim.cfg.dt - - @property - def physics_sim_view(self) -> "omni.physics.tensors.SimulationView": - """Physics simulation view with torch backend.""" - return PhysxManager.get_physics_sim_view() - - def is_fabric_enabled(self) -> bool: - """Returns whether the fabric interface is enabled.""" - return self._fabric_iface is not None - - def reset(self, soft: bool = False) -> None: - """Reset the physics simulation. - - Args: - soft: If True, skip full reinitialization. - """ - if not soft: - # initialize the physics simulation - if PhysxManager.get_physics_sim_view() is None: - PhysxManager.initialize_physics() - - # app.update() may be changing the cuda device in reset, so we force it back to our desired device here - if "cuda" in self._physics_device: - torch.cuda.set_device(self._physics_device) - - # enable kinematic rendering with fabric - physics_sim_view = PhysxManager.get_physics_sim_view() - if physics_sim_view is not None: - physics_sim_view._backend.initialize_kinematic_bodies() - - def forward(self) -> None: - """Update articulation kinematics and fabric for rendering.""" - if self._fabric_iface is not None and self._update_fabric is not None: - physics_sim_view = PhysxManager.get_physics_sim_view() - if physics_sim_view is not None and self._sim.is_playing(): - # Update the articulations' link's poses before rendering - physics_sim_view.update_articulations_kinematic() - self._update_fabric(0.0, 0.0) - - def step(self) -> None: - """Step the physics simulation (physics only, no rendering).""" - # update animation recorder if enabled - if self._anim_recorder.enabled and self._anim_recorder.update(): - logger.warning("Animation recording finished. Closing app.") - omni.kit.app.get_app().shutdown() - return - - # step physics only - self._physx_sim_iface.simulate(self._sim.cfg.dt, 0.0) - self._physx_sim_iface.fetch_results() - - # physics step may change cuda device, force it back - if "cuda" in self._physics_device: - torch.cuda.set_device(self._physics_device) - - def close(self) -> None: - """Clean up physics resources.""" - # clear the simulation manager state (notifies assets to cleanup) - PhysxManager.clear() - # detach the stage from physx - if self._physx_sim_iface is not None: - self._physx_sim_iface.detach_stage() - - # ------------------------- - # Private initialization methods - # ------------------------- - - def _configure_simulation_dt(self): - """Configures the physics simulation step size.""" - cfg = self._sim.cfg - carb_settings = self._sim.carb_settings - - # Get physics scene API from the physics interface - stage = self._sim.stage - physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) - physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) - - # 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(cfg.render_interval, 1) - - # set simulation step per second - steps_per_second = int(1.0 / cfg.dt) - physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) - # set minimum number of steps per frame - min_steps = int(steps_per_second / render_interval) - carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) - - def _apply_physics_settings(self): - """Sets various carb physics settings.""" - cfg = self._sim.cfg - carb_settings = self._sim.carb_settings - - # Get physics scene API from the physics interface - stage = self._sim.stage - physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) - physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) - - # -------------------------- - # Carb Physics API settings - # -------------------------- - - # enable hydra scene-graph instancing - # note: this allows rendering of instanceable assets on the GUI - carb_settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) - # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking - # note: dispatcher handles how threads are launched for multi-threaded physics - carb_settings.set_bool("/physics/physxDispatcher", True) - # disable contact processing in omni.physx - # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. - # The physics flag gets enabled when a contact sensor is created. - if hasattr(cfg, "disable_contact_processing"): - self._sim.logger.warning( - "The `disable_contact_processing` attribute is deprecated and always set to True" - " to avoid unnecessary overhead. Contact processing is automatically enabled when" - " a contact sensor is created, so manual configuration is no longer required." - ) - # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts - # are always processed. The issue is reported to the PhysX team by @mmittal. - carb_settings.set_bool("/physics/disableContactProcessing", True) - # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them - # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags - # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry - carb_settings.set_bool("/physics/collisionConeCustomGeometry", False) - carb_settings.set_bool("/physics/collisionCylinderCustomGeometry", False) - # hide the Simulation Settings window - carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) - - # handle device settings - if "cuda" in cfg.device: - parsed_device = 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 = carb_settings.get_as_int("/physics/cudaDevice") - if device_id < 0: - carb_settings.set_int("/physics/cudaDevice", 0) - device_id = 0 - else: - # if users specified "cuda:N", we use the provided device id - device_id = int(parsed_device[1]) - carb_settings.set_int("/physics/cudaDevice", device_id) - # suppress readback for GPU physics - 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 - carb_settings.set_int("/physics/cudaDevice", -1) - carb_settings.set_bool("/physics/suppressReadback", False) - # save the device - self._physics_device = "cpu" - - # Configure simulation manager backend - # Isaac Lab always uses torch tensors for consistency, even on CPU - PhysxManager.set_backend("torch") - PhysxManager.set_physics_sim_device(self._physics_device) - - # -------------------------- - # USDPhysics API settings - # -------------------------- - - # create the default physics material - # this material is used when no material is specified for a primitive - material_path = f"{cfg.physics_prim_path}/defaultMaterial" - cfg.physics_material.func(material_path, cfg.physics_material) - # bind the physics material to the scene - sim_utils.bind_physics_material(cfg.physics_prim_path, material_path) - - # -------------------------- - # PhysX API settings - # -------------------------- - - # set broadphase type - broadphase_type = "GPU" if "cuda" in cfg.device else "MBP" - physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) - # set gpu dynamics - enable_gpu_dynamics = "cuda" in cfg.device - 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 cfg.physx.enable_ccd: - cfg.physx.enable_ccd = False - self._sim.logger.warning( - "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxCfg config to avoid this" - " warning." - ) - physx_scene_api.CreateEnableCCDAttr(cfg.physx.enable_ccd) - - # set solver type - solver_type = "PGS" if cfg.physx.solver_type == 0 else "TGS" - physx_scene_api.CreateSolverTypeAttr(solver_type) - - # set solve articulation contact last - attr = physx_scene_api.GetPrim().CreateAttribute( - "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool - ) - attr.Set(cfg.physx.solve_articulation_contact_last) - - # iterate over all the settings and set them - for key, value in cfg.physx.to_dict().items(): # type: ignore - 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(physx_scene_api, key, value, camel_case=True) - - # throw warnings for helpful guidance - if cfg.physx.solver_type == 1 and not 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 cfg.physx.enable_stabilization and cfg.dt > 0.0333: - self._sim.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 _load_fabric_interface(self): - """Loads the fabric interface if enabled.""" - import omni.kit.app - - cfg = self._sim.cfg - carb_settings = self._sim.carb_settings - - extension_manager = omni.kit.app.get_app().get_extension_manager() - fabric_enabled = extension_manager.is_extension_enabled("omni.physx.fabric") - - if 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 - self._fabric_iface = get_physx_fabric_interface() - if hasattr(self._fabric_iface, "force_update"): - # The update method in the fabric interface only performs an update if a physics step has occurred. - # However, for rendering, we need to force an update since any element of the scene might have been - # modified in a reset (which occurs after the physics step) and we want the renderer to be aware of - # these changes. - self._update_fabric = self._fabric_iface.force_update - 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 fabric interface to None - self._fabric_iface = None - - # set carb settings for fabric - carb_settings.set_bool("/physics/fabricEnabled", cfg.use_fabric) - carb_settings.set_bool("/physics/updateToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateParticlesToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateVelocitiesToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateForceSensorsToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateResidualsToUsd", not cfg.use_fabric) - # disable simulation output window visibility - carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) diff --git a/source/isaaclab/isaaclab/physics/physx_manager.py b/source/isaaclab/isaaclab/physics/physx_manager.py index 2583f0a7a42..c3a6db0bfd6 100644 --- a/source/isaaclab/isaaclab/physics/physx_manager.py +++ b/source/isaaclab/isaaclab/physics/physx_manager.py @@ -3,28 +3,44 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Simulation manager for Isaac Lab. +"""PhysX Manager for Isaac Lab. -This module manages physics simulation lifecycle, callbacks, and physics views. +This module manages PhysX physics simulation lifecycle, configuration, callbacks, and physics views. """ from __future__ import annotations +import glob +import logging +import os +import re +import time import weakref from collections import OrderedDict +from datetime import datetime from enum import Enum -from typing import Any, Callable +from typing import TYPE_CHECKING, Any, Callable import carb import omni.kit +import omni.kit.app import omni.physics.tensors import omni.physx import omni.timeline import omni.usd -from pxr import PhysxSchema +import torch +from pxr import PhysxSchema, Sdf + +import isaaclab.sim as sim_utils +from .physics_manager import PhysicsManager + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext __all__ = ["IsaacEvents", "PhysxManager"] +logger = logging.getLogger(__name__) + class IsaacEvents(Enum): """Events dispatched during simulation lifecycle.""" @@ -39,10 +55,165 @@ class IsaacEvents(Enum): TIMELINE_STOP = "isaac.timeline_stop" -class PhysxManager: - """Manages physics simulation lifecycle and callbacks. +class AnimationRecorder: + """Handles animation recording for the simulation. - This is a class-level (singleton-like) manager for the simulation. + This class manages the recording of physics animations using the PhysX PVD + (Physics Visual Debugger) interface. It handles the setup, update, and + finalization of animation recordings. + """ + + def __init__(self, carb_settings: carb.settings.ISettings, app_iface: omni.kit.app.IApp): + """Initialize the animation recorder. + + Args: + carb_settings: The Carbonite settings interface. + app_iface: The Omniverse Kit application interface. + """ + self._carb_settings = carb_settings + self._app_iface = app_iface + self._enabled = False + self._start_time: float = 0.0 + self._stop_time: float = 0.0 + self._started_timestamp: float | None = None + self._output_dir: str = "" + self._timestamp: str = "" + self._physx_pvd_interface = None + + self._setup() + + @property + def enabled(self) -> bool: + """Whether animation recording is enabled.""" + return self._enabled + + def _setup(self) -> None: + """Sets up animation recording settings and initializes the recording.""" + self._enabled = bool(self._carb_settings.get("/isaaclab/anim_recording/enabled")) + if not self._enabled: + return + + # Import omni.physx.pvd.bindings here since it is not available by default + from omni.physxpvd.bindings import _physxPvd + + # Init anim recording settings + self._start_time = self._carb_settings.get("/isaaclab/anim_recording/start_time") + self._stop_time = self._carb_settings.get("/isaaclab/anim_recording/stop_time") + self._started_timestamp = None + + # Make output path relative to repo path + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + self._timestamp = datetime.now().strftime("%Y_%m_%d_%H%M%S") + self._output_dir = ( + os.path.join(repo_path, "anim_recordings", self._timestamp).replace("\\", "/").rstrip("/") + "/" + ) + os.makedirs(self._output_dir, exist_ok=True) + + # Acquire physx pvd interface and set output directory + self._physx_pvd_interface = _physxPvd.acquire_physx_pvd_interface() + + # Set carb settings for the output path and enabling pvd recording + self._carb_settings.set_string("/persistent/physics/omniPvdOvdRecordingDirectory", self._output_dir) + self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", True) + + def update(self) -> bool: + """Tracks timestamps and triggers finish if total time has elapsed. + + Returns: + True if animation recording has finished, False otherwise. + """ + if not self._enabled: + return False + + if self._started_timestamp is None: + self._started_timestamp = time.time() + + total_time = time.time() - self._started_timestamp + if total_time > self._stop_time: + self._finish() + return True + return False + + def _finish(self) -> bool: + """Finishes the animation recording and outputs the baked animation recording. + + Returns: + True if the recording was successfully finished. + """ + logger.warning( + "[INFO][SimulationContext]: Finishing animation recording. Stage must be saved. Might take a few minutes." + ) + + # Detaching the stage will also close it and force the serialization of the OVD file + physx = omni.physx.get_physx_simulation_interface() + physx.detach_stage() + + # Save stage to disk + stage_path = os.path.join(self._output_dir, "stage_simulation.usdc") + sim_utils.save_stage(stage_path, save_and_reload_in_place=False) + + # Find the latest ovd file not named tmp.ovd + ovd_files = [f for f in glob.glob(os.path.join(self._output_dir, "*.ovd")) if not f.endswith("tmp.ovd")] + input_ovd_path = max(ovd_files, key=os.path.getctime) + + # Invoke pvd interface to create recording + stage_filename = "baked_animation_recording.usda" + result = self._physx_pvd_interface.ovd_to_usd_over_with_layer_creation( + input_ovd_path, + stage_path, + self._output_dir, + stage_filename, + self._start_time, + self._stop_time, + True, # True: ASCII layers / False : USDC layers + False, # True: verify over layer + ) + + # Workaround for manually setting the truncated start time in the baked animation recording + self._update_usda_start_time(os.path.join(self._output_dir, stage_filename), self._start_time) + + # Disable recording + self._carb_settings.set_bool("/physics/omniPvdOutputEnabled", False) + + return result + + @staticmethod + def _update_usda_start_time(file_path: str, start_time: float) -> None: + """Updates the start time of the USDA baked animation recording file. + + Args: + file_path: Path to the USDA file. + start_time: The new start time to set. + """ + # Read the USDA file + with open(file_path) as file: + content = file.read() + + # Extract the timeCodesPerSecond value + time_code_match = re.search(r"timeCodesPerSecond\s*=\s*(\d+)", content) + if not time_code_match: + raise ValueError("timeCodesPerSecond not found in the file.") + time_codes_per_second = int(time_code_match.group(1)) + + # Compute the new start time code + new_start_time_code = int(start_time * time_codes_per_second) + + # Replace the startTimeCode in the file + content = re.sub(r"startTimeCode\s*=\s*\d+", f"startTimeCode = {new_start_time_code}", content) + + # Write the updated content back to the file + with open(file_path, "w") as file: + file.write(content) + + +class PhysxManager(PhysicsManager): + """Manages PhysX physics simulation lifecycle, configuration, callbacks, and physics views. + + This is a class-level (singleton-like) manager for the PhysX simulation. + It handles device settings (CPU/GPU), timestep/solver configuration, + fabric interface for fast data access, physics stepping, and reset. + + Lifecycle: initialize() -> reset() -> step() (repeated) -> close() """ # Core interfaces (names must match Isaac Sim's expected attributes) @@ -68,6 +239,15 @@ class PhysxManager: _callback_id: int = 0 _handles: dict = {} # Named internal handles + # Simulation context reference + _sim: "SimulationContext | None" = None + + # Device and fabric state (from PhysXBackend) + _physics_device: str = "cpu" + _fabric_iface = None + _update_fabric = None + _anim_recorder: AnimationRecorder | None = None + # Compatibility stub for Isaac Sim code that calls _simulation_manager_interface class _PhysxManagerInterfaceStub: """Minimal stub for Isaac Sim compatibility.""" @@ -116,11 +296,92 @@ def is_simulating() -> bool: # ------------------------------------------------------------------ @classmethod - def initialize(cls) -> None: - """Initialize the manager and set up timeline callbacks.""" + def initialize(cls, sim_context: "SimulationContext") -> None: + """Initialize the manager with simulation context and set up physics. + + Args: + sim_context: Parent simulation context. + """ + cls._sim = sim_context cls._setup_callbacks() cls._track_physics_scenes() + # Configure physics settings + cls._configure_simulation_dt() + cls._apply_physics_settings() + + # a stage update here is needed for the case when physics_dt != rendering_dt + cls._sim.set_setting("/app/player/playSimulations", False) + omni.kit.app.get_app().update() + cls._sim.set_setting("/app/player/playSimulations", True) + + # load fabric interface + cls._load_fabric_interface() + + # initialize animation recorder + cls._anim_recorder = AnimationRecorder(cls._sim.carb_settings, omni.kit.app.get_app()) + + @classmethod + def reset(cls, soft: bool = False) -> None: + """Reset the physics simulation. + + Args: + soft: If True, skip full reinitialization. + """ + if not soft: + # initialize the physics simulation + if cls._view is None: + cls.initialize_physics() + + # app.update() may be changing the cuda device in reset, so we force it back to our desired device here + if "cuda" in cls._physics_device: + torch.cuda.set_device(cls._physics_device) + + # enable kinematic rendering with fabric + if cls._view is not None: + cls._view._backend.initialize_kinematic_bodies() + + @classmethod + def forward(cls) -> None: + """Update articulation kinematics and fabric for rendering.""" + if cls._fabric_iface is not None and cls._update_fabric is not None: + if cls._view is not None and cls._sim is not None and cls._sim.is_playing(): + # Update the articulations' link's poses before rendering + cls._view.update_articulations_kinematic() + cls._update_fabric(0.0, 0.0) + + @classmethod + def step(cls) -> None: + """Step the physics simulation (physics only, no rendering).""" + if cls._sim is None: + return + + # update animation recorder if enabled + if cls._anim_recorder is not None and cls._anim_recorder.enabled and cls._anim_recorder.update(): + logger.warning("Animation recording finished. Closing app.") + omni.kit.app.get_app().shutdown() + return + + # step physics only + cls._physx_sim_interface.simulate(cls._sim.cfg.dt, 0.0) + cls._physx_sim_interface.fetch_results() + + # physics step may change cuda device, force it back + if "cuda" in cls._physics_device: + torch.cuda.set_device(cls._physics_device) + + @classmethod + def close(cls) -> None: + """Clean up physics resources.""" + # clear the simulation manager state (notifies assets to cleanup) + cls.clear() + # detach the stage from physx + if cls._physx_sim_interface is not None: + cls._physx_sim_interface.detach_stage() + # clear references + cls._sim = None + cls._anim_recorder = None + @classmethod def clear(cls) -> None: """Clear all state and callbacks.""" @@ -143,6 +404,9 @@ def clear(cls) -> None: cls._view_created = False cls._assets_loaded = True cls._physics_scene_apis.clear() + # Clear fabric interface + cls._fabric_iface = None + cls._update_fabric = None @classmethod def initialize_physics(cls) -> None: @@ -177,6 +441,10 @@ def set_backend(cls, backend: str) -> None: @classmethod def get_physics_dt(cls) -> float: """Get the physics timestep in seconds.""" + # Prefer sim context config if available + if cls._sim is not None: + return cls._sim.cfg.dt + # Fallback to USD scene if cls._physics_scene_apis: api = list(cls._physics_scene_apis.values())[0] hz = api.GetTimeStepsPerSecondAttr().Get() @@ -184,13 +452,19 @@ def get_physics_dt(cls) -> float: return 1.0 / 60.0 @classmethod - def get_physics_sim_device(cls) -> str: + def get_device(cls) -> str: """Get the physics simulation device.""" - suppress = cls._carb_settings.get_as_bool("/physics/suppressReadback") - if suppress and cls._is_gpu_enabled(): - device_id = max(0, cls._carb_settings.get_as_int("/physics/cudaDevice")) - return f"cuda:{device_id}" - return "cpu" + return cls._physics_device + + @classmethod + def get_physics_sim_device(cls) -> str: + """Get the physics simulation device (alias for get_device).""" + return cls._physics_device + + @classmethod + def is_fabric_enabled(cls) -> bool: + """Returns whether the fabric interface is enabled.""" + return cls._fabric_iface is not None @classmethod def set_physics_sim_device(cls, device: str) -> None: @@ -412,3 +686,218 @@ def _enable_fabric(cls, enable: bool) -> None: cls._carb_settings.set_bool("/physics/updateParticlesToUsd", not enable) cls._carb_settings.set_bool("/physics/updateVelocitiesToUsd", not enable) cls._carb_settings.set_bool("/physics/updateForceSensorsToUsd", not enable) + + # ------------------------------------------------------------------ + # Physics Configuration (from PhysXBackend) + # ------------------------------------------------------------------ + + @classmethod + def _configure_simulation_dt(cls) -> None: + """Configures the physics simulation step size.""" + if cls._sim is None: + return + + cfg = cls._sim.cfg + carb_settings = cls._sim.carb_settings + + # Get physics scene API from the physics interface + stage = cls._sim.stage + physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) + physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) + + # 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(cfg.render_interval, 1) + + # set simulation step per second + steps_per_second = int(1.0 / cfg.dt) + physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) + # set minimum number of steps per frame + min_steps = int(steps_per_second / render_interval) + carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) + + @classmethod + def _apply_physics_settings(cls) -> None: + """Sets various carb physics settings.""" + if cls._sim is None: + return + + cfg = cls._sim.cfg + carb_settings = cls._sim.carb_settings + + # Get physics scene API from the physics interface + stage = cls._sim.stage + physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) + physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) + + # -------------------------- + # Carb Physics API settings + # -------------------------- + + # enable hydra scene-graph instancing + # note: this allows rendering of instanceable assets on the GUI + carb_settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) + # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking + # note: dispatcher handles how threads are launched for multi-threaded physics + carb_settings.set_bool("/physics/physxDispatcher", True) + # disable contact processing in omni.physx + # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. + # The physics flag gets enabled when a contact sensor is created. + if hasattr(cfg, "disable_contact_processing"): + cls._sim.logger.warning( + "The `disable_contact_processing` attribute is deprecated and always set to True" + " to avoid unnecessary overhead. Contact processing is automatically enabled when" + " a contact sensor is created, so manual configuration is no longer required." + ) + # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts + # are always processed. The issue is reported to the PhysX team by @mmittal. + carb_settings.set_bool("/physics/disableContactProcessing", True) + # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them + # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags + # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry + carb_settings.set_bool("/physics/collisionConeCustomGeometry", False) + carb_settings.set_bool("/physics/collisionCylinderCustomGeometry", False) + # hide the Simulation Settings window + carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) + + # handle device settings + if "cuda" in cfg.device: + parsed_device = 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 = carb_settings.get_as_int("/physics/cudaDevice") + if device_id < 0: + carb_settings.set_int("/physics/cudaDevice", 0) + device_id = 0 + else: + # if users specified "cuda:N", we use the provided device id + device_id = int(parsed_device[1]) + carb_settings.set_int("/physics/cudaDevice", device_id) + # suppress readback for GPU physics + carb_settings.set_bool("/physics/suppressReadback", True) + # save the device + cls._physics_device = f"cuda:{device_id}" + else: + # enable USD read/write operations for CPU physics + carb_settings.set_int("/physics/cudaDevice", -1) + carb_settings.set_bool("/physics/suppressReadback", False) + # save the device + cls._physics_device = "cpu" + + # Configure simulation manager backend + # Isaac Lab always uses torch tensors for consistency, even on CPU + cls.set_backend("torch") + cls.set_physics_sim_device(cls._physics_device) + + # -------------------------- + # USDPhysics API settings + # -------------------------- + + # create the default physics material + # this material is used when no material is specified for a primitive + material_path = f"{cfg.physics_prim_path}/defaultMaterial" + cfg.physics_material.func(material_path, cfg.physics_material) + # bind the physics material to the scene + sim_utils.bind_physics_material(cfg.physics_prim_path, material_path) + + # -------------------------- + # PhysX API settings + # -------------------------- + + # set broadphase type + broadphase_type = "GPU" if "cuda" in cfg.device else "MBP" + physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) + # set gpu dynamics + enable_gpu_dynamics = "cuda" in cfg.device + 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 cfg.physics_manager_cfg.enable_ccd: + cfg.physics_manager_cfg.enable_ccd = False + cls._sim.logger.warning( + "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxCfg config to avoid this" + " warning." + ) + physx_scene_api.CreateEnableCCDAttr(cfg.physics_manager_cfg.enable_ccd) + + # set solver type + solver_type = "PGS" if cfg.physics_manager_cfg.solver_type == 0 else "TGS" + physx_scene_api.CreateSolverTypeAttr(solver_type) + + # set solve articulation contact last + attr = physx_scene_api.GetPrim().CreateAttribute( + "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool + ) + attr.Set(cfg.physics_manager_cfg.solve_articulation_contact_last) + + # iterate over all the settings and set them + for key, value in cfg.physics_manager_cfg.to_dict().items(): # type: ignore + 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(physx_scene_api, key, value, camel_case=True) + + # throw warnings for helpful guidance + if cfg.physics_manager_cfg.solver_type == 1 and not cfg.physics_manager_cfg.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 cfg.physics_manager_cfg.enable_stabilization and cfg.dt > 0.0333: + cls._sim.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." + ) + + @classmethod + def _load_fabric_interface(cls) -> None: + """Loads the fabric interface if enabled.""" + if cls._sim is None: + return + + cfg = cls._sim.cfg + carb_settings = cls._sim.carb_settings + + extension_manager = omni.kit.app.get_app().get_extension_manager() + fabric_enabled = extension_manager.is_extension_enabled("omni.physx.fabric") + + if 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 + cls._fabric_iface = get_physx_fabric_interface() + if hasattr(cls._fabric_iface, "force_update"): + # The update method in the fabric interface only performs an update if a physics step has occurred. + # However, for rendering, we need to force an update since any element of the scene might have been + # modified in a reset (which occurs after the physics step) and we want the renderer to be aware of + # these changes. + cls._update_fabric = cls._fabric_iface.force_update + else: + # Needed for backward compatibility with older Isaac Sim versions + cls._update_fabric = cls._fabric_iface.update + else: + if fabric_enabled: + extension_manager.set_extension_enabled_immediate("omni.physx.fabric", False) + # set fabric interface to None + cls._fabric_iface = None + + # set carb settings for fabric + carb_settings.set_bool("/physics/fabricEnabled", cfg.use_fabric) + carb_settings.set_bool("/physics/updateToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateParticlesToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateVelocitiesToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateForceSensorsToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/updateResidualsToUsd", not cfg.use_fabric) + # disable simulation output window visibility + carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) diff --git a/source/isaaclab/isaaclab/physics/physx_manager_cfg.py b/source/isaaclab/isaaclab/physics/physx_manager_cfg.py new file mode 100644 index 00000000000..e9be643c70a --- /dev/null +++ b/source/isaaclab/isaaclab/physics/physx_manager_cfg.py @@ -0,0 +1,215 @@ +# 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 + +"""Configuration for PhysX physics manager.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Literal + +from isaaclab.utils import configclass + +from .physics_manager_cfg import PhysicsManagerCfg + +if TYPE_CHECKING: + from .physics_manager import PhysicsManager + + +@configclass +class PhysxManagerCfg(PhysicsManagerCfg): + """Configuration for PhysX physics manager. + + This configuration includes all PhysX solver settings. For more information, + see the `PhysX 5 SDK documentation`_. + + PhysX 5 supports GPU-accelerated physics simulation. This is enabled by default, + but can be disabled by setting the :attr:`~SimulationCfg.device` to ``cpu`` in + :class:`SimulationCfg`. Unlike CPU PhysX, the GPU simulation feature is unable + to dynamically grow all the buffers. Therefore, it is necessary to provide a + reasonable estimate of the buffer sizes for GPU features. If insufficient buffer + sizes are provided, the simulation will fail with errors and lead to adverse + behaviors. The buffer sizes can be adjusted through the ``gpu_*`` parameters. + + .. _PhysX 5 SDK documentation: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxSceneDesc.html + """ + + # ------------------------------------------------------------------ + # Solver Settings + # ------------------------------------------------------------------ + + solver_type: Literal[0, 1] = 1 + """The type of solver to use. Default is 1 (TGS). + + Available solvers: + + * :obj:`0`: PGS (Projective Gauss-Seidel) + * :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. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_position_iteration_count, max_position_iteration_count]``. + """ + + max_position_iteration_count: int = 255 + """Maximum number of solver position iterations (rigid bodies, cloth, particles etc.). Default is 255. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_position_iteration_count, max_position_iteration_count]``. + """ + + min_velocity_iteration_count: int = 0 + """Minimum number of solver velocity iterations (rigid bodies, cloth, particles etc.). Default is 0. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_velocity_iteration_count, max_velocity_iteration_count]``. + """ + + max_velocity_iteration_count: int = 255 + """Maximum number of solver velocity iterations (rigid bodies, cloth, particles etc.). Default is 255. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_velocity_iteration_count, max_velocity_iteration_count]``. + """ + + enable_ccd: bool = False + """Enable a second broad-phase pass that makes it possible to prevent objects from tunneling through each other. + Default is False.""" + + enable_stabilization: bool = False + """Enable/disable additional stabilization pass in solver. Default is False. + + .. note:: + + We recommend setting this flag to true only when the simulation step size is large (i.e., less than 30 Hz or more than 0.0333 seconds). + + .. warning:: + + Enabling this flag may lead to incorrect contact forces report from the contact sensor. + """ + + enable_external_forces_every_iteration: bool = False + """Enable/disable external forces every position iteration in the TGS solver. Default is False. + + When using the TGS solver (:attr:`solver_type` is 1), this flag allows enabling external forces every solver position iteration. + This can help improve the accuracy of velocity updates. Consider enabling this flag if the velocities generated by + the simulation are noisy. Increasing the number of velocity iterations, together with this flag, can help improve + the accuracy of velocity updates. + + .. note:: + + This flag is ignored when using the PGS solver (:attr:`solver_type` is 0). + """ + + enable_enhanced_determinism: bool = False + """Enable/disable improved determinism at the expense of performance. Defaults to False. + + For more information on PhysX determinism, please check `here`_. + + .. _here: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/RigidBodyDynamics.html#enhanced-determinism + """ + + bounce_threshold_velocity: float = 0.5 + """Relative velocity threshold for contacts to bounce (in m/s). Default is 0.5 m/s.""" + + friction_offset_threshold: float = 0.04 + """Threshold for contact point to experience friction force (in m). Default is 0.04 m.""" + + friction_correlation_distance: float = 0.025 + """Distance threshold for merging contacts into a single friction anchor point (in m). Default is 0.025 m.""" + + # ------------------------------------------------------------------ + # GPU Buffer Settings + # ------------------------------------------------------------------ + + gpu_max_rigid_contact_count: int = 2**23 + """Size of rigid contact stream buffer allocated in pinned host memory. Default is 2 ** 23.""" + + gpu_max_rigid_patch_count: int = 5 * 2**15 + """Size of the rigid contact patch stream buffer allocated in pinned host memory. Default is 5 * 2 ** 15.""" + + gpu_found_lost_pairs_capacity: int = 2**21 + """Capacity of found and lost buffers allocated in GPU global memory. Default is 2 ** 21. + + This is used for the found/lost pair reports in the BP. + """ + + gpu_found_lost_aggregate_pairs_capacity: int = 2**25 + """Capacity of found and lost buffers in aggregate system allocated in GPU global memory. + Default is 2 ** 25. + + This is used for the found/lost pair reports in AABB manager. + """ + + gpu_total_aggregate_pairs_capacity: int = 2**21 + """Capacity of total number of aggregate pairs allocated in GPU global memory. Default is 2 ** 21.""" + + gpu_collision_stack_size: int = 2**26 + """Size of the collision stack buffer allocated in pinned host memory. Default is 2 ** 26.""" + + gpu_heap_capacity: int = 2**26 + """Initial capacity of the GPU and pinned host memory heaps. Additional memory will be allocated + if more memory is required. Default is 2 ** 26.""" + + gpu_temp_buffer_capacity: int = 2**24 + """Capacity of temp buffer allocated in pinned host memory. Default is 2 ** 24.""" + + gpu_max_num_partitions: int = 8 + """Limitation for the partitions in the GPU dynamics pipeline. Default is 8. + + This variable must be power of 2. A value greater than 32 is currently not supported. Range: (1, 32) + """ + + gpu_max_soft_body_contacts: int = 2**20 + """Size of soft body contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + gpu_max_particle_contacts: int = 2**20 + """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + # ------------------------------------------------------------------ + # Factory Method + # ------------------------------------------------------------------ + + def create_manager(self) -> type["PhysicsManager"]: + """Create and return the PhysxManager class. + + Returns: + The PhysxManager class. + """ + from .physx_manager import PhysxManager + return PhysxManager diff --git a/source/isaaclab/isaaclab/sim/__init__.py b/source/isaaclab/isaaclab/sim/__init__.py index 16b32df44de..d4ed0865e40 100644 --- a/source/isaaclab/isaaclab/sim/__init__.py +++ b/source/isaaclab/isaaclab/sim/__init__.py @@ -28,7 +28,7 @@ from .converters import * # noqa: F401, F403 from .schemas import * # noqa: F401, F403 -from .simulation_cfg import PhysxCfg, RenderCfg, SimulationCfg # noqa: F401, F403 +from .simulation_cfg import RenderCfg, SimulationCfg # noqa: F401, F403 from .simulation_context import SimulationContext, build_simulation_context # noqa: F401, F403 from isaaclab.physics.physx_manager import PhysxManager, IsaacEvents # noqa: F401, F403 from .spawners import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sim/physics_interface.py b/source/isaaclab/isaaclab/sim/physics_interface.py index a9e76b4299d..57084d9606b 100644 --- a/source/isaaclab/isaaclab/sim/physics_interface.py +++ b/source/isaaclab/isaaclab/sim/physics_interface.py @@ -14,8 +14,7 @@ from pxr import Gf, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils -from isaaclab.physics.physx_backend import PhysXBackend -from isaaclab.physics.physics_backend import PhysicsBackend +from isaaclab.physics.physics_manager import PhysicsManager from .interface import Interface if TYPE_CHECKING: @@ -25,16 +24,16 @@ class PhysicsInterface(Interface): - """Manages USD physics scene and delegates to physics backends. + """Manages USD physics scene and delegates to PhysicsManager. This interface handles: - USD physics scene creation and configuration - Gravity, timestep, and unit settings - - Delegating lifecycle operations to registered backends + - Delegating lifecycle operations to the physics manager """ def __init__(self, sim_context: "SimulationContext"): - """Initialize physics scene and backends. + """Initialize physics scene and physics manager. Args: sim_context: Parent simulation context. @@ -43,19 +42,19 @@ def __init__(self, sim_context: "SimulationContext"): self.physics_prim_path = self._sim.cfg.physics_prim_path self.backend = "torch" + + # Get the physics manager class from config + self.physics_manager: type[PhysicsManager] = self._sim.cfg.physics_manager_cfg.create_manager() + # Initialize USD physics scene self._init_usd_physics_scene() - self._backends: list[PhysicsBackend] = [PhysXBackend(sim_context)] - - @property - def backends(self) -> list[PhysicsBackend]: - """List of active physics backends.""" - return self._backends + # Initialize the physics manager + self.physics_manager.initialize(sim_context) @property def physics_dt(self) -> float: """Physics timestep.""" - return self._sim.cfg.dt + return self.physics_manager.get_physics_dt() @property def rendering_dt(self) -> float: @@ -65,7 +64,16 @@ def rendering_dt(self) -> float: @property def device(self) -> str: """Device used for physics simulation.""" - return self._sim.cfg.device + return self.physics_manager.get_device() + + @property + def physics_sim_view(self): + """Physics simulation view with torch backend.""" + return self.physics_manager.get_physics_sim_view() + + def is_fabric_enabled(self) -> bool: + """Returns whether the fabric interface is enabled.""" + return self.physics_manager.is_fabric_enabled() def _init_usd_physics_scene(self) -> None: """Create and configure the USD physics scene.""" @@ -88,7 +96,7 @@ def _init_usd_physics_scene(self) -> None: physics_scene = UsdPhysics.Scene.Define(stage, self._sim.cfg.physics_prim_path) # Pre-create gravity tensor to avoid torch heap corruption issues (torch 2.1+) - gravity = torch.tensor(self._sim.cfg.gravity, dtype=torch.float32, device=self.device) + gravity = torch.tensor(self._sim.cfg.gravity, dtype=torch.float32, device=self._sim.cfg.device) gravity_magnitude = torch.norm(gravity).item() # Avoid division by zero @@ -101,30 +109,25 @@ def _init_usd_physics_scene(self) -> None: physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) def reset(self, soft: bool = False) -> None: - """Reset all physics backends. + """Reset physics simulation. Args: soft: If True, skip full reinitialization. """ - for backend in self._backends: - backend.reset(soft) + self.physics_manager.reset(soft) def forward(self) -> None: - """Update articulation kinematics on all backends.""" - for backend in self._backends: - backend.forward() + """Update articulation kinematics for rendering.""" + self.physics_manager.forward() def step(self, render: bool = True) -> None: - """Step all physics backends (physics only). + """Step physics simulation (physics only). Args: render: Unused, kept for interface compatibility. """ - for backend in self._backends: - backend.step() + self.physics_manager.step() def close(self) -> None: - """Clean up all physics backends.""" - for backend in self._backends: - backend.close() - self._backends.clear() + """Clean up physics resources.""" + self.physics_manager.close() diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index 355cda5711c..5988d40d972 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -9,190 +9,15 @@ configuring the environment instances, viewer settings, and simulation parameters. """ -from typing import Any, Literal +from typing import Any, Literal # Literal used by RenderCfg from isaaclab.utils import configclass from .spawners.materials import RigidBodyMaterialCfg +from isaaclab.physics.physics_manager_cfg import PhysicsManagerCfg +from isaaclab.physics.physx_manager_cfg import PhysxManagerCfg from isaaclab.visualizers import VisualizerCfg -@configclass -class PhysxCfg: - """Configuration for PhysX solver-related parameters. - - These parameters are used to configure the PhysX solver. For more information, see the `PhysX 5 SDK - documentation`_. - - PhysX 5 supports GPU-accelerated physics simulation. This is enabled by default, but can be disabled - by setting the :attr:`~SimulationCfg.device` to ``cpu`` in :class:`SimulationCfg`. Unlike CPU PhysX, the GPU - simulation feature is unable to dynamically grow all the buffers. Therefore, it is necessary to provide - a reasonable estimate of the buffer sizes for GPU features. If insufficient buffer sizes are provided, the - simulation will fail with errors and lead to adverse behaviors. The buffer sizes can be adjusted through the - ``gpu_*`` parameters. - - .. _PhysX 5 SDK documentation: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxSceneDesc.html - - """ - - solver_type: Literal[0, 1] = 1 - """The type of solver to use.Default is 1 (TGS). - - Available solvers: - - * :obj:`0`: PGS (Projective Gauss-Seidel) - * :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. - - .. note:: - - Each physics actor in Omniverse specifies its own solver iteration count. The solver takes - the number of iterations specified by the actor with the highest iteration and clamps it to - the range ``[min_position_iteration_count, max_position_iteration_count]``. - """ - - max_position_iteration_count: int = 255 - """Maximum number of solver position iterations (rigid bodies, cloth, particles etc.). Default is 255. - - .. note:: - - Each physics actor in Omniverse specifies its own solver iteration count. The solver takes - the number of iterations specified by the actor with the highest iteration and clamps it to - the range ``[min_position_iteration_count, max_position_iteration_count]``. - """ - - min_velocity_iteration_count: int = 0 - """Minimum number of solver velocity iterations (rigid bodies, cloth, particles etc.). Default is 0. - - .. note:: - - Each physics actor in Omniverse specifies its own solver iteration count. The solver takes - the number of iterations specified by the actor with the highest iteration and clamps it to - the range ``[min_velocity_iteration_count, max_velocity_iteration_count]``. - """ - - max_velocity_iteration_count: int = 255 - """Maximum number of solver velocity iterations (rigid bodies, cloth, particles etc.). Default is 255. - - .. note:: - - Each physics actor in Omniverse specifies its own solver iteration count. The solver takes - the number of iterations specified by the actor with the highest iteration and clamps it to - the range ``[min_velocity_iteration_count, max_velocity_iteration_count]``. - """ - - enable_ccd: bool = False - """Enable a second broad-phase pass that makes it possible to prevent objects from tunneling through each other. - Default is False.""" - - enable_stabilization: bool = False - """Enable/disable additional stabilization pass in solver. Default is False. - - .. note:: - - We recommend setting this flag to true only when the simulation step size is large (i.e., less than 30 Hz or more than 0.0333 seconds). - - .. warning:: - - Enabling this flag may lead to incorrect contact forces report from the contact sensor. - """ - - enable_external_forces_every_iteration: bool = False - """Enable/disable external forces every position iteration in the TGS solver. Default is False. - - When using the TGS solver (:attr:`solver_type` is 1), this flag allows enabling external forces every solver position iteration. - This can help improve the accuracy of velocity updates. Consider enabling this flag if the velocities generated by - the simulation are noisy. Increasing the number of velocity iterations, together with this flag, can help improve - the accuracy of velocity updates. - - .. note:: - - This flag is ignored when using the PGS solver (:attr:`solver_type` is 0). - """ - - enable_enhanced_determinism: bool = False - """Enable/disable improved determinism at the expense of performance. Defaults to False. - - For more information on PhysX determinism, please check `here`_. - - .. _here: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/RigidBodyDynamics.html#enhanced-determinism - """ - - bounce_threshold_velocity: float = 0.5 - """Relative velocity threshold for contacts to bounce (in m/s). Default is 0.5 m/s.""" - - friction_offset_threshold: float = 0.04 - """Threshold for contact point to experience friction force (in m). Default is 0.04 m.""" - - friction_correlation_distance: float = 0.025 - """Distance threshold for merging contacts into a single friction anchor point (in m). Default is 0.025 m.""" - - gpu_max_rigid_contact_count: int = 2**23 - """Size of rigid contact stream buffer allocated in pinned host memory. Default is 2 ** 23.""" - - gpu_max_rigid_patch_count: int = 5 * 2**15 - """Size of the rigid contact patch stream buffer allocated in pinned host memory. Default is 5 * 2 ** 15.""" - - gpu_found_lost_pairs_capacity: int = 2**21 - """Capacity of found and lost buffers allocated in GPU global memory. Default is 2 ** 21. - - This is used for the found/lost pair reports in the BP. - """ - - gpu_found_lost_aggregate_pairs_capacity: int = 2**25 - """Capacity of found and lost buffers in aggregate system allocated in GPU global memory. - Default is 2 ** 25. - - This is used for the found/lost pair reports in AABB manager. - """ - - gpu_total_aggregate_pairs_capacity: int = 2**21 - """Capacity of total number of aggregate pairs allocated in GPU global memory. Default is 2 ** 21.""" - - gpu_collision_stack_size: int = 2**26 - """Size of the collision stack buffer allocated in pinned host memory. Default is 2 ** 26.""" - - gpu_heap_capacity: int = 2**26 - """Initial capacity of the GPU and pinned host memory heaps. Additional memory will be allocated - if more memory is required. Default is 2 ** 26.""" - - gpu_temp_buffer_capacity: int = 2**24 - """Capacity of temp buffer allocated in pinned host memory. Default is 2 ** 24.""" - - gpu_max_num_partitions: int = 8 - """Limitation for the partitions in the GPU dynamics pipeline. Default is 8. - - This variable must be power of 2. A value greater than 32 is currently not supported. Range: (1, 32) - """ - - gpu_max_soft_body_contacts: int = 2**20 - """Size of soft body contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" - - gpu_max_particle_contacts: int = 2**20 - """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" - @configclass class RenderCfg: @@ -403,8 +228,12 @@ class SimulationCfg: running under the hood. """ - physx: PhysxCfg = PhysxCfg() - """PhysX solver settings. Default is PhysxCfg().""" + physics_manager_cfg: PhysicsManagerCfg = PhysxManagerCfg() + """Physics manager configuration. Default is PhysxManagerCfg(). + + This configuration determines which physics manager to use. Override with + a different config (e.g., NewtonManagerCfg) to use a different physics backend. + """ physics_material: RigidBodyMaterialCfg = RigidBodyMaterialCfg() """Default physics material settings for rigid bodies. Default is RigidBodyMaterialCfg(). @@ -438,4 +267,4 @@ class SimulationCfg: """ visualizer_cfgs: list[VisualizerCfg] | VisualizerCfg | None = None - """The list of visualizer configurations. Default is None.""" \ No newline at end of file + """The list of visualizer configurations. Default is None.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py index 289d4f75f8c..9c1f411490e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py @@ -177,7 +177,7 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 120.0 self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 # default friction material self.sim.physics_material.static_friction = 1.0 self.sim.physics_material.dynamic_friction = 1.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py index 37b9426df9b..b8e1db8c0a7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py @@ -214,7 +214,7 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 120.0 self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 # default friction material self.sim.physics_material.static_friction = 1.0 self.sim.physics_material.dynamic_friction = 1.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index d7094e77701..b76009415af 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -311,7 +311,7 @@ def __post_init__(self): self.sim.dt = 0.005 self.sim.render_interval = self.decimation self.sim.physics_material = self.scene.terrain.physics_material - self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 + self.sim.physics_manager_cfg.gpu_max_rigid_patch_count = 10 * 2**15 # update sensor update periods # we tick all the sensors based on the smallest update period (physics update period) if self.scene.height_scanner is not None: diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index 85b7e5ae9ba..49ec7abdbe2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -273,6 +273,6 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 60 # 60Hz self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py index f0a139607c0..ede3e8a0ad7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py @@ -277,5 +277,5 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 60 # 60Hz self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py index 3f521362a8a..f11fb35ad76 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py @@ -424,9 +424,9 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 120 self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_max_rigid_patch_count = 4 * 5 * 2**15 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.gpu_max_rigid_patch_count = 4 * 5 * 2**15 if self.curriculum is not None: self.curriculum.adr.params["pos_tol"] = self.rewards.success.params["pos_std"] / 2 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py index b2758f06a81..af41481270e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py @@ -233,7 +233,7 @@ def __post_init__(self): self.sim.dt = 0.01 # 100Hz self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physics_manager_cfg.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py index 272661bda61..2416420f835 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py @@ -215,8 +215,8 @@ def __post_init__(self): self.sim.dt = 0.01 # 100Hz self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physics_manager_cfg.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py index 224c19adbf6..cc8bed7510a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -192,11 +192,11 @@ def __post_init__(self): self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physics_manager_cfg.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 # set viewer to see the whole scene self.viewer.eye = [1.5, -1.0, 1.5] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py index e36a5407402..ea349b6a618 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py @@ -133,7 +133,7 @@ def __post_init__(self): self.episode_length_s = 30.0 # Enable CCD to avoid tunneling - self.sim.physx.enable_ccd = True + self.sim.physics_manager_cfg.enable_ccd = True self.teleop_devices = DevicesCfg( devices={ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py index 5c772a11760..aadd440688b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py @@ -192,8 +192,8 @@ def __post_init__(self): self.sim.dt = 0.01 # 100Hz self.sim.render_interval = 2 - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physics_manager_cfg.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py index 526297b9561..3afdff9037f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py @@ -128,8 +128,8 @@ def __post_init__(self): self.sim.dt = 0.01 # 100Hz self.sim.render_interval = self.decimation - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 + self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 + self.sim.physics_manager_cfg.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physics_manager_cfg.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 From dc20d8e5bfb910809bb0fafba802b331599ab213 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 07:01:19 -0800 Subject: [PATCH 122/130] refactor step 17: refactored simulation_cfg --- scripts/benchmarks/benchmark_cameras.py | 2 +- scripts/demos/quadcopter.py | 2 +- .../state_machine/lift_cube_sm.py | 2 +- .../state_machine/lift_teddy_bear.py | 2 +- .../state_machine/open_cabinet_sm.py | 2 +- .../03_envs/create_cartpole_base_env.py | 2 +- .../tutorials/03_envs/create_cube_base_env.py | 2 +- .../03_envs/create_quadruped_base_env.py | 4 +- .../03_envs/policy_inference_in_usd.py | 2 +- .../isaaclab/isaaclab/envs/direct_marl_env.py | 6 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 6 +- .../isaaclab/envs/manager_based_env.py | 8 +- source/isaaclab/isaaclab/envs/mdp/events.py | 4 +- .../isaaclab/managers/recorder_manager.py | 2 +- .../isaaclab/physics/physics_manager_cfg.py | 29 ++++- .../isaaclab/physics/physx_manager.py | 104 +++++++++--------- .../isaaclab/physics/physx_manager_cfg.py | 43 ++++++-- .../isaaclab/sim/physics_interface.py | 16 +-- .../isaaclab/isaaclab/sim/simulation_cfg.py | 32 ------ .../isaaclab/sim/simulation_context.py | 21 ++-- .../isaaclab/sim/visualizer_interface.py | 2 +- .../visualizers/physx_ov_visualizer.py | 4 +- .../isaaclab/test/assets/test_articulation.py | 46 ++++---- .../test/assets/test_deformable_object.py | 10 +- .../isaaclab/test/assets/test_rigid_object.py | 36 +++--- .../assets/test_rigid_object_collection.py | 24 ++-- .../test/assets/test_surface_gripper.py | 4 +- .../isaaclab/test/devices/test_oxr_device.py | 2 +- ...eck_manager_based_env_anymal_locomotion.py | 4 +- .../check_manager_based_env_floating_cube.py | 2 +- .../test/envs/test_color_randomization.py | 2 +- .../test/envs/test_env_rendering_logic.py | 4 +- .../test/envs/test_manager_based_env.py | 4 +- .../test/envs/test_manager_based_rl_env_ui.py | 2 +- .../test/envs/test_scale_randomization.py | 2 +- .../test/envs/test_texture_randomization.py | 2 +- .../test/managers/test_recorder_manager.py | 2 +- source/isaaclab/test/sensors/test_camera.py | 2 +- .../test_build_simulation_context_headless.py | 10 +- ...st_build_simulation_context_nonheadless.py | 10 +- .../test/sim/test_simulation_context.py | 2 +- .../envs/g1_locomanipulation_sdg_env.py | 2 +- .../direct/automate/assembly_env.py | 2 +- .../direct/automate/disassembly_env.py | 2 +- .../direct/factory/factory_env.py | 2 +- .../franka_cabinet/franka_cabinet_env.py | 2 +- .../direct/locomotion/locomotion_env.py | 4 +- .../direct/quadcopter/quadcopter_env.py | 2 +- .../manager_based/classic/ant/ant_env_cfg.py | 2 +- .../classic/cartpole/cartpole_env_cfg.py | 2 +- .../classic/humanoid/humanoid_env_cfg.py | 2 +- .../fixed_base_upper_body_ik_g1_env_cfg.py | 2 +- .../pick_place/locomanipulation_g1_env_cfg.py | 2 +- .../velocity/config/digit/rough_env_cfg.py | 6 +- .../velocity/config/spot/flat_env_cfg.py | 4 +- .../locomotion/velocity/velocity_env_cfg.py | 6 +- .../manipulation/cabinet/cabinet_env_cfg.py | 2 +- .../config/openarm/cabinet_openarm_env_cfg.py | 2 +- .../deploy/reach/reach_env_cfg.py | 2 +- .../manipulation/dexsuite/dexsuite_env_cfg.py | 2 +- .../manipulation/inhand/inhand_env_cfg.py | 2 +- .../config/openarm/lift_openarm_env_cfg.py | 2 +- .../manipulation/lift/lift_env_cfg.py | 2 +- .../exhaustpipe_gr1t2_base_env_cfg.py | 2 +- .../pick_place/nutpour_gr1t2_base_env_cfg.py | 2 +- .../pick_place/pickplace_gr1t2_env_cfg.py | 2 +- .../pickplace_gr1t2_waist_enabled_env_cfg.py | 2 +- ...ckplace_unitree_g1_inspire_hand_env_cfg.py | 2 +- .../agibot/place_toy2box_rmp_rel_env_cfg.py | 2 +- .../place_upright_mug_rmp_rel_env_cfg.py | 2 +- .../bimanual/reach_openarm_bi_env_cfg.py | 2 +- .../unimanual/reach_openarm_uni_env_cfg.py | 2 +- .../manipulation/reach/reach_env_cfg.py | 2 +- .../config/galbot/stack_rmp_rel_env_cfg.py | 4 +- .../ur10_gripper/stack_joint_pos_env_cfg.py | 2 +- .../manipulation/stack/stack_env_cfg.py | 2 +- .../stack/stack_instance_randomize_env_cfg.py | 2 +- .../config/anymal_c/navigation_env_cfg.py | 6 +- .../isaaclab_tasks/utils/parse_cfg.py | 2 +- 79 files changed, 291 insertions(+), 270 deletions(-) diff --git a/scripts/benchmarks/benchmark_cameras.py b/scripts/benchmarks/benchmark_cameras.py index 15d4284308e..b31f6655c0e 100644 --- a/scripts/benchmarks/benchmark_cameras.py +++ b/scripts/benchmarks/benchmark_cameras.py @@ -509,7 +509,7 @@ def inject_cameras_into_task( """Loads the task, sticks cameras into the config, and creates the environment.""" cfg = load_cfg_from_registry(task, "env_cfg_entry_point") cfg.sim.device = args_cli.device - cfg.sim.use_fabric = args_cli.use_fabric + cfg.sim.physics_manager_cfg.use_fabric = args_cli.use_fabric scene_cfg = cfg.scene num_envs = int(num_cams / num_cameras_per_env) diff --git a/scripts/demos/quadcopter.py b/scripts/demos/quadcopter.py index 2296245d510..52d8eefc140 100644 --- a/scripts/demos/quadcopter.py +++ b/scripts/demos/quadcopter.py @@ -73,7 +73,7 @@ def main(): # Fetch relevant parameters to make the quadcopter hover in place prop_body_ids = robot.find_bodies("m.*_prop")[0] robot_mass = robot.root_physx_view.get_masses().sum() - gravity = torch.tensor(sim.cfg.gravity, device=sim.device).norm() + gravity = torch.tensor(sim.cfg.physics_manager_cfg.gravity, device=sim.device).norm() # Now we are ready! print("[INFO]: Setup complete...") diff --git a/scripts/environments/state_machine/lift_cube_sm.py b/scripts/environments/state_machine/lift_cube_sm.py index 21338f74ed6..7d1b6d91880 100644 --- a/scripts/environments/state_machine/lift_cube_sm.py +++ b/scripts/environments/state_machine/lift_cube_sm.py @@ -276,7 +276,7 @@ def main(): desired_orientation[:, 1] = 1.0 # create state machine pick_sm = PickAndLiftSm( - env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device, position_threshold=0.01 + env_cfg.sim.physics_manager_cfg.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device, position_threshold=0.01 ) while simulation_app.is_running(): diff --git a/scripts/environments/state_machine/lift_teddy_bear.py b/scripts/environments/state_machine/lift_teddy_bear.py index ad4e560fa65..372ae185c32 100644 --- a/scripts/environments/state_machine/lift_teddy_bear.py +++ b/scripts/environments/state_machine/lift_teddy_bear.py @@ -297,7 +297,7 @@ def main(): object_local_grasp_position = torch.tensor([0.02, -0.08, 0.0], device=env.unwrapped.device) # create state machine - pick_sm = PickAndLiftSm(env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) + pick_sm = PickAndLiftSm(env_cfg.sim.physics_manager_cfg.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) while simulation_app.is_running(): # run everything in inference mode diff --git a/scripts/environments/state_machine/open_cabinet_sm.py b/scripts/environments/state_machine/open_cabinet_sm.py index 94c8d4d832e..b3e2b6858e9 100644 --- a/scripts/environments/state_machine/open_cabinet_sm.py +++ b/scripts/environments/state_machine/open_cabinet_sm.py @@ -293,7 +293,7 @@ def main(): desired_orientation = torch.zeros((env.unwrapped.num_envs, 4), device=env.unwrapped.device) desired_orientation[:, 1] = 1.0 # create state machine - open_sm = OpenDrawerSm(env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) + open_sm = OpenDrawerSm(env_cfg.sim.physics_manager_cfg.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) while simulation_app.is_running(): # run everything in inference mode diff --git a/scripts/tutorials/03_envs/create_cartpole_base_env.py b/scripts/tutorials/03_envs/create_cartpole_base_env.py index cbaf2b07086..53f0dd2ce86 100644 --- a/scripts/tutorials/03_envs/create_cartpole_base_env.py +++ b/scripts/tutorials/03_envs/create_cartpole_base_env.py @@ -132,7 +132,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz def main(): diff --git a/scripts/tutorials/03_envs/create_cube_base_env.py b/scripts/tutorials/03_envs/create_cube_base_env.py index 09ee7fac30f..cb227de2ecf 100644 --- a/scripts/tutorials/03_envs/create_cube_base_env.py +++ b/scripts/tutorials/03_envs/create_cube_base_env.py @@ -301,7 +301,7 @@ def __post_init__(self): # general settings self.decimation = 2 # simulation settings - self.sim.dt = 0.01 + self.sim.physics_manager_cfg.dt = 0.01 self.sim.physics_material = self.scene.terrain.physics_material self.sim.render_interval = 2 # render interval should be a multiple of decimation self.sim.device = args_cli.device diff --git a/scripts/tutorials/03_envs/create_quadruped_base_env.py b/scripts/tutorials/03_envs/create_quadruped_base_env.py index 78f5b75ec5f..90d4934adab 100644 --- a/scripts/tutorials/03_envs/create_quadruped_base_env.py +++ b/scripts/tutorials/03_envs/create_quadruped_base_env.py @@ -192,13 +192,13 @@ def __post_init__(self): # general settings self.decimation = 4 # env decimation -> 50 Hz control # simulation settings - self.sim.dt = 0.005 # simulation timestep -> 200 Hz physics + self.sim.physics_manager_cfg.dt = 0.005 # simulation timestep -> 200 Hz physics self.sim.physics_material = self.scene.terrain.physics_material self.sim.device = args_cli.device # update sensor update periods # we tick all the sensors based on the smallest update period (physics update period) if self.scene.height_scanner is not None: - self.scene.height_scanner.update_period = self.decimation * self.sim.dt # 50 Hz + self.scene.height_scanner.update_period = self.decimation * self.sim.physics_manager_cfg.dt # 50 Hz def main(): diff --git a/scripts/tutorials/03_envs/policy_inference_in_usd.py b/scripts/tutorials/03_envs/policy_inference_in_usd.py index 4cf9723932f..ee178ef0a18 100644 --- a/scripts/tutorials/03_envs/policy_inference_in_usd.py +++ b/scripts/tutorials/03_envs/policy_inference_in_usd.py @@ -69,7 +69,7 @@ def main(): ) env_cfg.sim.device = args_cli.device if args_cli.device == "cpu": - env_cfg.sim.use_fabric = False + env_cfg.sim.physics_manager_cfg.use_fabric = False # create environment env = ManagerBasedRLEnv(cfg=env_cfg) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 41c58ee390b..acd493270c1 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -254,7 +254,7 @@ def physics_dt(self) -> float: This is the lowest time-decimation at which the simulation is happening. """ - return self.cfg.sim.dt + return self.cfg.sim.physics_manager_cfg.dt @property def step_dt(self) -> float: @@ -262,7 +262,7 @@ def step_dt(self) -> float: This is the time-step at which the environment steps forward. """ - return self.cfg.sim.dt * self.cfg.decimation + return self.cfg.sim.physics_manager_cfg.dt * self.cfg.decimation @property def device(self): @@ -277,7 +277,7 @@ def max_episode_length_s(self) -> float: @property def max_episode_length(self): """The maximum episode length in steps adjusted from s.""" - return math.ceil(self.max_episode_length_s / (self.cfg.sim.dt * self.cfg.decimation)) + return math.ceil(self.max_episode_length_s / (self.cfg.sim.physics_manager_cfg.dt * self.cfg.decimation)) """ Space methods diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 9e7e0280bd4..a9c861d7057 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -256,7 +256,7 @@ def physics_dt(self) -> float: This is the lowest time-decimation at which the simulation is happening. """ - return self.cfg.sim.dt + return self.cfg.sim.physics_manager_cfg.dt @property def step_dt(self) -> float: @@ -264,7 +264,7 @@ def step_dt(self) -> float: This is the time-step at which the environment steps forward. """ - return self.cfg.sim.dt * self.cfg.decimation + return self.cfg.sim.physics_manager_cfg.dt * self.cfg.decimation @property def device(self): @@ -279,7 +279,7 @@ def max_episode_length_s(self) -> float: @property def max_episode_length(self): """The maximum episode length in steps adjusted from s.""" - return math.ceil(self.max_episode_length_s / (self.cfg.sim.dt * self.cfg.decimation)) + return math.ceil(self.max_episode_length_s / (self.cfg.sim.physics_manager_cfg.dt * self.cfg.decimation)) """ Operations. diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 95c5e0d3ffa..324dd9ae01d 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -66,7 +66,7 @@ class ManagerBasedEnv: The environment steps forward in time at a fixed time-step. The physics simulation is decimated at a lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of simulation steps per environment step) - and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step) parameters. Based on these parameters, the + and the :attr:`ManagerBasedEnvCfg.sim.physics_manager_cfg.dt` (physics time-step) parameters. Based on these parameters, the environment time-step is computed as the product of the two. The two time-steps can be obtained by querying the :attr:`physics_dt` and the :attr:`step_dt` properties respectively. """ @@ -221,7 +221,7 @@ def physics_dt(self) -> float: This is the lowest time-decimation at which the simulation is happening. """ - return self.cfg.sim.dt + return self.cfg.sim.physics_manager_cfg.dt @property def step_dt(self) -> float: @@ -229,7 +229,7 @@ def step_dt(self) -> float: This is the time-step at which the environment steps forward. """ - return self.cfg.sim.dt * self.cfg.decimation + return self.cfg.sim.physics_manager_cfg.dt * self.cfg.decimation @property def device(self): @@ -445,7 +445,7 @@ def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: The environment steps forward at a fixed time-step, while the physics simulation is decimated at a lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of - simulation steps per environment step) and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step). + simulation steps per environment step) and the :attr:`ManagerBasedEnvCfg.sim.physics_manager_cfg.dt` (physics time-step). Based on these parameters, the environment time-step is computed as the product of the two. Args: diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index cf024553b94..35855724237 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -517,7 +517,7 @@ def randomize_physics_scene_gravity( This function uses CPU tensors to assign gravity. """ # get the current gravity - gravity = torch.tensor(env.sim.cfg.gravity, device="cpu").unsqueeze(0) + gravity = torch.tensor(env.sim.cfg.physics_manager_cfg.gravity, device="cpu").unsqueeze(0) dist_param_0 = torch.tensor(gravity_distribution_params[0], device="cpu") dist_param_1 = torch.tensor(gravity_distribution_params[1], device="cpu") gravity = _randomize_prop_by_op( @@ -532,7 +532,7 @@ def randomize_physics_scene_gravity( gravity = gravity[0].tolist() # set the gravity into the physics simulation - physics_sim_view: physx.SimulationView = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view + physics_sim_view: physx.SimulationView = sim_utils.SimulationContext.instance()._physics_interface.physics_sim_view physics_sim_view.set_gravity(carb.Float3(*gravity)) diff --git a/source/isaaclab/isaaclab/managers/recorder_manager.py b/source/isaaclab/isaaclab/managers/recorder_manager.py index dd254dc145b..9e1b6047601 100644 --- a/source/isaaclab/isaaclab/managers/recorder_manager.py +++ b/source/isaaclab/isaaclab/managers/recorder_manager.py @@ -437,7 +437,7 @@ def get_ep_meta(self) -> dict: # Add basic episode metadata ep_meta = dict() ep_meta["sim_args"] = { - "dt": self._env.cfg.sim.dt, + "dt": self._env.cfg.sim.physics_manager_cfg.dt, "decimation": self._env.cfg.decimation, "render_interval": self._env.cfg.sim.render_interval, "num_envs": self._env.cfg.scene.num_envs, diff --git a/source/isaaclab/isaaclab/physics/physics_manager_cfg.py b/source/isaaclab/isaaclab/physics/physics_manager_cfg.py index f30d0fa24f8..e523834b816 100644 --- a/source/isaaclab/isaaclab/physics/physics_manager_cfg.py +++ b/source/isaaclab/isaaclab/physics/physics_manager_cfg.py @@ -19,10 +19,35 @@ class PhysicsManagerCfg: """Abstract base configuration for physics managers. - Subclasses should override :meth:`create_manager` to return the appropriate - physics manager class. + This base class contains common simulation parameters shared across + all physics backends. Subclasses should override :meth:`create_manager` + to return the appropriate physics manager class. """ + # ------------------------------------------------------------------ + # Common Simulation Parameters + # ------------------------------------------------------------------ + + dt: float = 1.0 / 60.0 + """The physics simulation time-step (in seconds). Default is 0.0167 seconds.""" + + device: str = "cuda:0" + """The device to run the simulation on. Default is ``"cuda:0"``. + + Valid options are: + + - ``"cpu"``: Use CPU. + - ``"cuda"``: Use GPU, where the device ID is inferred from config. + - ``"cuda:N"``: Use GPU, where N is the device ID. For example, "cuda:0". + """ + + gravity: tuple[float, float, float] = (0.0, 0.0, -9.81) + """The gravity vector (in m/s^2). Default is (0.0, 0.0, -9.81).""" + + # ------------------------------------------------------------------ + # Factory Method + # ------------------------------------------------------------------ + def create_manager(self) -> type["PhysicsManager"]: """Create and return the physics manager class for this configuration. diff --git a/source/isaaclab/isaaclab/physics/physx_manager.py b/source/isaaclab/isaaclab/physics/physx_manager.py index c3a6db0bfd6..1432bcf3c31 100644 --- a/source/isaaclab/isaaclab/physics/physx_manager.py +++ b/source/isaaclab/isaaclab/physics/physx_manager.py @@ -36,6 +36,7 @@ if TYPE_CHECKING: from isaaclab.sim.simulation_context import SimulationContext + from .physx_manager_cfg import PhysxManagerCfg __all__ = ["IsaacEvents", "PhysxManager"] @@ -239,10 +240,13 @@ class PhysxManager(PhysicsManager): _callback_id: int = 0 _handles: dict = {} # Named internal handles - # Simulation context reference + # Simulation context reference (for stage, carb_settings, logger access) _sim: "SimulationContext | None" = None - # Device and fabric state (from PhysXBackend) + # Manager configuration (contains all physics settings) + _cfg: "PhysxManagerCfg | None" = None + + # Device and fabric state _physics_device: str = "cpu" _fabric_iface = None _update_fabric = None @@ -300,9 +304,11 @@ def initialize(cls, sim_context: "SimulationContext") -> None: """Initialize the manager with simulation context and set up physics. Args: - sim_context: Parent simulation context. + sim_context: Parent simulation context (provides stage, carb_settings, logger). """ cls._sim = sim_context + # Store config reference for easy access + cls._cfg = sim_context.cfg.physics_manager_cfg # type: ignore[assignment] cls._setup_callbacks() cls._track_physics_scenes() @@ -353,7 +359,7 @@ def forward(cls) -> None: @classmethod def step(cls) -> None: """Step the physics simulation (physics only, no rendering).""" - if cls._sim is None: + if cls._cfg is None: return # update animation recorder if enabled @@ -363,7 +369,7 @@ def step(cls) -> None: return # step physics only - cls._physx_sim_interface.simulate(cls._sim.cfg.dt, 0.0) + cls._physx_sim_interface.simulate(cls._cfg.dt, 0.0) cls._physx_sim_interface.fetch_results() # physics step may change cuda device, force it back @@ -380,6 +386,7 @@ def close(cls) -> None: cls._physx_sim_interface.detach_stage() # clear references cls._sim = None + cls._cfg = None cls._anim_recorder = None @classmethod @@ -441,9 +448,9 @@ def set_backend(cls, backend: str) -> None: @classmethod def get_physics_dt(cls) -> float: """Get the physics timestep in seconds.""" - # Prefer sim context config if available - if cls._sim is not None: - return cls._sim.cfg.dt + # Prefer config if available + if cls._cfg is not None: + return cls._cfg.dt # Fallback to USD scene if cls._physics_scene_apis: api = list(cls._physics_scene_apis.values())[0] @@ -694,24 +701,23 @@ def _enable_fabric(cls, enable: bool) -> None: @classmethod def _configure_simulation_dt(cls) -> None: """Configures the physics simulation step size.""" - if cls._sim is None: + if cls._sim is None or cls._cfg is None: return - cfg = cls._sim.cfg carb_settings = cls._sim.carb_settings # Get physics scene API from the physics interface stage = cls._sim.stage - physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) + physics_scene_prim = stage.GetPrimAtPath(cls._cfg.physics_prim_path) physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) # 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(cfg.render_interval, 1) + render_interval = max(cls._sim.cfg.render_interval, 1) # set simulation step per second - steps_per_second = int(1.0 / cfg.dt) + steps_per_second = int(1.0 / cls._cfg.dt) physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) # set minimum number of steps per frame min_steps = int(steps_per_second / render_interval) @@ -720,15 +726,14 @@ def _configure_simulation_dt(cls) -> None: @classmethod def _apply_physics_settings(cls) -> None: """Sets various carb physics settings.""" - if cls._sim is None: + if cls._sim is None or cls._cfg is None: return - cfg = cls._sim.cfg carb_settings = cls._sim.carb_settings # Get physics scene API from the physics interface stage = cls._sim.stage - physics_scene_prim = stage.GetPrimAtPath(cfg.physics_prim_path) + physics_scene_prim = stage.GetPrimAtPath(cls._cfg.physics_prim_path) physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) # -------------------------- @@ -744,12 +749,6 @@ def _apply_physics_settings(cls) -> None: # disable contact processing in omni.physx # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. # The physics flag gets enabled when a contact sensor is created. - if hasattr(cfg, "disable_contact_processing"): - cls._sim.logger.warning( - "The `disable_contact_processing` attribute is deprecated and always set to True" - " to avoid unnecessary overhead. Contact processing is automatically enabled when" - " a contact sensor is created, so manual configuration is no longer required." - ) # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts # are always processed. The issue is reported to the PhysX team by @mmittal. carb_settings.set_bool("/physics/disableContactProcessing", True) @@ -762,8 +761,8 @@ def _apply_physics_settings(cls) -> None: carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) # handle device settings - if "cuda" in cfg.device: - parsed_device = cfg.device.split(":") + if "cuda" in cls._cfg.device: + parsed_device = cls._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 @@ -797,78 +796,81 @@ def _apply_physics_settings(cls) -> None: # create the default physics material # this material is used when no material is specified for a primitive - material_path = f"{cfg.physics_prim_path}/defaultMaterial" - cfg.physics_material.func(material_path, cfg.physics_material) - # bind the physics material to the scene - sim_utils.bind_physics_material(cfg.physics_prim_path, material_path) + if cls._cfg.physics_material is not None: + material_path = f"{cls._cfg.physics_prim_path}/defaultMaterial" + cls._cfg.physics_material.func(material_path, cls._cfg.physics_material) + # bind the physics material to the scene + sim_utils.bind_physics_material(cls._cfg.physics_prim_path, material_path) # -------------------------- # PhysX API settings # -------------------------- # set broadphase type - broadphase_type = "GPU" if "cuda" in cfg.device else "MBP" + broadphase_type = "GPU" if "cuda" in cls._cfg.device else "MBP" physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) # set gpu dynamics - enable_gpu_dynamics = "cuda" in cfg.device + enable_gpu_dynamics = "cuda" in cls._cfg.device 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 cfg.physics_manager_cfg.enable_ccd: - cfg.physics_manager_cfg.enable_ccd = False + if enable_gpu_dynamics and cls._cfg.enable_ccd: + cls._cfg.enable_ccd = False cls._sim.logger.warning( - "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxCfg config to avoid this" + "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxManagerCfg to avoid this" " warning." ) - physx_scene_api.CreateEnableCCDAttr(cfg.physics_manager_cfg.enable_ccd) + physx_scene_api.CreateEnableCCDAttr(cls._cfg.enable_ccd) # set solver type - solver_type = "PGS" if cfg.physics_manager_cfg.solver_type == 0 else "TGS" + solver_type = "PGS" if cls._cfg.solver_type == 0 else "TGS" physx_scene_api.CreateSolverTypeAttr(solver_type) # set solve articulation contact last attr = physx_scene_api.GetPrim().CreateAttribute( "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool ) - attr.Set(cfg.physics_manager_cfg.solve_articulation_contact_last) + attr.Set(cls._cfg.solve_articulation_contact_last) # iterate over all the settings and set them - for key, value in cfg.physics_manager_cfg.to_dict().items(): # type: ignore - if key in ["solver_type", "enable_ccd", "solve_articulation_contact_last"]: + for key, value in cls._cfg.to_dict().items(): # type: ignore + # Skip non-PhysX settings and already-handled settings + if key in ["solver_type", "enable_ccd", "solve_articulation_contact_last", + "dt", "device", "render_interval", "gravity", + "physics_prim_path", "use_fabric", "physics_material"]: continue if key == "bounce_threshold_velocity": key = "bounce_threshold" sim_utils.safe_set_attribute_on_usd_schema(physx_scene_api, key, value, camel_case=True) # throw warnings for helpful guidance - if cfg.physics_manager_cfg.solver_type == 1 and not cfg.physics_manager_cfg.enable_external_forces_every_iteration: + if cls._cfg.solver_type == 1 and not cls._cfg.enable_external_forces_every_iteration: logger.warning( - "The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are" + "The `enable_external_forces_every_iteration` parameter in PhysxManagerCfg 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 cfg.physics_manager_cfg.enable_stabilization and cfg.dt > 0.0333: + if not cls._cfg.enable_stabilization and cls._cfg.dt > 0.0333: cls._sim.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" + " Consider setting the `enable_stabilization` flag to True in PhysxManagerCfg, or reducing the" " simulation step size if you run into physics issues." ) @classmethod def _load_fabric_interface(cls) -> None: """Loads the fabric interface if enabled.""" - if cls._sim is None: + if cls._sim is None or cls._cfg is None: return - cfg = cls._sim.cfg carb_settings = cls._sim.carb_settings extension_manager = omni.kit.app.get_app().get_extension_manager() fabric_enabled = extension_manager.is_extension_enabled("omni.physx.fabric") - if cfg.use_fabric: + if cls._cfg.use_fabric: if not fabric_enabled: extension_manager.set_extension_enabled_immediate("omni.physx.fabric", True) @@ -893,11 +895,11 @@ def _load_fabric_interface(cls) -> None: cls._fabric_iface = None # set carb settings for fabric - carb_settings.set_bool("/physics/fabricEnabled", cfg.use_fabric) - carb_settings.set_bool("/physics/updateToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateParticlesToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateVelocitiesToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateForceSensorsToUsd", not cfg.use_fabric) - carb_settings.set_bool("/physics/updateResidualsToUsd", not cfg.use_fabric) + carb_settings.set_bool("/physics/fabricEnabled", cls._cfg.use_fabric) + carb_settings.set_bool("/physics/updateToUsd", not cls._cfg.use_fabric) + carb_settings.set_bool("/physics/updateParticlesToUsd", not cls._cfg.use_fabric) + carb_settings.set_bool("/physics/updateVelocitiesToUsd", not cls._cfg.use_fabric) + carb_settings.set_bool("/physics/updateForceSensorsToUsd", not cls._cfg.use_fabric) + carb_settings.set_bool("/physics/updateResidualsToUsd", not cls._cfg.use_fabric) # disable simulation output window visibility carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) diff --git a/source/isaaclab/isaaclab/physics/physx_manager_cfg.py b/source/isaaclab/isaaclab/physics/physx_manager_cfg.py index e9be643c70a..55dbd179f21 100644 --- a/source/isaaclab/isaaclab/physics/physx_manager_cfg.py +++ b/source/isaaclab/isaaclab/physics/physx_manager_cfg.py @@ -15,26 +15,55 @@ if TYPE_CHECKING: from .physics_manager import PhysicsManager + from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg @configclass class PhysxManagerCfg(PhysicsManagerCfg): """Configuration for PhysX physics manager. - This configuration includes all PhysX solver settings. For more information, + This configuration includes all PhysX-specific settings including solver + parameters, scene configuration, and GPU buffer sizes. For more information, see the `PhysX 5 SDK documentation`_. PhysX 5 supports GPU-accelerated physics simulation. This is enabled by default, - but can be disabled by setting the :attr:`~SimulationCfg.device` to ``cpu`` in - :class:`SimulationCfg`. Unlike CPU PhysX, the GPU simulation feature is unable - to dynamically grow all the buffers. Therefore, it is necessary to provide a - reasonable estimate of the buffer sizes for GPU features. If insufficient buffer - sizes are provided, the simulation will fail with errors and lead to adverse - behaviors. The buffer sizes can be adjusted through the ``gpu_*`` parameters. + but can be disabled by setting :attr:`device` to ``cpu``. Unlike CPU PhysX, the + GPU simulation feature is unable to dynamically grow all the buffers. Therefore, + it is necessary to provide a reasonable estimate of the buffer sizes for GPU + features. If insufficient buffer sizes are provided, the simulation will fail + with errors and lead to adverse behaviors. The buffer sizes can be adjusted + through the ``gpu_*`` parameters. .. _PhysX 5 SDK documentation: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxSceneDesc.html """ + # ------------------------------------------------------------------ + # PhysX Scene Settings + # ------------------------------------------------------------------ + + physics_prim_path: str = "/physicsScene" + """The prim path where the USD PhysicsScene is created. Default is "/physicsScene".""" + + use_fabric: bool = True + """Enable/disable reading of physics buffers directly. Default is True. + + When running the simulation, updates in the states in the scene is normally synchronized with USD. + This leads to an overhead in reading the data and does not scale well with massive parallelization. + This flag allows disabling the synchronization and reading the data directly from the physics buffers. + + It is recommended to set this flag to :obj:`True` when running the simulation with a large number + of primitives in the scene. + """ + + physics_material: "RigidBodyMaterialCfg | None" = None + """Default physics material settings for rigid bodies. Default is None (uses RigidBodyMaterialCfg defaults). + + The physics engine defaults to this physics material for all the rigid body prims that do not have any + physics material specified on them. + + The material is created at the path: ``{physics_prim_path}/defaultMaterial``. + """ + # ------------------------------------------------------------------ # Solver Settings # ------------------------------------------------------------------ diff --git a/source/isaaclab/isaaclab/sim/physics_interface.py b/source/isaaclab/isaaclab/sim/physics_interface.py index 57084d9606b..3e55b8b4ad1 100644 --- a/source/isaaclab/isaaclab/sim/physics_interface.py +++ b/source/isaaclab/isaaclab/sim/physics_interface.py @@ -40,11 +40,13 @@ def __init__(self, sim_context: "SimulationContext"): """ super().__init__(sim_context) - self.physics_prim_path = self._sim.cfg.physics_prim_path + # Get config reference + self._cfg = self._sim.cfg.physics_manager_cfg + self.physics_prim_path = self._cfg.physics_prim_path self.backend = "torch" # Get the physics manager class from config - self.physics_manager: type[PhysicsManager] = self._sim.cfg.physics_manager_cfg.create_manager() + self.physics_manager: type[PhysicsManager] = self._cfg.create_manager() # Initialize USD physics scene self._init_usd_physics_scene() @@ -59,7 +61,7 @@ def physics_dt(self) -> float: @property def rendering_dt(self) -> float: """Rendering timestep.""" - return self._sim.cfg.dt * self._sim.cfg.render_interval + return self._cfg.dt * self._cfg.render_interval @property def device(self) -> str: @@ -90,13 +92,13 @@ def _init_usd_physics_scene(self) -> None: sim_utils.delete_prim(prim.GetPath().pathString, stage=stage) # Create a new physics scene - if stage.GetPrimAtPath(self._sim.cfg.physics_prim_path).IsValid(): - raise RuntimeError(f"A prim already exists at path '{self._sim.cfg.physics_prim_path}'.") + if stage.GetPrimAtPath(self._cfg.physics_prim_path).IsValid(): + raise RuntimeError(f"A prim already exists at path '{self._cfg.physics_prim_path}'.") - physics_scene = UsdPhysics.Scene.Define(stage, self._sim.cfg.physics_prim_path) + physics_scene = UsdPhysics.Scene.Define(stage, self._cfg.physics_prim_path) # Pre-create gravity tensor to avoid torch heap corruption issues (torch 2.1+) - gravity = torch.tensor(self._sim.cfg.gravity, dtype=torch.float32, device=self._sim.cfg.device) + gravity = torch.tensor(self._cfg.gravity, dtype=torch.float32, device=self._cfg.device) gravity_magnitude = torch.norm(gravity).item() # Avoid division by zero diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index 5988d40d972..be5a2576134 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -168,9 +168,6 @@ class RenderCfg: class SimulationCfg: """Configuration for simulation physics.""" - physics_prim_path: str = "/physicsScene" - """The prim path where the USD PhysicsScene is created. Default is "/physicsScene".""" - device: str = "cuda:0" """The device to run the simulation on. Default is ``"cuda:0"``. @@ -181,18 +178,9 @@ class SimulationCfg: - ``"cuda:N"``: Use GPU, where N is the device ID. For example, "cuda:0". """ - dt: float = 1.0 / 60.0 - """The physics simulation time-step (in seconds). Default is 0.0167 seconds.""" - render_interval: int = 1 """The number of physics simulation steps per rendering step. Default is 1.""" - gravity: tuple[float, float, float] = (0.0, 0.0, -9.81) - """The gravity vector (in m/s^2). Default is (0.0, 0.0, -9.81). - - If set to (0.0, 0.0, 0.0), gravity is disabled. - """ - enable_scene_query_support: bool = False """Enable/disable scene query support for collision shapes. Default is False. @@ -208,26 +196,6 @@ class SimulationCfg: with the GUI enabled. This is to allow certain GUI features to work properly. """ - use_fabric: bool = True - """Enable/disable reading of physics buffers directly. Default is True. - - When running the simulation, updates in the states in the scene is normally synchronized with USD. - This leads to an overhead in reading the data and does not scale well with massive parallelization. - This flag allows disabling the synchronization and reading the data directly from the physics buffers. - - It is recommended to set this flag to :obj:`True` when running the simulation with a large number - of primitives in the scene. - - Note: - When enabled, the GUI will not update the physics parameters in real-time. To enable real-time - updates, please set this flag to :obj:`False`. - - When using GPU simulation, it is required to enable Fabric to visualize updates in the renderer. - Transform updates are propagated to the renderer through Fabric. If Fabric is disabled with GPU simulation, - the renderer will not be able to render any updates in the simulation, although simulation will still be - running under the hood. - """ - physics_manager_cfg: PhysicsManagerCfg = PhysxManagerCfg() """Physics manager configuration. Default is PhysxManagerCfg(). diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 906f6ea872b..14da3be3731 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -230,12 +230,12 @@ def get_physics_dt(self) -> float: Returns: The physics time step of the simulation. """ - return self.cfg.dt + return self.cfg.physics_manager_cfg.dt @property def physics_prim_path(self) -> str: """The path to the physics scene prim.""" - return self.cfg.physics_prim_path + return self.cfg.physics_manager_cfg.physics_prim_path def get_rendering_dt(self) -> float: """Returns the rendering time step of the simulation. @@ -243,7 +243,7 @@ def get_rendering_dt(self) -> float: Returns: The rendering time step of the simulation. """ - return self.cfg.dt * self.cfg.render_interval + return self.cfg.physics_manager_cfg.dt * self.cfg.render_interval """ Operations - Utilities. @@ -496,17 +496,12 @@ def build_simulation_context( sim_utils.create_new_stage() if sim_cfg is None: - # Construct one and overwrite the dt, gravity, and device - sim_cfg = SimulationCfg(dt=dt) + # Construct one and overwrite the dt, gravity, and device in physics_manager_cfg + from isaaclab.physics.physx_manager_cfg import PhysxManagerCfg - # Set up gravity - if gravity_enabled: - sim_cfg.gravity = (0.0, 0.0, -9.81) - else: - sim_cfg.gravity = (0.0, 0.0, 0.0) - - # Set device - sim_cfg.device = device + gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) + physics_manager_cfg = PhysxManagerCfg(dt=dt, device=device, gravity=gravity) + sim_cfg = SimulationCfg(physics_manager_cfg=physics_manager_cfg) # Construct simulation context sim = SimulationContext(sim_cfg) diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/visualizer_interface.py index 9fab9486c41..2866174eb9d 100644 --- a/source/isaaclab/isaaclab/sim/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/visualizer_interface.py @@ -31,7 +31,7 @@ def __init__(self, sim_context: "SimulationContext"): sim_context: Parent simulation context. """ super().__init__(sim_context) - self.dt = self._sim.cfg.dt * self._sim.cfg.render_interval + self.dt = self._sim.cfg.physics_manager_cfg.dt * self._sim.cfg.render_interval # Visualizer state visualizers = "omniverse" diff --git a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py index cf68bbd6feb..27e44b47aa6 100644 --- a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py @@ -737,7 +737,7 @@ def _configure_rendering_dt(self): # compute rendering frequency render_interval = max(cfg.render_interval, 1) - rendering_hz = int(1.0 / (cfg.dt * render_interval)) + rendering_hz = int(1.0 / (cfg.physics_manager_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 @@ -756,7 +756,7 @@ def _configure_rendering_dt(self): import omni.kit.loop._loop as omni_loop _loop_runner = omni_loop.acquire_loop_interface() - _loop_runner.set_manual_step_size(cfg.dt * render_interval) + _loop_runner.set_manual_step_size(cfg.physics_manager_cfg.dt * render_interval) _loop_runner.set_manual_mode(True) except Exception: logger.warning( diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index ab6c9534fa9..57fdb9744e2 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -252,7 +252,7 @@ def test_initialization_floating_base_non_root(sim, num_articulations, device, a # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_articulations", [1, 2]) @@ -310,7 +310,7 @@ def test_initialization_floating_base(sim, num_articulations, device, add_ground # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_articulations", [1, 2]) @@ -367,7 +367,7 @@ def test_initialization_fixed_base(sim, num_articulations, device): # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check that the root is at the correct state - its default state as it is fixed base default_root_state = articulation.data.default_root_state.clone() @@ -431,7 +431,7 @@ def test_initialization_fixed_base_single_joint(sim, num_articulations, device, # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check that the root is at the correct state - its default state as it is fixed base default_root_state = articulation.data.default_root_state.clone() @@ -491,7 +491,7 @@ def test_initialization_hand_with_tendons(sim, num_articulations, device): # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_articulations", [1, 2]) @@ -544,7 +544,7 @@ def test_initialization_floating_base_made_fixed_base(sim, num_articulations, de # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check that the root is at the correct state - its default state as it is fixed base default_root_state = articulation.data.default_root_state.clone() @@ -603,7 +603,7 @@ def test_initialization_fixed_base_made_floating_base(sim, num_articulations, de # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_articulations", [1, 2]) @@ -861,7 +861,7 @@ def test_external_force_buffer(sim, num_articulations, device): sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_articulations", [1, 2]) @@ -917,7 +917,7 @@ def test_external_force_on_single_body(sim, num_articulations, device): # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check condition that the articulations have fallen down for i in range(num_articulations): assert articulation.data.root_pos_w[i, 2].item() < 0.2 @@ -984,7 +984,7 @@ def test_external_force_on_single_body_at_position(sim, num_articulations, devic # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check condition that the articulations have fallen down for i in range(num_articulations): assert articulation.data.root_pos_w[i, 2].item() < 0.2 @@ -1042,7 +1042,7 @@ def test_external_force_on_multiple_bodies(sim, num_articulations, device): # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check condition for i in range(num_articulations): # since there is a moment applied on the articulation, the articulation should rotate @@ -1108,7 +1108,7 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check condition for i in range(num_articulations): # since there is a moment applied on the articulation, the articulation should rotate @@ -1523,7 +1523,7 @@ def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # reset dof state joint_pos = articulation.data.default_joint_pos @@ -1537,7 +1537,7 @@ def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # Check that current joint position is not the same as default joint position, meaning # the articulation moved. We can't check that it reached its desired joint position as the gains @@ -1597,7 +1597,7 @@ def test_body_root_state(sim, num_articulations, device, with_offset): # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # get state properties root_state_w = articulation.data.root_state_w @@ -1717,7 +1717,7 @@ def test_write_root_state(sim, num_articulations, device, with_offset, state_loc # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) if state_location == "com": if i % 2 == 0: @@ -1778,7 +1778,7 @@ def test_body_incoming_joint_wrench_b_single_joint(sim, num_articulations, devic # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # check shape assert articulation.data.body_incoming_joint_wrench_b.shape == (num_articulations, articulation.num_bodies, 6) @@ -1789,7 +1789,7 @@ def test_body_incoming_joint_wrench_b_single_joint(sim, num_articulations, devic quat_w = articulation.data.body_quat_w mass_link2 = mass[:, 1].view(num_articulations, -1) - gravity = torch.tensor(sim.cfg.gravity, device="cpu").repeat(num_articulations, 1).view((num_articulations, 3)) + gravity = torch.tensor(sim.cfg.physics_manager_cfg.gravity, device="cpu").repeat(num_articulations, 1).view((num_articulations, 3)) # NOTE: the com and link pose for single joint are colocated weight_vector_w = mass_link2 * gravity @@ -1988,7 +1988,7 @@ def test_spatial_tendons(sim, num_articulations, device): # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -2008,7 +2008,7 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # apply action to the articulation dynamic_friction = torch.rand(num_articulations, articulation.num_joints, device=device) @@ -2030,7 +2030,7 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground # perform step sim.step() # update buffers - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) if get_isaac_sim_version().major >= 5: friction_props_from_sim = articulation.root_physx_view.get_dof_friction_properties() @@ -2053,7 +2053,7 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground # Warm up a few steps to populate buffers for _ in range(100): sim.step() - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) # New random coefficients dynamic_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) @@ -2074,7 +2074,7 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground # Step to let sim ingest new params and refresh data buffers for _ in range(100): sim.step() - articulation.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) friction_props_from_sim_2 = articulation.root_physx_view.get_dof_friction_properties() joint_friction_coeff_sim_2 = friction_props_from_sim_2[:, :, 0] diff --git a/source/isaaclab/test/assets/test_deformable_object.py b/source/isaaclab/test/assets/test_deformable_object.py index 90c183e38f6..34e8158fe30 100644 --- a/source/isaaclab/test/assets/test_deformable_object.py +++ b/source/isaaclab/test/assets/test_deformable_object.py @@ -133,7 +133,7 @@ def test_initialization(sim, num_cubes, material_path): # Simulate physics for _ in range(2): sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # Check sim data assert cube_object.data.sim_element_quat_w.shape == (num_cubes, cube_object.max_sim_elements_per_body, 4) @@ -200,7 +200,7 @@ def test_initialization_with_kinematic_enabled(sim, num_cubes): # Simulate physics for _ in range(2): sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) default_nodal_state_w = cube_object.data.default_nodal_state_w.clone() torch.testing.assert_close(cube_object.data.nodal_state_w, default_nodal_state_w) @@ -254,7 +254,7 @@ def test_set_nodal_state(sim, num_cubes): torch.testing.assert_close(cube_object.data.nodal_state_w, nodal_state, rtol=1e-5, atol=1e-5) sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_cubes", [1, 2]) @@ -300,7 +300,7 @@ def test_set_nodal_state_with_applied_transform(sim, num_cubes, randomize_pos, r for _ in range(50): sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) torch.testing.assert_close(cube_object.data.root_pos_w, mean_nodal_pos_init, rtol=1e-5, atol=1e-5) @@ -331,7 +331,7 @@ def test_set_kinematic_targets(sim, num_cubes): for _ in range(20): sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) torch.testing.assert_close( cube_object.data.nodal_pos_w[0], nodal_kinematic_targets[0, :, :3], rtol=1e-5, atol=1e-5 diff --git a/source/isaaclab/test/assets/test_rigid_object.py b/source/isaaclab/test/assets/test_rigid_object.py index 3a4d34847e5..2d7c1e5d79e 100644 --- a/source/isaaclab/test/assets/test_rigid_object.py +++ b/source/isaaclab/test/assets/test_rigid_object.py @@ -125,7 +125,7 @@ def test_initialization(num_cubes, device): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_cubes", [1, 2]) @@ -157,7 +157,7 @@ def test_initialization_with_kinematic_enabled(num_cubes, device): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # check that the object is kinematic default_root_state = cube_object.data.default_root_state.clone() default_root_state[:, :3] += origins @@ -276,7 +276,7 @@ def test_external_force_buffer(device): sim.step() # update buffers - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_cubes", [2, 4]) @@ -331,7 +331,7 @@ def test_external_force_on_single_body(num_cubes, device): sim.step() # update buffers - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # First object should still be at the same Z position (1.0) torch.testing.assert_close( @@ -397,7 +397,7 @@ def test_external_force_on_single_body_at_position(num_cubes, device): sim.step() # update buffers - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # The first object should be rotating around it's X axis assert torch.all(torch.abs(cube_object.data.root_ang_vel_b[0::2, 0]) > 0.1) @@ -469,7 +469,7 @@ def test_set_rigid_object_state(num_cubes, device): value = getattr(cube_object.data, key) torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_cubes", [1, 2]) @@ -490,7 +490,7 @@ def test_reset_rigid_object(num_cubes, device): sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # Move the object to a random position root_state = cube_object.data.default_root_state.clone() @@ -541,7 +541,7 @@ def test_rigid_body_set_material_properties(num_cubes, device): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # Get material properties materials_to_check = cube_object.root_physx_view.get_material_properties() @@ -595,7 +595,7 @@ def test_rigid_body_no_friction(num_cubes, device): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # Non-deterministic when on GPU, so we use different tolerances if device == "cuda:0": @@ -652,10 +652,10 @@ def test_rigid_body_with_static_friction(num_cubes, device): # let everything settle for _ in range(100): sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) cube_mass = cube_object.root_physx_view.get_masses() - gravity_magnitude = abs(sim.cfg.gravity[2]) + gravity_magnitude = abs(sim.cfg.physics_manager_cfg.gravity[2]) # 2 cases: force applied is below and above mu # below mu: block should not move as the force applied is <= mu # above mu: block should move as the force applied is > mu @@ -682,7 +682,7 @@ def test_rigid_body_with_static_friction(num_cubes, device): cube_object.write_data_to_sim() sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) if force == "below_mu": # Assert that the block has not moved torch.testing.assert_close(cube_object.data.root_pos_w, initial_root_pos, rtol=2e-3, atol=2e-3) @@ -750,7 +750,7 @@ def test_rigid_body_with_restitution(num_cubes, device): sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() if expected_collision_type == "inelastic": @@ -804,7 +804,7 @@ def test_rigid_body_set_mass(num_cubes, device): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) masses_to_check = cube_object.root_physx_view.get_masses() @@ -842,7 +842,7 @@ def test_gravity_vec_w(num_cubes, device, gravity_enabled): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # Expected gravity value is the acceleration of the body gravity = torch.zeros(num_cubes, 1, 6, device=device) @@ -895,7 +895,7 @@ def test_body_root_state_properties(num_cubes, device, with_offset): # perform rendering sim.step() # update object - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # get state properties root_state_w = cube_object.data.root_state_w @@ -1001,7 +1001,7 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): # perform step sim.step() # update buffers - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) if state_location == "com": if i % 2 == 0: @@ -1063,7 +1063,7 @@ def test_write_state_functions_data_consistency(num_cubes, device, with_offset, # perform step sim.step() # update buffers - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) if state_location == "com": cube_object.write_root_com_state_to_sim(rand_state) diff --git a/source/isaaclab/test/assets/test_rigid_object_collection.py b/source/isaaclab/test/assets/test_rigid_object_collection.py index 3e75bf6c14a..581c4f66ad5 100644 --- a/source/isaaclab/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab/test/assets/test_rigid_object_collection.py @@ -131,7 +131,7 @@ def test_initialization(sim, num_envs, num_cubes, device): # Simulate physics for _ in range(2): sim.step() - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @@ -193,7 +193,7 @@ def test_initialization_with_kinematic_enabled(sim, num_envs, num_cubes, device) # Simulate physics for _ in range(2): sim.step() - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) # check that the object is kinematic default_object_state = object_collection.data.default_object_state.clone() default_object_state[..., :3] += origins.unsqueeze(1) @@ -268,7 +268,7 @@ def test_external_force_buffer(sim, device): # apply action to the object collection object_collection.write_data_to_sim() sim.step() - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_envs", [1, 2]) @@ -307,7 +307,7 @@ def test_external_force_on_single_body(sim, num_envs, num_cubes, device): # step sim sim.step() # update object collection - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) # First object should still be at the same Z position (1.0) torch.testing.assert_close( @@ -364,7 +364,7 @@ def test_external_force_on_single_body_at_position(sim, num_envs, num_cubes, dev # step sim sim.step() # update object collection - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) # First object should be rotating around it's X axis assert torch.all(object_collection.data.object_ang_vel_b[:, 0::2, 0] > 0.1) @@ -434,7 +434,7 @@ def test_set_object_state(sim, num_envs, num_cubes, device, gravity_enabled): value = getattr(object_collection.data, key) torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("num_envs", [1, 4]) @@ -477,7 +477,7 @@ def test_object_state_properties(sim, num_envs, num_cubes, device, with_offset, # spin the object around Z axis (com) cube_object.write_object_velocity_to_sim(spin_twist.repeat(num_envs, num_cubes, 1)) sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) # get state properties object_state_w = cube_object.data.object_state_w @@ -570,7 +570,7 @@ def test_write_object_state(sim, num_envs, num_cubes, device, with_offset, state object_ids = object_ids.to(device) for i in range(10): sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) if state_location == "com": if i % 2 == 0: @@ -599,7 +599,7 @@ def test_reset_object_collection(sim, num_envs, num_cubes, device): for i in range(5): sim.step() - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) # Move the object to a random position object_state = object_collection.data.default_object_state.clone() @@ -640,7 +640,7 @@ def test_set_material_properties(sim, num_envs, num_cubes, device): # Perform simulation sim.step() - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) # Get material properties materials_to_check = object_collection.root_physx_view.get_material_properties() @@ -670,7 +670,7 @@ def test_gravity_vec_w(sim, num_envs, num_cubes, device, gravity_enabled): # Perform simulation for _ in range(2): sim.step() - object_collection.update(sim.cfg.dt) + object_collection.update(sim.cfg.physics_manager_cfg.dt) # Expected gravity value is the acceleration of the body gravity = torch.zeros(num_envs, num_cubes, 6, device=device) @@ -724,7 +724,7 @@ def test_write_object_state_functions_data_consistency( env_ids = env_ids.to(device) object_ids = object_ids.to(device) sim.step() - cube_object.update(sim.cfg.dt) + cube_object.update(sim.cfg.physics_manager_cfg.dt) object_link_to_com_pos, object_link_to_com_quat = subtract_frame_transforms( cube_object.data.object_link_state_w[..., :3].view(-1, 3), diff --git a/source/isaaclab/test/assets/test_surface_gripper.py b/source/isaaclab/test/assets/test_surface_gripper.py index 86f112fdf98..c13f2fcd513 100644 --- a/source/isaaclab/test/assets/test_surface_gripper.py +++ b/source/isaaclab/test/assets/test_surface_gripper.py @@ -196,8 +196,8 @@ def test_initialization(sim, num_articulations, device, add_ground_plane) -> Non # perform rendering sim.step() # update articulation - articulation.update(sim.cfg.dt) - surface_gripper.update(sim.cfg.dt) + articulation.update(sim.cfg.physics_manager_cfg.dt) + surface_gripper.update(sim.cfg.physics_manager_cfg.dt) @pytest.mark.parametrize("device", ["cuda:0"]) diff --git a/source/isaaclab/test/devices/test_oxr_device.py b/source/isaaclab/test/devices/test_oxr_device.py index 6a9bdd8b0f6..f022016d8ba 100644 --- a/source/isaaclab/test/devices/test_oxr_device.py +++ b/source/isaaclab/test/devices/test_oxr_device.py @@ -78,7 +78,7 @@ def __post_init__(self): """Post initialization.""" self.decimation = 5 self.episode_length_s = 30.0 - self.sim.dt = 0.01 # 100Hz + self.sim.physics_manager_cfg.dt = 0.01 # 100Hz self.sim.render_interval = 2 diff --git a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py index c6169c94d19..5bfeaa89eed 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py +++ b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py @@ -203,11 +203,11 @@ def __post_init__(self): self.decimation = 4 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 0.005 + self.sim.physics_manager_cfg.dt = 0.005 # update sensor update periods # we tick all the sensors based on the smallest update period (physics update period) if self.scene.height_scanner is not None: - self.scene.height_scanner.update_period = self.decimation * self.sim.dt + self.scene.height_scanner.update_period = self.decimation * self.sim.physics_manager_cfg.dt def main(): diff --git a/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py b/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py index fb7622ae67c..b2dfb8081e0 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py +++ b/source/isaaclab/test/envs/check_manager_based_env_floating_cube.py @@ -225,7 +225,7 @@ def __post_init__(self): # general settings self.decimation = 2 # simulation settings - self.sim.dt = 0.01 + self.sim.physics_manager_cfg.dt = 0.01 self.sim.physics_material = self.scene.terrain.physics_material diff --git a/source/isaaclab/test/envs/test_color_randomization.py b/source/isaaclab/test/envs/test_color_randomization.py index 6b4b004eb30..764baf0f2e1 100644 --- a/source/isaaclab/test/envs/test_color_randomization.py +++ b/source/isaaclab/test/envs/test_color_randomization.py @@ -131,7 +131,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz @pytest.mark.parametrize("device", ["cpu", "cuda"]) diff --git a/source/isaaclab/test/envs/test_env_rendering_logic.py b/source/isaaclab/test/envs/test_env_rendering_logic.py index f37ee535273..2eed5d93aaa 100644 --- a/source/isaaclab/test/envs/test_env_rendering_logic.py +++ b/source/isaaclab/test/envs/test_env_rendering_logic.py @@ -193,7 +193,7 @@ def test_env_rendering_logic(env_type, render_interval, physics_callback, render assert num_physics_steps == (i + 1) * env.cfg.decimation, "Physics steps mismatch" # check that we have simulated physics for the correct amount of time physics_time, _ = get_physics_stats() - assert abs(physics_time - num_physics_steps * env.cfg.sim.dt) < 1e-6, "Physics time mismatch" + assert abs(physics_time - num_physics_steps * env.cfg.sim.physics_manager_cfg.dt) < 1e-6, "Physics time mismatch" # check that we have completed the correct number of rendering steps _, num_render_steps = get_render_stats() @@ -201,7 +201,7 @@ def test_env_rendering_logic(env_type, render_interval, physics_callback, render # check that we have rendered for the correct amount of time render_time, _ = get_render_stats() assert ( - abs(render_time - num_render_steps * env.cfg.sim.dt * env.cfg.sim.render_interval) < 1e-6 + abs(render_time - num_render_steps * env.cfg.sim.physics_manager_cfg.dt * env.cfg.sim.render_interval) < 1e-6 ), "Render time mismatch" # close the environment diff --git a/source/isaaclab/test/envs/test_manager_based_env.py b/source/isaaclab/test/envs/test_manager_based_env.py index 7ec9ef2d43f..6244461cb52 100644 --- a/source/isaaclab/test/envs/test_manager_based_env.py +++ b/source/isaaclab/test/envs/test_manager_based_env.py @@ -77,7 +77,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz self.sim.render_interval = self.decimation # render every 4 sim steps # pass device down from test self.sim.device = device @@ -103,7 +103,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz self.sim.render_interval = self.decimation # render every 4 sim steps # pass device down from test self.sim.device = device diff --git a/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py b/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py index f35c11a1c40..9b7e7030dbc 100644 --- a/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py +++ b/source/isaaclab/test/envs/test_manager_based_rl_env_ui.py @@ -64,7 +64,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz self.sim.render_interval = self.decimation # render every 4 sim steps # pass device down from test self.sim.device = device diff --git a/source/isaaclab/test/envs/test_scale_randomization.py b/source/isaaclab/test/envs/test_scale_randomization.py index 42db1edb7fa..7e8c141d906 100644 --- a/source/isaaclab/test/envs/test_scale_randomization.py +++ b/source/isaaclab/test/envs/test_scale_randomization.py @@ -274,7 +274,7 @@ def __post_init__(self): # general settings self.decimation = 2 # simulation settings - self.sim.dt = 0.01 + self.sim.physics_manager_cfg.dt = 0.01 self.sim.physics_material = self.scene.terrain.physics_material self.sim.render_interval = self.decimation diff --git a/source/isaaclab/test/envs/test_texture_randomization.py b/source/isaaclab/test/envs/test_texture_randomization.py index d2e4903fae5..179b331fc3d 100644 --- a/source/isaaclab/test/envs/test_texture_randomization.py +++ b/source/isaaclab/test/envs/test_texture_randomization.py @@ -177,7 +177,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz @pytest.mark.parametrize("device", ["cpu", "cuda"]) diff --git a/source/isaaclab/test/managers/test_recorder_manager.py b/source/isaaclab/test/managers/test_recorder_manager.py index 1ba17f65b87..fab99ed02e2 100644 --- a/source/isaaclab/test/managers/test_recorder_manager.py +++ b/source/isaaclab/test/managers/test_recorder_manager.py @@ -118,7 +118,7 @@ def __post_init__(self): # step settings self.decimation = 4 # env step every 4 sim steps: 200Hz / 4 = 50Hz # simulation settings - self.sim.dt = 0.005 # sim step every 5ms: 200Hz + self.sim.physics_manager_cfg.dt = 0.005 # sim step every 5ms: 200Hz self.sim.render_interval = self.decimation # render every 4 sim steps # pass device down from test self.sim.device = device diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index 7b8d59b998f..a8b9a02ce37 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -126,7 +126,7 @@ def test_camera_init(setup_sim_camera): # perform rendering sim.step() # update camera - camera.update(sim.cfg.dt) + camera.update(sim.cfg.physics_manager_cfg.dt) # check image data for im_data in camera.data.output.values(): assert im_data.shape == (1, camera_cfg.height, camera_cfg.width, 1) diff --git a/source/isaaclab/test/sim/test_build_simulation_context_headless.py b/source/isaaclab/test/sim/test_build_simulation_context_headless.py index e592a7abea6..a52dd0ae5f7 100644 --- a/source/isaaclab/test/sim/test_build_simulation_context_headless.py +++ b/source/isaaclab/test/sim/test_build_simulation_context_headless.py @@ -34,12 +34,12 @@ def test_build_simulation_context_no_cfg(gravity_enabled, device, dt): """Test that the simulation context is built when no simulation cfg is passed in.""" with build_simulation_context(gravity_enabled=gravity_enabled, device=device, dt=dt) as sim: if gravity_enabled: - assert sim.cfg.gravity == (0.0, 0.0, -9.81) + assert sim.cfg.physics_manager_cfg.gravity == (0.0, 0.0, -9.81) else: - assert sim.cfg.gravity == (0.0, 0.0, 0.0) + assert sim.cfg.physics_manager_cfg.gravity == (0.0, 0.0, 0.0) assert sim.cfg.device == device - assert sim.cfg.dt == dt + assert sim.cfg.physics_manager_cfg.dt == dt # Ensure that dome light didn't get added automatically as we are headless assert not sim.stage.GetPrimAtPath("/World/defaultDomeLight").IsValid() @@ -86,6 +86,6 @@ def test_build_simulation_context_cfg(): ) with build_simulation_context(sim_cfg=cfg, gravity_enabled=False, dt=0.01, device="cpu") as sim: - assert sim.cfg.gravity == gravity + assert sim.cfg.physics_manager_cfg.gravity == gravity assert sim.cfg.device == device - assert sim.cfg.dt == dt + assert sim.cfg.physics_manager_cfg.dt == dt diff --git a/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py b/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py index 08107020683..1e19f526850 100644 --- a/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py +++ b/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py @@ -32,12 +32,12 @@ def test_build_simulation_context_no_cfg(gravity_enabled, device, dt): """Test that the simulation context is built when no simulation cfg is passed in.""" with build_simulation_context(gravity_enabled=gravity_enabled, device=device, dt=dt) as sim: if gravity_enabled: - assert sim.cfg.gravity == (0.0, 0.0, -9.81) + assert sim.cfg.physics_manager_cfg.gravity == (0.0, 0.0, -9.81) else: - assert sim.cfg.gravity == (0.0, 0.0, 0.0) + assert sim.cfg.physics_manager_cfg.gravity == (0.0, 0.0, 0.0) assert sim.cfg.device == device - assert sim.cfg.dt == dt + assert sim.cfg.physics_manager_cfg.dt == dt @pytest.mark.parametrize("add_ground_plane", [True, False]) @@ -78,6 +78,6 @@ def test_build_simulation_context_cfg(): ) with build_simulation_context(sim_cfg=cfg, gravity_enabled=False, dt=0.01, device="cpu") as sim: - assert sim.cfg.gravity == gravity + assert sim.cfg.physics_manager_cfg.gravity == gravity assert sim.cfg.device == device - assert sim.cfg.dt == dt + assert sim.cfg.physics_manager_cfg.dt == dt diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 82c9e2b0157..420fff13138 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -335,7 +335,7 @@ def test_clear_stage(): 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() + assert sim.stage.GetPrimAtPath(sim.cfg.physics_manager_cfg.physics_prim_path).IsValid() """ diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py index 052039480a4..e40820054d5 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py @@ -130,7 +130,7 @@ def __post_init__(self): self.decimation = 4 self.episode_length_s = 100.0 # simulation settings - self.sim.dt = 1 / 200 # 200Hz + self.sim.physics_manager_cfg.dt = 1 / 200 # 200Hz self.sim.render_interval = 6 # Set the URDF and mesh paths for the IK controller diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py index cf6234b6354..26b438928b6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py @@ -822,7 +822,7 @@ def randomize_held_initial_state(self, env_ids, pre_grasp): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view + physics_sim_view = sim_utils.SimulationContext.instance()._physics_interface.physics_sim_view physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) self.randomize_fixed_initial_state(env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py index 19bd89e26da..ec7cf29d70c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py @@ -689,7 +689,7 @@ def close_gripper(self, env_ids): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view + physics_sim_view = sim_utils.SimulationContext.instance()._physics_interface.physics_sim_view physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) self.randomize_fixed_initial_state(env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index 960232218cb..2356553c27f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -614,7 +614,7 @@ def step_sim_no_action(self): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance()._physics_backend.physics_sim_view + physics_sim_view = sim_utils.SimulationContext.instance()._physics_interface.physics_sim_view physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) # (1.) Randomize fixed asset pose. diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py index 8b87e1bdb25..8a096a2a41a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py @@ -189,7 +189,7 @@ def get_env_local_pose(env_pos: torch.Tensor, xformable: UsdGeom.Xformable, devi return torch.tensor([px, py, pz, qw, qx, qy, qz], device=device) - self.dt = self.cfg.sim.dt * self.cfg.decimation + self.dt = self.cfg.sim.physics_manager_cfg.dt * self.cfg.decimation # create auxiliary variables for computing applied action, observations and rewards self.robot_dof_lower_limits = self._robot.data.soft_joint_pos_limits[0, :, 0].to(device=self.device) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py index faac10e1a71..895bc312d1f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py @@ -102,7 +102,7 @@ def _compute_intermediate_values(self): self.basis_vec1, self.potentials, self.prev_potentials, - self.cfg.sim.dt, + self.cfg.sim.physics_manager_cfg.dt, ) def _get_observations(self) -> dict: @@ -169,7 +169,7 @@ def _reset_idx(self, env_ids: torch.Tensor | None): to_target = self.targets[env_ids] - default_root_state[:, :3] to_target[:, 2] = 0.0 - self.potentials[env_ids] = -torch.norm(to_target, p=2, dim=-1) / self.cfg.sim.dt + self.potentials[env_ids] = -torch.norm(to_target, p=2, dim=-1) / self.cfg.sim.physics_manager_cfg.dt self._compute_intermediate_values() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py index b3a6b5b9d25..3bebee57236 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/quadcopter_env.py @@ -125,7 +125,7 @@ def __init__(self, cfg: QuadcopterEnvCfg, render_mode: str | None = None, **kwar # Get specific body indices self._body_id = self._robot.find_bodies("body")[0] self._robot_mass = self._robot.root_physx_view.get_masses()[0].sum() - self._gravity_magnitude = torch.tensor(self.sim.cfg.gravity, device=self.device).norm() + self._gravity_magnitude = torch.tensor(self.sim.cfg.physics_manager_cfg.gravity, device=self.device).norm() self._robot_weight = (self._robot_mass * self._gravity_magnitude).item() # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py index 9c1f411490e..0b3980b2815 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py @@ -175,7 +175,7 @@ def __post_init__(self): self.decimation = 2 self.episode_length_s = 16.0 # simulation settings - self.sim.dt = 1 / 120.0 + self.sim.physics_manager_cfg.dt = 1 / 120.0 self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 # default friction material diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py index 788920af058..1e72a7d6056 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py @@ -177,5 +177,5 @@ def __post_init__(self) -> None: # viewer settings self.viewer.eye = (8.0, 0.0, 5.0) # simulation settings - self.sim.dt = 1 / 120 + self.sim.physics_manager_cfg.dt = 1 / 120 self.sim.render_interval = self.decimation diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py index b8e1db8c0a7..474f5449735 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py @@ -212,7 +212,7 @@ def __post_init__(self): self.decimation = 2 self.episode_length_s = 16.0 # simulation settings - self.sim.dt = 1 / 120.0 + self.sim.physics_manager_cfg.dt = 1 / 120.0 self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 # default friction material diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py index 2c896bc604f..4f08cd00743 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py @@ -187,7 +187,7 @@ def __post_init__(self): self.decimation = 4 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 200 # 200Hz + self.sim.physics_manager_cfg.dt = 1 / 200 # 200Hz self.sim.render_interval = 2 # Set the URDF and mesh paths for the IK controller diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py index e74baaf0d0d..739d99480dc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py @@ -204,7 +204,7 @@ def __post_init__(self): self.decimation = 4 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 200 # 200Hz + self.sim.physics_manager_cfg.dt = 1 / 200 # 200Hz self.sim.render_interval = 2 # Set the URDF and mesh paths for the IK controller diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py index a431b32657c..c8809563992 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py @@ -218,14 +218,14 @@ class DigitRoughEnvCfg(LocomotionVelocityRoughEnvCfg): def __post_init__(self): super().__post_init__() self.decimation = 4 - self.sim.dt = 0.005 + self.sim.physics_manager_cfg.dt = 0.005 # Scene self.scene.robot = DIGIT_V4_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_base" self.scene.contact_forces.history_length = self.decimation - self.scene.contact_forces.update_period = self.sim.dt - self.scene.height_scanner.update_period = self.decimation * self.sim.dt + self.scene.contact_forces.update_period = self.sim.physics_manager_cfg.dt + self.scene.height_scanner.update_period = self.decimation * self.sim.physics_manager_cfg.dt # Events: self.events.add_base_mass.params["asset_cfg"] = SceneEntityCfg("robot", body_names="torso_base") diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py index 23b853691a0..272e34f1563 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py @@ -317,7 +317,7 @@ def __post_init__(self): self.decimation = 10 # 50 Hz self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 0.002 # 500 Hz + self.sim.physics_manager_cfg.dt = 0.002 # 500 Hz self.sim.render_interval = self.decimation self.sim.physics_material.static_friction = 1.0 self.sim.physics_material.dynamic_friction = 1.0 @@ -325,7 +325,7 @@ def __post_init__(self): self.sim.physics_material.restitution_combine_mode = "multiply" # update sensor update periods # we tick all the sensors based on the smallest update period (physics update period) - self.scene.contact_forces.update_period = self.sim.dt + self.scene.contact_forces.update_period = self.sim.physics_manager_cfg.dt # switch robot to Spot-d self.scene.robot = SPOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index b76009415af..cc551c8e547 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -308,16 +308,16 @@ def __post_init__(self): self.decimation = 4 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 0.005 + self.sim.physics_manager_cfg.dt = 0.005 self.sim.render_interval = self.decimation self.sim.physics_material = self.scene.terrain.physics_material self.sim.physics_manager_cfg.gpu_max_rigid_patch_count = 10 * 2**15 # update sensor update periods # we tick all the sensors based on the smallest update period (physics update period) if self.scene.height_scanner is not None: - self.scene.height_scanner.update_period = self.decimation * self.sim.dt + self.scene.height_scanner.update_period = self.decimation * self.sim.physics_manager_cfg.dt if self.scene.contact_forces is not None: - self.scene.contact_forces.update_period = self.sim.dt + self.scene.contact_forces.update_period = self.sim.physics_manager_cfg.dt # check if terrain levels curriculum is enabled - if so, enable curriculum for terrain generator # this generates terrains with increasing difficulty and is useful for training diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index 49ec7abdbe2..b29bd75285a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -271,7 +271,7 @@ def __post_init__(self): self.viewer.eye = (-2.0, 2.0, 2.0) self.viewer.lookat = (0.8, 0.0, 0.5) # simulation settings - self.sim.dt = 1 / 60 # 60Hz + self.sim.physics_manager_cfg.dt = 1 / 60 # 60Hz self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py index ede3e8a0ad7..6c3b5b7247a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/cabinet_openarm_env_cfg.py @@ -275,7 +275,7 @@ def __post_init__(self): self.viewer.eye = (-2.0, 2.0, 2.0) self.viewer.lookat = (0.8, 0.0, 0.5) # simulation settings - self.sim.dt = 1 / 60 # 60Hz + self.sim.physics_manager_cfg.dt = 1 / 60 # 60Hz self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 self.sim.physics_manager_cfg.friction_correlation_distance = 0.00625 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py index 90b65a0f96c..ec7e7baab92 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/reach_env_cfg.py @@ -212,4 +212,4 @@ def __post_init__(self): self.episode_length_s = 12.0 self.viewer.eye = (3.5, 3.5, 3.5) # simulation settings - self.sim.dt = 1.0 / 120.0 + self.sim.physics_manager_cfg.dt = 1.0 / 120.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py index f11fb35ad76..0aa4c458419 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py @@ -422,7 +422,7 @@ def __post_init__(self): self.is_finite_horizon = True # simulation settings - self.sim.dt = 1 / 120 + self.sim.physics_manager_cfg.dt = 1 / 120 self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py index 71594ae210d..8f5fd4b6d6c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py @@ -340,7 +340,7 @@ def __post_init__(self): self.decimation = 4 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1.0 / 120.0 + self.sim.physics_manager_cfg.dt = 1.0 / 120.0 self.sim.render_interval = self.decimation # change viewer settings self.viewer.eye = (2.0, 2.0, 2.0) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py index af41481270e..94952d3ef3d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/lift_openarm_env_cfg.py @@ -230,7 +230,7 @@ def __post_init__(self): self.decimation = 2 self.episode_length_s = 5.0 # simulation settings - self.sim.dt = 0.01 # 100Hz + self.sim.physics_manager_cfg.dt = 0.01 # 100Hz self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.01 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py index 2416420f835..f2b2eaf9468 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py @@ -212,7 +212,7 @@ def __post_init__(self): self.decimation = 2 self.episode_length_s = 5.0 # simulation settings - self.sim.dt = 0.01 # 100Hz + self.sim.physics_manager_cfg.dt = 0.01 # 100Hz self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index 1aceb299621..90a37e299ea 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -320,7 +320,7 @@ def __post_init__(self): self.decimation = 5 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 100 + self.sim.physics_manager_cfg.dt = 1 / 100 self.sim.render_interval = 2 # Set settings for camera rendering diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index abac9493a90..aa613d6e94a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -355,7 +355,7 @@ def __post_init__(self): self.decimation = 5 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 100 + self.sim.physics_manager_cfg.dt = 1 / 100 self.sim.render_interval = 2 # Set settings for camera rendering diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index 169fc08cb42..35783824aca 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -373,7 +373,7 @@ def __post_init__(self): self.decimation = 6 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 120 # 120Hz + self.sim.physics_manager_cfg.dt = 1 / 120 # 120Hz self.sim.render_interval = 2 # Convert USD to URDF and change revolute joints to fixed diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py index 23ed8d984bc..90302e57403 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -51,7 +51,7 @@ def __post_init__(self): self.decimation = 6 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 120 # 120Hz + self.sim.physics_manager_cfg.dt = 1 / 120 # 120Hz self.sim.render_interval = 2 # Add waist joint to pink_ik_cfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py index 8d67478cc5e..4797b4043e1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py @@ -365,7 +365,7 @@ def __post_init__(self): self.decimation = 6 self.episode_length_s = 20.0 # simulation settings - self.sim.dt = 1 / 120 # 120Hz + self.sim.physics_manager_cfg.dt = 1 / 120 # 120Hz self.sim.render_interval = 2 # Convert USD to URDF and change revolute joints to fixed diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py index cc8bed7510a..a048f1e789a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -339,7 +339,7 @@ def __post_init__(self): ) # Set the simulation parameters - self.sim.dt = 1 / 60 + self.sim.physics_manager_cfg.dt = 1 / 60 self.sim.render_interval = 6 self.decimation = 3 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py index 2cad32da6b1..da0b984fb17 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py @@ -275,7 +275,7 @@ def __post_init__(self): ) # Set the simulation parameters - self.sim.dt = 1 / 60 + self.sim.physics_manager_cfg.dt = 1 / 60 self.sim.render_interval = 6 self.decimation = 3 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py index 7ccdfa0f851..1a00aa98e56 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/reach_openarm_bi_env_cfg.py @@ -332,4 +332,4 @@ def __post_init__(self): self.episode_length_s = 24.0 self.viewer.eye = (3.5, 3.5, 3.5) # simulation settings - self.sim.dt = 1.0 / 60.0 + self.sim.physics_manager_cfg.dt = 1.0 / 60.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py index ed9bcbfc08b..907b41b9e69 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/reach_openarm_uni_env_cfg.py @@ -245,4 +245,4 @@ def __post_init__(self): self.episode_length_s = 12.0 self.viewer.eye = (3.5, 3.5, 3.5) # simulation settings - self.sim.dt = 1.0 / 60.0 + self.sim.physics_manager_cfg.dt = 1.0 / 60.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py index bad88b401c7..2410a2ef95c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py @@ -209,7 +209,7 @@ def __post_init__(self): self.episode_length_s = 12.0 self.viewer.eye = (3.5, 3.5, 3.5) # simulation settings - self.sim.dt = 1.0 / 60.0 + self.sim.physics_manager_cfg.dt = 1.0 / 60.0 self.teleop_devices = DevicesCfg( devices={ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py index ea349b6a618..782fba534d3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py @@ -59,7 +59,7 @@ def __post_init__(self): ) # Set the simulation parameters - self.sim.dt = 1 / 60 + self.sim.physics_manager_cfg.dt = 1 / 60 self.sim.render_interval = 6 self.decimation = 3 @@ -126,7 +126,7 @@ def __post_init__(self): use_relative_mode=self.use_relative_mode, ) # Set the simulation parameters - self.sim.dt = 1 / 120 + self.sim.physics_manager_cfg.dt = 1 / 120 self.sim.render_interval = 6 self.decimation = 6 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py index 976f14f6802..e6782d6caa4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py @@ -129,7 +129,7 @@ def __post_init__(self): self.decimation = 5 self.episode_length_s = 30.0 # simulation settings - self.sim.dt = 0.01 # 100Hz + self.sim.physics_manager_cfg.dt = 0.01 # 100Hz self.sim.render_interval = 5 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py index aadd440688b..14c45a5535b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py @@ -189,7 +189,7 @@ def __post_init__(self): self.decimation = 5 self.episode_length_s = 30.0 # simulation settings - self.sim.dt = 0.01 # 100Hz + self.sim.physics_manager_cfg.dt = 0.01 # 100Hz self.sim.render_interval = 2 self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py index 3afdff9037f..252cba9cda0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py @@ -125,7 +125,7 @@ def __post_init__(self): self.decimation = 5 self.episode_length_s = 30.0 # simulation settings - self.sim.dt = 0.01 # 100Hz + self.sim.physics_manager_cfg.dt = 0.01 # 100Hz self.sim.render_interval = self.decimation self.sim.physics_manager_cfg.bounce_threshold_velocity = 0.2 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py index 96b60705bb5..26a69a3b2d5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py @@ -135,17 +135,17 @@ class NavigationEnvCfg(ManagerBasedRLEnvCfg): def __post_init__(self): """Post initialization.""" - self.sim.dt = LOW_LEVEL_ENV_CFG.sim.dt + self.sim.physics_manager_cfg.dt = LOW_LEVEL_ENV_CFG.sim.physics_manager_cfg.dt self.sim.render_interval = LOW_LEVEL_ENV_CFG.decimation self.decimation = LOW_LEVEL_ENV_CFG.decimation * 10 self.episode_length_s = self.commands.pose_command.resampling_time_range[1] if self.scene.height_scanner is not None: self.scene.height_scanner.update_period = ( - self.actions.pre_trained_policy_action.low_level_decimation * self.sim.dt + self.actions.pre_trained_policy_action.low_level_decimation * self.sim.physics_manager_cfg.dt ) if self.scene.contact_forces is not None: - self.scene.contact_forces.update_period = self.sim.dt + self.scene.contact_forces.update_period = self.sim.physics_manager_cfg.dt class NavigationEnvCfg_PLAY(NavigationEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py index 57675039d70..d8e1fc47949 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py @@ -148,7 +148,7 @@ def parse_env_cfg( cfg.sim.device = device # disable fabric to read/write through USD if use_fabric is not None: - cfg.sim.use_fabric = use_fabric + cfg.sim.physics_manager_cfg.use_fabric = use_fabric # number of environments if num_envs is not None: cfg.scene.num_envs = num_envs From 4b1d3ae9d17ebdde681e4f09cba1cf8885f7efc4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 07:13:05 -0800 Subject: [PATCH 123/130] refactor step 18: refactored newton manager --- source/isaaclab/isaaclab/physics/__init__.py | 9 +- .../isaaclab/physics/newton_backend.py | 66 --- .../isaaclab/physics/newton_manager.py | 402 +++++++++++------- .../isaaclab/physics/newton_manager_cfg.py | 202 ++++++++- .../isaaclab/isaaclab/physics/solvers_cfg.py | 177 -------- 5 files changed, 463 insertions(+), 393 deletions(-) delete mode 100644 source/isaaclab/isaaclab/physics/newton_backend.py delete mode 100644 source/isaaclab/isaaclab/physics/solvers_cfg.py diff --git a/source/isaaclab/isaaclab/physics/__init__.py b/source/isaaclab/isaaclab/physics/__init__.py index 4af76ff4d3a..ad93e53f69d 100644 --- a/source/isaaclab/isaaclab/physics/__init__.py +++ b/source/isaaclab/isaaclab/physics/__init__.py @@ -5,17 +5,20 @@ """Implementation backends for simulation interfaces.""" -# from .newton_backend import NewtonBackend from .physics_manager import PhysicsManager from .physics_manager_cfg import PhysicsManagerCfg from .physx_manager import PhysxManager, IsaacEvents from .physx_manager_cfg import PhysxManagerCfg +# from .newton_manager import NewtonManager +# from .newton_manager_cfg import NewtonManagerCfg + __all__ = [ - # "NewtonBackend", "PhysicsManager", "PhysicsManagerCfg", "PhysxManager", - "PhysxManagerCfg", "IsaacEvents", + "PhysxManagerCfg", + # "NewtonManager", + # "NewtonManagerCfg", ] diff --git a/source/isaaclab/isaaclab/physics/newton_backend.py b/source/isaaclab/isaaclab/physics/newton_backend.py deleted file mode 100644 index 3b5538b6012..00000000000 --- a/source/isaaclab/isaaclab/physics/newton_backend.py +++ /dev/null @@ -1,66 +0,0 @@ -# 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 - -"""Newton physics backend implementation.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from .newton_manager import NewtonManager -from .physics_backend import PhysicsBackend - -if TYPE_CHECKING: - from isaaclab.sim.simulation_context import SimulationContext - - -class NewtonBackend(PhysicsBackend): - """Newton physics backend wrapping NewtonManager.""" - - def __init__(self, sim_context: "SimulationContext"): - """Initialize and configure Newton backend. - - Args: - sim_context: Parent simulation context. - """ - super().__init__(sim_context) - cfg = self._sim.cfg - - # Set simulation parameters - NewtonManager.set_simulation_dt(cfg.dt) - NewtonManager._gravity_vector = cfg.gravity - - # Extract and apply solver settings from config - to_dict = getattr(cfg, "to_dict", None) - params = to_dict() if callable(to_dict) else {} - newton_cfg = dict(params.get("newton_cfg", {})) if isinstance(params, dict) else {} - NewtonManager.set_solver_settings(newton_cfg) - - # USD fabric sync only needed for OV rendering - NewtonManager._clone_physics_only = "omniverse" not in self._sim._visualizer_interface._visualizers_str - - def reset(self, soft: bool = False) -> None: - """Reset physics simulation. - - Args: - soft: If True, skip full reinitialization. - """ - if not soft: - NewtonManager.start_simulation() - NewtonManager.initialize_solver() - - def forward(self) -> None: - """Update articulation kinematics without stepping physics.""" - NewtonManager.forward_kinematics() - - def step(self) -> None: - """Step physics simulation.""" - if self._sim.is_playing(): - NewtonManager.step() - - def close(self) -> None: - """Clean up Newton physics resources.""" - NewtonManager.clear() - self._initialized = False diff --git a/source/isaaclab/isaaclab/physics/newton_manager.py b/source/isaaclab/isaaclab/physics/newton_manager.py index 83496da575e..bfd8a04d21a 100644 --- a/source/isaaclab/isaaclab/physics/newton_manager.py +++ b/source/isaaclab/isaaclab/physics/newton_manager.py @@ -3,10 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause +"""Newton physics manager for Isaac Lab.""" + from __future__ import annotations import numpy as np import re +from typing import TYPE_CHECKING import warp as wp from newton import Axis, Contacts, Control, Model, ModelBuilder, State, eval_fk @@ -15,10 +18,16 @@ from newton.sensors import populate_contacts from newton.solvers import SolverBase, SolverFeatherstone, SolverMuJoCo, SolverNotifyFlags, SolverXPBD -from isaaclab.physics.newton_manager_cfg import NewtonCfg from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils.timer import Timer +from .physics_manager import PhysicsManager + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext + from .newton_manager_cfg import NewtonManagerCfg + from .solvers_cfg import NewtonSolverCfg + def flipped_match(x: str, y: str) -> re.Match | None: """Flipped match function. @@ -35,7 +44,22 @@ def flipped_match(x: str, y: str) -> re.Match | None: return re.match(y, x) -class NewtonManager: +class NewtonManager(PhysicsManager): + """Newton physics manager for Isaac Lab. + + This is a class-level (singleton-like) manager for the Newton simulation. + It handles solver configuration, physics stepping, and reset. + + Lifecycle: initialize() -> reset() -> step() (repeated) -> close() + """ + + # Simulation context reference + _sim: "SimulationContext | None" = None + + # Manager configuration + _cfg: "NewtonManagerCfg | None" = None + + # Newton-specific state _builder: ModelBuilder = None _model: Model = None _device: str = "cuda:0" @@ -60,57 +84,127 @@ class NewtonManager: _usdrt_stage = None _newton_index_attr = "newton:index" _clone_physics_only = False - _cfg: NewtonCfg = NewtonCfg() _solver_type: str = "mujoco_warp" _gravity_vector: tuple[float, float, float] = (0.0, 0.0, -9.81) _up_axis: str = "Z" - _num_envs: int = None + _num_envs: int | None = None _model_changes: set[int] = set() + # ------------------------------------------------------------------ + # PhysicsManager Interface + # ------------------------------------------------------------------ + + @classmethod + def initialize(cls, sim_context: "SimulationContext") -> None: + """Initialize the manager with simulation context. + + Args: + sim_context: Parent simulation context. + """ + cls._sim = sim_context + cls._cfg = sim_context.cfg.physics_manager_cfg # type: ignore[assignment] + + # Set simulation parameters from config + cls._dt = cls._cfg.dt + cls._device = cls._cfg.device + cls._gravity_vector = cls._cfg.gravity + + # USD fabric sync only needed for OV rendering + if cls._sim is not None: + cls._clone_physics_only = "omniverse" not in cls._sim._visualizer_interface._visualizers_str # type: ignore[union-attr] + + @classmethod + def reset(cls, soft: bool = False) -> None: + """Reset physics simulation. + + Args: + soft: If True, skip full reinitialization. + """ + if not soft: + cls.start_simulation() + cls.initialize_solver() + + @classmethod + def forward(cls) -> None: + """Update articulation kinematics without stepping physics.""" + cls.forward_kinematics() + + @classmethod + def step(cls) -> None: + """Step the physics simulation.""" + if cls._sim is not None and cls._sim.is_playing(): + cls._step_internal() + + @classmethod + def close(cls) -> None: + """Clean up Newton physics resources.""" + cls.clear() + cls._sim = None + cls._cfg = None + + @classmethod + def get_physics_dt(cls) -> float: + """Get the physics timestep in seconds.""" + return cls._dt + + @classmethod + def get_device(cls) -> str: + """Get the physics simulation device.""" + return cls._device + + @classmethod + def get_physics_sim_view(cls): + """Get the physics simulation view (not applicable for Newton).""" + return None + + @classmethod + def is_fabric_enabled(cls) -> bool: + """Check if fabric interface is enabled (not applicable for Newton).""" + return False + + # ------------------------------------------------------------------ + # Newton-specific API + # ------------------------------------------------------------------ + @classmethod def clear(cls): - NewtonManager._builder = None - NewtonManager._model = None - NewtonManager._solver = None - NewtonManager._state_0 = None - NewtonManager._state_1 = None - NewtonManager._state_temp = None - NewtonManager._control = None - NewtonManager._contacts = None - NewtonManager._needs_collision_pipeline = False - NewtonManager._collision_pipeline = None - NewtonManager._newton_contact_sensor = None - NewtonManager._report_contacts = False - NewtonManager._graph = None - NewtonManager._newton_stage_path = None - NewtonManager._sim_time = 0.0 - NewtonManager._on_init_callbacks = [] - NewtonManager._on_start_callbacks = [] - NewtonManager._usdrt_stage = None - # Only create new config if not during Python shutdown - try: - NewtonManager._cfg = NewtonCfg() - except (ImportError, AttributeError, TypeError): - NewtonManager._cfg = None - NewtonManager._up_axis = "Z" - NewtonManager._first_call = True - NewtonManager._model_changes = set() + """Clear all state.""" + cls._builder = None + cls._model = None + cls._solver = None + cls._state_0 = None + cls._state_1 = None + cls._state_temp = None + cls._control = None + cls._contacts = None + cls._needs_collision_pipeline = False + cls._collision_pipeline = None + cls._newton_contact_sensor = None + cls._report_contacts = False + cls._graph = None + cls._newton_stage_path = None + cls._sim_time = 0.0 + cls._on_init_callbacks = [] + cls._on_start_callbacks = [] + cls._usdrt_stage = None + cls._up_axis = "Z" + cls._model_changes = set() @classmethod def set_builder(cls, builder): - NewtonManager._builder = builder + cls._builder = builder @classmethod def add_on_init_callback(cls, callback) -> None: - NewtonManager._on_init_callbacks.append(callback) + cls._on_init_callbacks.append(callback) @classmethod def add_on_start_callback(cls, callback) -> None: - NewtonManager._on_start_callbacks.append(callback) + cls._on_start_callbacks.append(callback) @classmethod def add_model_change(cls, change: SolverNotifyFlags) -> None: - NewtonManager._model_changes.add(change) + cls._model_changes.add(change) @classmethod def start_simulation(cls) -> None: @@ -119,41 +213,41 @@ def start_simulation(cls) -> None: This function finalizes the model and initializes the simulation state. """ - print(f"[INFO] Builder: {NewtonManager._builder}") - if NewtonManager._builder is None: - NewtonManager.instantiate_builder_from_stage() + print(f"[INFO] Builder: {cls._builder}") + if cls._builder is None: + cls.instantiate_builder_from_stage() print("[INFO] Running on init callbacks") - for callback in NewtonManager._on_init_callbacks: + for callback in cls._on_init_callbacks: callback() - print(f"[INFO] Finalizing model on device: {NewtonManager._device}") - NewtonManager._builder.gravity = np.array(NewtonManager._gravity_vector)[-1] - NewtonManager._builder.up_axis = Axis.from_string(NewtonManager._up_axis) + print(f"[INFO] Finalizing model on device: {cls._device}") + cls._builder.gravity = np.array(cls._gravity_vector)[-1] + cls._builder.up_axis = Axis.from_string(cls._up_axis) with Timer(name="newton_finalize_builder", msg="Finalize builder took:", enable=True, format="ms"): - NewtonManager._model = NewtonManager._builder.finalize(device=NewtonManager._device) - NewtonManager._model.num_envs = NewtonManager._num_envs - NewtonManager._state_0 = NewtonManager._model.state() - NewtonManager._state_1 = NewtonManager._model.state() - NewtonManager._state_temp = NewtonManager._model.state() - NewtonManager._control = NewtonManager._model.control() - NewtonManager.forward_kinematics() - if NewtonManager._needs_collision_pipeline: - NewtonManager._collision_pipeline = create_collision_pipeline(NewtonManager._model) - NewtonManager._contacts = NewtonManager._model.collide( - NewtonManager._state_0, collision_pipeline=NewtonManager._collision_pipeline + cls._model = cls._builder.finalize(device=cls._device) + cls._model.num_envs = cls._num_envs + cls._state_0 = cls._model.state() + cls._state_1 = cls._model.state() + cls._state_temp = cls._model.state() + cls._control = cls._model.control() + cls.forward_kinematics() + if cls._needs_collision_pipeline: + cls._collision_pipeline = create_collision_pipeline(cls._model) + cls._contacts = cls._model.collide( + cls._state_0, collision_pipeline=cls._collision_pipeline ) else: - NewtonManager._contacts = Contacts(0, 0) + cls._contacts = Contacts(0, 0) print("[INFO] Running on start callbacks") - for callback in NewtonManager._on_start_callbacks: + for callback in cls._on_start_callbacks: callback() - if not NewtonManager._clone_physics_only: + if not cls._clone_physics_only: import usdrt - NewtonManager._usdrt_stage = get_current_stage(fabric=True) - for i, prim_path in enumerate(NewtonManager._model.body_key): - prim = NewtonManager._usdrt_stage.GetPrimAtPath(prim_path) - prim.CreateAttribute(NewtonManager._newton_index_attr, usdrt.Sdf.ValueTypeNames.UInt, True) - prim.GetAttribute(NewtonManager._newton_index_attr).Set(i) + cls._usdrt_stage = get_current_stage(fabric=True) + for i, prim_path in enumerate(cls._model.body_key): + prim = cls._usdrt_stage.GetPrimAtPath(prim_path) + prim.CreateAttribute(cls._newton_index_attr, usdrt.Sdf.ValueTypeNames.UInt, True) + prim.GetAttribute(cls._newton_index_attr).Set(i) xformable_prim = usdrt.Rt.Xformable(prim) if not xformable_prim.HasWorldXform(): xformable_prim.SetWorldXformFromUsd() @@ -166,11 +260,16 @@ def instantiate_builder_from_stage(cls): up_axis = UsdGeom.GetStageUpAxis(stage) builder = ModelBuilder(up_axis=up_axis) builder.add_usd(stage) - NewtonManager.set_builder(builder) + cls.set_builder(builder) @classmethod - def set_solver_settings(cls, newton_params: dict): - NewtonManager._cfg = NewtonCfg(**newton_params) + def set_solver_settings(cls, cfg: "NewtonManagerCfg") -> None: + """Set solver settings from config. + + Args: + cfg: Newton manager configuration. + """ + cls._cfg = cfg @classmethod def initialize_solver(cls): @@ -185,27 +284,29 @@ def initialize_solver(cls): simulation once to capture the graph. Hence, this function should only be called after everything else in the simulation is initialized. """ + if cls._cfg is None: + return + with Timer(name="newton_initialize_solver", msg="Initialize solver took:", enable=True, format="ms"): - NewtonManager._num_substeps = NewtonManager._cfg.num_substeps - NewtonManager._solver_dt = NewtonManager._dt / NewtonManager._num_substeps - print(NewtonManager._model.gravity) - NewtonManager._solver = NewtonManager._get_solver(NewtonManager._model, NewtonManager._cfg.solver_cfg) - if isinstance(NewtonManager._solver, SolverMuJoCo): - NewtonManager._needs_collision_pipeline = not NewtonManager._cfg.solver_cfg.get( - "use_mujoco_contacts", False - ) + cls._num_substeps = cls._cfg.num_substeps + cls._solver_dt = cls._dt / cls._num_substeps + print(cls._model.gravity) + cls._solver = cls._get_solver(cls._model, cls._cfg.solver_cfg) + if isinstance(cls._solver, SolverMuJoCo): + use_mujoco_contacts = getattr(cls._cfg.solver_cfg, "use_mujoco_contacts", False) + cls._needs_collision_pipeline = not use_mujoco_contacts else: - NewtonManager._needs_collision_pipeline = True + cls._needs_collision_pipeline = True # Ensure we are using a CUDA enabled device - assert NewtonManager._device.startswith("cuda"), "NewtonManager only supports CUDA enabled devices" + assert cls._device.startswith("cuda"), "NewtonManager only supports CUDA enabled devices" # Capture the graph if CUDA is enabled with Timer(name="newton_cuda_graph", msg="CUDA graph took:", enable=True, format="ms"): - if NewtonManager._cfg.use_cuda_graph: + if cls._cfg.use_cuda_graph: with wp.ScopedCapture() as capture: - NewtonManager.simulate() - NewtonManager._graph = capture.graph + cls.simulate() + cls._graph = capture.graph @classmethod def simulate(cls) -> None: @@ -215,56 +316,56 @@ def simulate(cls) -> None: may need to explicitly compute the collisions. This function also aggregates the contacts and evaluates the contact sensors. Finally, it performs the state swapping for Newton. """ - state_0_dict = NewtonManager._state_0.__dict__ - state_1_dict = NewtonManager._state_1.__dict__ - state_temp_dict = NewtonManager._state_temp.__dict__ + state_0_dict = cls._state_0.__dict__ + state_1_dict = cls._state_1.__dict__ + state_temp_dict = cls._state_temp.__dict__ contacts = None # MJWarp computes its own collisions. - if NewtonManager._needs_collision_pipeline: - contacts = NewtonManager._model.collide( - NewtonManager._state_0, collision_pipeline=NewtonManager._collision_pipeline + if cls._needs_collision_pipeline: + contacts = cls._model.collide( + cls._state_0, collision_pipeline=cls._collision_pipeline ) - if NewtonManager._num_substeps % 2 == 0: - for i in range(NewtonManager._num_substeps): - NewtonManager._solver.step( - NewtonManager._state_0, - NewtonManager._state_1, - NewtonManager._control, + if cls._num_substeps % 2 == 0: + for i in range(cls._num_substeps): + cls._solver.step( + cls._state_0, + cls._state_1, + cls._control, contacts, - NewtonManager._solver_dt, + cls._solver_dt, ) - NewtonManager._state_0, NewtonManager._state_1 = NewtonManager._state_1, NewtonManager._state_0 - NewtonManager._state_0.clear_forces() + cls._state_0, cls._state_1 = cls._state_1, cls._state_0 + cls._state_0.clear_forces() else: - for i in range(NewtonManager._num_substeps): - NewtonManager._solver.step( - NewtonManager._state_0, - NewtonManager._state_1, - NewtonManager._control, + for i in range(cls._num_substeps): + cls._solver.step( + cls._state_0, + cls._state_1, + cls._control, contacts, - NewtonManager._solver_dt, + cls._solver_dt, ) # FIXME: Ask Lukasz help to deal with non-even number of substeps. This should not be needed. - if i < NewtonManager._num_substeps - 1 or not NewtonManager._cfg.use_cuda_graph: + if i < cls._num_substeps - 1 or not cls._cfg.use_cuda_graph: # we can just swap the state references - NewtonManager._state_0, NewtonManager._state_1 = NewtonManager._state_1, NewtonManager._state_0 - elif NewtonManager._cfg.use_cuda_graph: + cls._state_0, cls._state_1 = cls._state_1, cls._state_0 + elif cls._cfg.use_cuda_graph: # swap states by actually copying the state arrays to make sure the graph capture works for key, value in state_0_dict.items(): - if isinstance(value, wp.array): + if isinstance(value, wp.array): # type: ignore[arg-type] if key not in state_temp_dict: state_temp_dict[key] = wp.empty_like(value) state_temp_dict[key].assign(value) state_0_dict[key].assign(state_1_dict[key]) state_1_dict[key].assign(state_temp_dict[key]) - NewtonManager._state_0.clear_forces() + cls._state_0.clear_forces() - if NewtonManager._report_contacts: - populate_contacts(NewtonManager._contacts, NewtonManager._solver) - NewtonManager._newton_contact_sensor.eval(NewtonManager._contacts) + if cls._report_contacts: + populate_contacts(cls._contacts, cls._solver) + cls._newton_contact_sensor.eval(cls._contacts) @classmethod def set_device(cls, device: str) -> None: @@ -273,35 +374,35 @@ def set_device(cls, device: str) -> None: Args: device (str): The device to use for the Newton simulation. """ - NewtonManager._device = device + cls._device = device @classmethod - def step(cls) -> None: - """Steps the simulation. + def _step_internal(cls) -> None: + """Steps the simulation (internal). This function steps the simulation by the specified time step in the simulation configuration. """ - if NewtonManager._model_changes: - for change in NewtonManager._model_changes: - NewtonManager._solver.notify_model_changed(change) - NewtonManager._model_changes = set() + if cls._model_changes: + for change in cls._model_changes: + cls._solver.notify_model_changed(change) + cls._model_changes = set() - if NewtonManager._cfg.use_cuda_graph: - wp.capture_launch(NewtonManager._graph) + if cls._cfg is not None and cls._cfg.use_cuda_graph: + wp.capture_launch(cls._graph) # type: ignore[arg-type] else: - NewtonManager.simulate() + cls.simulate() - if NewtonManager._cfg.debug_mode: - convergence_data = NewtonManager.get_solver_convergence_steps() + if cls._cfg is not None and cls._cfg.debug_mode: + convergence_data = cls.get_solver_convergence_steps() # print(f"solver niter: {convergence_data}") - if convergence_data["max"] == NewtonManager._solver.mjw_model.opt.iterations: + if convergence_data["max"] == cls._solver.mjw_model.opt.iterations: print("solver didn't converge!", convergence_data["max"]) - NewtonManager._sim_time += NewtonManager._solver_dt * NewtonManager._num_substeps + cls._sim_time += cls._solver_dt * cls._num_substeps @classmethod def get_solver_convergence_steps(cls) -> dict[str, float | int]: - niter = NewtonManager._solver.mjw_data.solver_niter.numpy() + niter = cls._solver.mjw_data.solver_niter.numpy() max_niter = np.max(niter) mean_niter = np.mean(niter) min_niter = np.min(niter) @@ -315,31 +416,31 @@ def set_simulation_dt(cls, dt: float) -> None: Args: dt (float): The simulation time step. """ - NewtonManager._dt = dt + cls._dt = dt @classmethod def get_model(cls): - return NewtonManager._model + return cls._model @classmethod def get_state_0(cls): - return NewtonManager._state_0 + return cls._state_0 @classmethod def get_state_1(cls): - return NewtonManager._state_1 + return cls._state_1 @classmethod def get_control(cls): - return NewtonManager._control + return cls._control @classmethod def get_dt(cls): - return NewtonManager._dt + return cls._dt @classmethod def get_solver_dt(cls): - return NewtonManager._solver_dt + return cls._solver_dt @classmethod def forward_kinematics(cls, mask: wp.array | None = None) -> None: @@ -348,25 +449,40 @@ def forward_kinematics(cls, mask: wp.array | None = None) -> None: This function evaluates the forward kinematics for the selected articulations. """ eval_fk( - NewtonManager._model, - NewtonManager._state_0.joint_q, - NewtonManager._state_0.joint_qd, - NewtonManager._state_0, + cls._model, + cls._state_0.joint_q, + cls._state_0.joint_qd, + cls._state_0, None, ) @classmethod - def _get_solver(cls, model: Model, solver_cfg: dict) -> SolverBase: - NewtonManager._solver_type = solver_cfg.pop("solver_type") + def _get_solver(cls, model: Model, solver_cfg: "NewtonSolverCfg") -> SolverBase: + """Create and return the appropriate solver based on config. + + Args: + model: The Newton model. + solver_cfg: Solver configuration. + + Returns: + The initialized solver. + """ + # Convert config to dict if needed (configclass adds to_dict method) + if hasattr(solver_cfg, "to_dict"): + cfg_dict = solver_cfg.to_dict() # type: ignore[union-attr] + else: + cfg_dict = dict(solver_cfg) if isinstance(solver_cfg, dict) else {} # type: ignore[arg-type] + + cls._solver_type = cfg_dict.pop("solver_type", "mujoco_warp") - if NewtonManager._solver_type == "mujoco_warp": - return SolverMuJoCo(model, **solver_cfg) - elif NewtonManager._solver_type == "xpbd": - return SolverXPBD(model, **solver_cfg) - elif NewtonManager._solver_type == "featherstone": - return SolverFeatherstone(model, **solver_cfg) + if cls._solver_type == "mujoco_warp": + return SolverMuJoCo(model, **cfg_dict) + elif cls._solver_type == "xpbd": + return SolverXPBD(model, **cfg_dict) + elif cls._solver_type == "featherstone": + return SolverFeatherstone(model, **cfg_dict) else: - raise ValueError(f"Invalid solver type: {NewtonManager._solver_type}") + raise ValueError(f"Invalid solver type: {cls._solver_type}") @classmethod def add_contact_sensor( @@ -421,8 +537,8 @@ def add_contact_sensor( f"[INFO] Adding contact view for {shape_names_expr} with filter {contact_partners_shape_expr}." ) - NewtonManager._newton_contact_sensor = NewtonContactSensor( - NewtonManager._model, + cls._newton_contact_sensor = NewtonContactSensor( + cls._model, sensing_obj_bodies=body_names_expr, sensing_obj_shapes=shape_names_expr, counterpart_bodies=contact_partners_body_expr, @@ -432,4 +548,4 @@ def add_contact_sensor( prune_noncolliding=prune_noncolliding, verbose=verbose, ) - NewtonManager._report_contacts = True + cls._report_contacts = True diff --git a/source/isaaclab/isaaclab/physics/newton_manager_cfg.py b/source/isaaclab/isaaclab/physics/newton_manager_cfg.py index c9c6dc99098..a2dd6e5edcd 100644 --- a/source/isaaclab/isaaclab/physics/newton_manager_cfg.py +++ b/source/isaaclab/isaaclab/physics/newton_manager_cfg.py @@ -3,16 +3,196 @@ # # SPDX-License-Identifier: BSD-3-Clause +"""Configuration for Newton physics manager.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + from isaaclab.utils import configclass -from .solvers_cfg import MJWarpSolverCfg, NewtonSolverCfg +from .physics_manager_cfg import PhysicsManagerCfg + +if TYPE_CHECKING: + from .physics_manager import PhysicsManager @configclass -class NewtonCfg: - """Configuration for Newton-related parameters. +class NewtonSolverCfg: + """Configuration for Newton solver-related parameters. + + These parameters are used to configure the Newton solver. For more information, see the `Newton documentation`_. + + .. _Newton documentation: https://newton.readthedocs.io/en/latest/ + """ + + solver_type: str = "None" + """Solver type. + + Used to select the right solver class. + """ + + +@configclass +class MJWarpSolverCfg(NewtonSolverCfg): + """Configuration for MuJoCo Warp solver-related parameters. + + These parameters are used to configure the MuJoCo Warp solver. For more information, see the + `MuJoCo Warp documentation`_. + + .. _MuJoCo Warp documentation: https://github.com/google-deepmind/mujoco_warp + """ + + solver_type: str = "mujoco_warp" + """Solver type. Can be "mujoco_warp".""" + + njmax: int = 300 + """Number of constraints per environment (world).""" + + nconmax: int | None = None + """Number of contact points per environment (world).""" - These parameters are used to configure the Newton physics simulation. + iterations: int = 100 + """Number of solver iterations.""" + + ls_iterations: int = 50 + """Number of line search iterations for the solver.""" + + solver: str = "newton" + """Solver type. Can be "cg" or "newton", or their corresponding MuJoCo integer constants.""" + + integrator: str = "euler" + """Integrator type. Can be "euler", "rk4", or "implicit", or their corresponding MuJoCo integer constants.""" + + use_mujoco_cpu: bool = False + """Whether to use the pure MuJoCo backend instead of `mujoco_warp`.""" + + disable_contacts: bool = False + """Whether to disable contact computation in MuJoCo.""" + + default_actuator_gear: float | None = None + """Default gear ratio for all actuators.""" + + actuator_gears: dict[str, float] | None = None + """Dictionary mapping joint names to specific gear ratios, overriding the `default_actuator_gear`.""" + + update_data_interval: int = 1 + """Frequency (in simulation steps) at which to update the MuJoCo Data object from the Newton state. + + If 0, Data is never updated after initialization. + """ + + save_to_mjcf: str | None = None + """Optional path to save the generated MJCF model file. + + If None, the MJCF model is not saved. + """ + + impratio: float = 1.0 + """Frictional-to-normal constraint impedance ratio.""" + + cone: str = "pyramidal" + """The type of contact friction cone. Can be "pyramidal" or "elliptic".""" + + ls_parallel: bool = False + """Whether to use parallel line search.""" + + use_mujoco_contacts: bool = True + """Whether to use MuJoCo's contact solver.""" + + +@configclass +class XPBOSolverCfg(NewtonSolverCfg): + """An implicit integrator using eXtended Position-Based Dynamics (XPBD) for rigid and soft body simulation. + + References: + - Miles Macklin, Matthias Müller, and Nuttapong Chentanez. 2016. XPBD: position-based simulation of compliant + constrained dynamics. In Proceedings of the 9th International Conference on Motion in Games (MIG '16). + Association for Computing Machinery, New York, NY, USA, 49-54. https://doi.org/10.1145/2994258.2994272 + - Matthias Müller, Miles Macklin, Nuttapong Chentanez, Stefan Jeschke, and Tae-Yong Kim. 2020. Detailed rigid + body simulation with extended position based dynamics. In Proceedings of the ACM SIGGRAPH/Eurographics + Symposium on Computer Animation (SCA '20). Eurographics Association, Goslar, DEU, + Article 10, 1-12. https://doi.org/10.1111/cgf.14105 + + """ + + solver_type: str = "xpbd" + """Solver type. Can be "xpbd".""" + + iterations: int = 2 + """Number of solver iterations.""" + + soft_body_relaxation: float = 0.9 + """Relaxation parameter for soft body simulation.""" + + soft_contact_relaxation: float = 0.9 + """Relaxation parameter for soft contact simulation.""" + + joint_linear_relaxation: float = 0.7 + """Relaxation parameter for joint linear simulation.""" + + joint_angular_relaxation: float = 0.4 + """Relaxation parameter for joint angular simulation.""" + + joint_linear_compliance: float = 0.0 + """Compliance parameter for joint linear simulation.""" + + joint_angular_compliance: float = 0.0 + """Compliance parameter for joint angular simulation.""" + + rigid_contact_relaxation: float = 0.8 + """Relaxation parameter for rigid contact simulation.""" + + rigid_contact_con_weighting: bool = True + """Whether to use contact constraint weighting for rigid contact simulation.""" + + angular_damping: float = 0.0 + """Angular damping parameter for rigid contact simulation.""" + + enable_restitution: bool = False + """Whether to enable restitution for rigid contact simulation.""" + + +@configclass +class FeatherstoneSolverCfg(NewtonSolverCfg): + """A semi-implicit integrator using symplectic Euler. + + It operates on reduced (also called generalized) coordinates to simulate articulated rigid body dynamics + based on Featherstone's composite rigid body algorithm (CRBA). + + See: Featherstone, Roy. Rigid Body Dynamics Algorithms. Springer US, 2014. + + Semi-implicit time integration is a variational integrator that + preserves energy, however it not unconditionally stable, and requires a time-step + small enough to support the required stiffness and damping forces. + + See: https://en.wikipedia.org/wiki/Semi-implicit_Euler_method + """ + + solver_type: str = "featherstone" + """Solver type. Can be "featherstone".""" + + angular_damping: float = 0.05 + """Angular damping parameter for rigid contact simulation.""" + + update_mass_matrix_interval: int = 1 + """Frequency (in simulation steps) at which to update the mass matrix.""" + + friction_smoothing: float = 1.0 + """Friction smoothing parameter.""" + + use_tile_gemm: bool = False + """Whether to use tile-based GEMM for the mass matrix.""" + + fuse_cholesky: bool = True + """Whether to fuse the Cholesky decomposition.""" + + +@configclass +class NewtonManagerCfg(PhysicsManagerCfg): + """Configuration for Newton physics manager. + + This configuration includes Newton-specific simulation settings and solver configuration. """ num_substeps: int = 1 @@ -28,3 +208,17 @@ class NewtonCfg: """ solver_cfg: NewtonSolverCfg = MJWarpSolverCfg() + """Solver configuration. Default is MJWarpSolverCfg().""" + + # ------------------------------------------------------------------ + # Factory Method + # ------------------------------------------------------------------ + + def create_manager(self) -> type["PhysicsManager"]: + """Create and return the NewtonManager class. + + Returns: + The NewtonManager class. + """ + from .newton_manager import NewtonManager + return NewtonManager diff --git a/source/isaaclab/isaaclab/physics/solvers_cfg.py b/source/isaaclab/isaaclab/physics/solvers_cfg.py deleted file mode 100644 index e9db3b2ee67..00000000000 --- a/source/isaaclab/isaaclab/physics/solvers_cfg.py +++ /dev/null @@ -1,177 +0,0 @@ -# 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 isaaclab.utils import configclass - - -@configclass -class NewtonSolverCfg: - """Configuration for Newton solver-related parameters. - - These parameters are used to configure the Newton solver. For more information, see the `Newton documentation`_. - - .. _Newton documentation: https://newton.readthedocs.io/en/latest/ - """ - - solver_type: str = "None" - """Solver type. - - Used to select the right solver class. - """ - - -@configclass -class MJWarpSolverCfg(NewtonSolverCfg): - """Configuration for MuJoCo Warp solver-related parameters. - - These parameters are used to configure the MuJoCo Warp solver. For more information, see the - `MuJoCo Warp documentation`_. - - .. _MuJoCo Warp documentation: https://github.com/google-deepmind/mujoco_warp - """ - - solver_type: str = "mujoco_warp" - """Solver type. Can be "mujoco_warp".""" - - njmax: int = 300 - """Number of constraints per environment (world).""" - - nconmax: int | None = None - """Number of contact points per environment (world).""" - - iterations: int = 100 - """Number of solver iterations.""" - - ls_iterations: int = 50 - """Number of line search iterations for the solver.""" - - solver: str = "newton" - """Solver type. Can be "cg" or "newton", or their corresponding MuJoCo integer constants.""" - - integrator: str = "euler" - """Integrator type. Can be "euler", "rk4", or "implicit", or their corresponding MuJoCo integer constants.""" - - use_mujoco_cpu: bool = False - """Whether to use the pure MuJoCo backend instead of `mujoco_warp`.""" - - disable_contacts: bool = False - """Whether to disable contact computation in MuJoCo.""" - - default_actuator_gear: float | None = None - """Default gear ratio for all actuators.""" - - actuator_gears: dict[str, float] | None = None - """Dictionary mapping joint names to specific gear ratios, overriding the `default_actuator_gear`.""" - - update_data_interval: int = 1 - """Frequency (in simulation steps) at which to update the MuJoCo Data object from the Newton state. - - If 0, Data is never updated after initialization. - """ - - save_to_mjcf: str | None = None - """Optional path to save the generated MJCF model file. - - If None, the MJCF model is not saved. - """ - - impratio: float = 1.0 - """Frictional-to-normal constraint impedance ratio.""" - - cone: str = "pyramidal" - """The type of contact friction cone. Can be "pyramidal" or "elliptic".""" - - ls_parallel: bool = False - """Whether to use parallel line search.""" - - use_mujoco_contacts: bool = True - """Whether to use MuJoCo's contact solver.""" - - -@configclass -class XPBOSolverCfg(NewtonSolverCfg): - """An implicit integrator using eXtended Position-Based Dynamics (XPBD) for rigid and soft body simulation. - - References: - - Miles Macklin, Matthias Müller, and Nuttapong Chentanez. 2016. XPBD: position-based simulation of compliant - constrained dynamics. In Proceedings of the 9th International Conference on Motion in Games (MIG '16). - Association for Computing Machinery, New York, NY, USA, 49-54. https://doi.org/10.1145/2994258.2994272 - - Matthias Müller, Miles Macklin, Nuttapong Chentanez, Stefan Jeschke, and Tae-Yong Kim. 2020. Detailed rigid - body simulation with extended position based dynamics. In Proceedings of the ACM SIGGRAPH/Eurographics - Symposium on Computer Animation (SCA '20). Eurographics Association, Goslar, DEU, - Article 10, 1-12. https://doi.org/10.1111/cgf.14105 - - """ - - solver_type: str = "xpbd" - """Solver type. Can be "xpbd".""" - - iterations: int = 2 - """Number of solver iterations.""" - - soft_body_relaxation: float = 0.9 - """Relaxation parameter for soft body simulation.""" - - soft_contact_relaxation: float = 0.9 - """Relaxation parameter for soft contact simulation.""" - - joint_linear_relaxation: float = 0.7 - """Relaxation parameter for joint linear simulation.""" - - joint_angular_relaxation: float = 0.4 - """Relaxation parameter for joint angular simulation.""" - - joint_linear_compliance: float = 0.0 - """Compliance parameter for joint linear simulation.""" - - joint_angular_compliance: float = 0.0 - """Compliance parameter for joint angular simulation.""" - - rigid_contact_relaxation: float = 0.8 - """Relaxation parameter for rigid contact simulation.""" - - rigid_contact_con_weighting: bool = True - """Whether to use contact constraint weighting for rigid contact simulation.""" - - angular_damping: float = 0.0 - """Angular damping parameter for rigid contact simulation.""" - - enable_restitution: bool = False - """Whether to enable restitution for rigid contact simulation.""" - - -@configclass -class FeatherstoneSolverCfg(NewtonSolverCfg): - """A semi-implicit integrator using symplectic Euler. - - It operates on reduced (also called generalized) coordinates to simulate articulated rigid body dynamics - based on Featherstone's composite rigid body algorithm (CRBA). - - See: Featherstone, Roy. Rigid Body Dynamics Algorithms. Springer US, 2014. - - Semi-implicit time integration is a variational integrator that - preserves energy, however it not unconditionally stable, and requires a time-step - small enough to support the required stiffness and damping forces. - - See: https://en.wikipedia.org/wiki/Semi-implicit_Euler_method - """ - - solver_type: str = "featherstone" - """Solver type. Can be "featherstone".""" - - angular_damping: float = 0.05 - """Angular damping parameter for rigid contact simulation.""" - - update_mass_matrix_interval: int = 1 - """Frequency (in simulation steps) at which to update the mass matrix.""" - - friction_smoothing: float = 1.0 - """Friction smoothing parameter.""" - - use_tile_gemm: bool = False - """Whether to use tile-based GEMM for the mass matrix.""" - - fuse_cholesky: bool = True - """Whether to fuse the Cholesky decomposition.""" From 1a1188077515d1108429385d1c5e2b70bab56618 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 07:13:35 -0800 Subject: [PATCH 124/130] added in dummy refactored visualizers to show structure --- .../isaaclab/visualizers/newton_visualizer.py | 393 ++++++++++++ .../visualizers/newton_visualizer_cfg.py | 63 ++ .../isaaclab/visualizers/ov_visualizer.py | 586 ++++++++++++++++++ .../isaaclab/visualizers/ov_visualizer_cfg.py | 49 ++ .../isaaclab/visualizers/rerun_visualizer.py | 285 +++++++++ .../visualizers/rerun_visualizer_cfg.py | 39 ++ 6 files changed, 1415 insertions(+) create mode 100644 source/isaaclab/isaaclab/visualizers/newton_visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py create mode 100644 source/isaaclab/isaaclab/visualizers/ov_visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py create mode 100644 source/isaaclab/isaaclab/visualizers/rerun_visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py new file mode 100644 index 00000000000..be6b5e54d06 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py @@ -0,0 +1,393 @@ +# 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 + +"""Newton OpenGL Visualizer implementation.""" + +from __future__ import annotations + +import contextlib +import numpy as np +from typing import Any + +import warp as wp +from newton.viewer import ViewerGL + +from .newton_visualizer_cfg import NewtonVisualizerCfg +from .visualizer import Visualizer + + +class NewtonViewerGL(ViewerGL): + """Wrapper around Newton's ViewerGL with training/rendering pause controls. + + Adds two pause modes: + - Training pause: Stops physics simulation, continues rendering + - Rendering pause: Stops rendering updates, continues physics (SPACE key) + """ + + def __init__(self, *args, metadata: dict | None = None, update_frequency: int = 1, **kwargs): + super().__init__(*args, **kwargs) + self._paused_training = False + self._paused_rendering = False + self._metadata = metadata or {} + self._fallback_draw_controls = False + self._update_frequency = update_frequency + + try: + self.register_ui_callback(self._render_training_controls, position="side") + except AttributeError: + self._fallback_draw_controls = True + + def is_training_paused(self) -> bool: + return self._paused_training + + def is_rendering_paused(self) -> bool: + return self._paused_rendering + + def _render_training_controls(self, imgui): + imgui.separator() + imgui.text("IsaacLab Controls") + + # Pause training/simulation button + pause_label = "Resume Training" if self._paused_training else "Pause Training" + if imgui.button(pause_label): + self._paused_training = not self._paused_training + + # Pause rendering button + rendering_label = "Resume Rendering" if self._paused_rendering else "Pause Rendering" + if imgui.button(rendering_label): + self._paused_rendering = not self._paused_rendering + self._paused = self._paused_rendering # Sync with parent class pause state + + # Visualizer update frequency control + imgui.text("Visualizer Update Frequency") + current_frequency = self._update_frequency + changed, new_frequency = imgui.slider_int( + "##VisualizerUpdateFreq", current_frequency, 1, 20, f"Every {current_frequency} frames" + ) + if changed: + self._update_frequency = new_frequency + + if imgui.is_item_hovered(): + imgui.set_tooltip( + "Controls visualizer update frequency\nlower values -> more responsive visualizer but slower" + " training\nhigher values -> less responsive visualizer but faster training" + ) + + def on_key_press(self, symbol, modifiers): + if self.ui.is_capturing(): + return + + try: + import pyglet # noqa: PLC0415 + except Exception: + return + + if symbol == pyglet.window.key.SPACE: + self._paused_rendering = not self._paused_rendering + self._paused = self._paused_rendering # Sync with parent class pause state + return + + super().on_key_press(symbol, modifiers) + + def _render_ui(self): + if not self._fallback_draw_controls: + return super()._render_ui() + + # Render base UI first + super()._render_ui() + + # Then render a small floating window with training controls + imgui = self.ui.imgui + # Place near left panel but offset + from contextlib import suppress + + with suppress(Exception): + imgui.set_next_window_pos(imgui.ImVec2(320, 10)) + + flags = 0 + if imgui.begin("Training Controls", flags=flags): + self._render_training_controls(imgui) + imgui.end() + return None + + def _render_left_panel(self): + """Override the left panel to remove the base pause checkbox.""" + import newton as nt + + imgui = self.ui.imgui + + # Use theme colors directly + nav_highlight_color = self.ui.get_theme_color(imgui.Col_.nav_cursor, (1.0, 1.0, 1.0, 1.0)) + + # Position the window on the left side + io = self.ui.io + imgui.set_next_window_pos(imgui.ImVec2(10, 10)) + imgui.set_next_window_size(imgui.ImVec2(300, io.display_size[1] - 20)) + + # Main control panel window - use safe flag values + flags = imgui.WindowFlags_.no_resize.value + + if imgui.begin(f"Newton Viewer v{nt.__version__}", flags=flags): + imgui.separator() + + header_flags = 0 + + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("IsaacLab Options"): + # Render UI callbacks for side panel + for callback in self._ui_callbacks["side"]: + callback(self.ui.imgui) + + # Model Information section + if self.model is not None: + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Model Information", flags=header_flags): + imgui.separator() + num_envs = self._metadata.get("num_envs", 0) + imgui.text(f"Environments: {num_envs}") + axis_names = ["X", "Y", "Z"] + imgui.text(f"Up Axis: {axis_names[self.model.up_axis]}") + gravity = wp.to_torch(self.model.gravity)[0] + gravity_text = f"Gravity: ({gravity[0]:.2f}, {gravity[1]:.2f}, {gravity[2]:.2f})" + imgui.text(gravity_text) + + # Visualization Controls section + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Visualization", flags=header_flags): + imgui.separator() + + # Joint visualization + show_joints = self.show_joints + changed, self.show_joints = imgui.checkbox("Show Joints", show_joints) + + # Contact visualization + show_contacts = self.show_contacts + changed, self.show_contacts = imgui.checkbox("Show Contacts", show_contacts) + + # Spring visualization + show_springs = self.show_springs + changed, self.show_springs = imgui.checkbox("Show Springs", show_springs) + + # Center of mass visualization + show_com = self.show_com + changed, self.show_com = imgui.checkbox("Show Center of Mass", show_com) + + # Rendering Options section + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Rendering Options"): + imgui.separator() + + # Sky rendering + changed, self.renderer.draw_sky = imgui.checkbox("Sky", self.renderer.draw_sky) + + # Shadow rendering + changed, self.renderer.draw_shadows = imgui.checkbox("Shadows", self.renderer.draw_shadows) + + # Wireframe mode + changed, self.renderer.draw_wireframe = imgui.checkbox("Wireframe", self.renderer.draw_wireframe) + + # Light color + changed, self.renderer._light_color = imgui.color_edit3("Light Color", self.renderer._light_color) + # Sky color + changed, self.renderer.sky_upper = imgui.color_edit3("Sky Color", self.renderer.sky_upper) + # Ground color + changed, self.renderer.sky_lower = imgui.color_edit3("Ground Color", self.renderer.sky_lower) + + # Camera Information section + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Camera"): + imgui.separator() + + pos = self.camera.pos + pos_text = f"Position: ({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})" + imgui.text(pos_text) + imgui.text(f"FOV: {self.camera.fov:.1f}°") + imgui.text(f"Yaw: {self.camera.yaw:.1f}°") + imgui.text(f"Pitch: {self.camera.pitch:.1f}°") + + # Camera controls hint - update to reflect new controls + imgui.separator() + imgui.push_style_color(imgui.Col_.text, imgui.ImVec4(*nav_highlight_color)) + imgui.text("Controls:") + imgui.pop_style_color() + imgui.text("WASD - Forward/Left/Back/Right") + imgui.text("QE - Down/Up") + imgui.text("Left Click - Look around") + imgui.text("Scroll - Zoom") + imgui.text("Space - Pause/Resume Rendering") + imgui.text("H - Toggle UI") + imgui.text("ESC - Exit") + + imgui.end() + return + + +class NewtonVisualizer(Visualizer): + """Newton OpenGL visualizer for Isaac Lab. + + Lightweight OpenGL-based visualization with training/rendering pause controls. + """ + + def __init__(self, cfg: NewtonVisualizerCfg): + super().__init__(cfg) + self.cfg: NewtonVisualizerCfg = cfg + self._viewer: NewtonViewerGL | None = None + self._sim_time = 0.0 + self._step_counter = 0 + self._model = None + self._state = None + self._update_frequency = cfg.update_frequency + self._scene_data_provider = None + + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + """Initialize visualizer with scene data.""" + if self._is_initialized: + return + + # Import NewtonManager for metadata access + from isaaclab.physics.newton_manager import NewtonManager + + # Store scene data provider for accessing physics state + if scene_data and "scene_data_provider" in scene_data: + self._scene_data_provider = scene_data["scene_data_provider"] + + # Get Newton-specific data from scene data provider + if self._scene_data_provider: + self._model = self._scene_data_provider.get_model() + self._state = self._scene_data_provider.get_state() + else: + # Fallback: direct access to NewtonManager (for backward compatibility) + self._model = NewtonManager._model + self._state = NewtonManager._state_0 + + if self._model is None: + raise RuntimeError("Newton visualizer requires Newton Model. Ensure Newton physics is initialized first.") + + # Build metadata from NewtonManager + metadata = { + "physics_backend": "newton", + "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, + "gravity_vector": NewtonManager._gravity_vector, + "clone_physics_only": NewtonManager._clone_physics_only, + } + + # Create the viewer with metadata + self._viewer = NewtonViewerGL( + width=self.cfg.window_width, + height=self.cfg.window_height, + metadata=metadata, + update_frequency=self.cfg.update_frequency, + ) + + # Set the model + self._viewer.set_model(self._model) + + # Disable auto world spacing in Newton Viewer to display envs at actual world positions + self._viewer.set_world_offsets((0.0, 0.0, 0.0)) + + # Configure camera position and orientation (Z-up axis) + self._viewer.camera.pos = wp.vec3(*self.cfg.camera_position) + self._viewer.up_axis = 2 # Z-up + + # Calculate pitch and yaw from camera_position and camera_target + cam_pos = np.array(self.cfg.camera_position, dtype=np.float32) + cam_target = np.array(self.cfg.camera_target, dtype=np.float32) + direction = cam_target - cam_pos + + # Calculate yaw and pitch for Z-up coordinate system + # Yaw: rotation around Z axis (horizontal plane) + yaw = np.degrees(np.arctan2(direction[1], direction[0])) + # Pitch: elevation angle + horizontal_dist = np.sqrt(direction[0] ** 2 + direction[1] ** 2) + pitch = np.degrees(np.arctan2(direction[2], horizontal_dist)) + + self._viewer.camera.yaw = float(yaw) + self._viewer.camera.pitch = float(pitch) + + self._viewer.scaling = 1.0 + self._viewer._paused = False + + # Configure visualization options + self._viewer.show_joints = self.cfg.show_joints + self._viewer.show_contacts = self.cfg.show_contacts + self._viewer.show_springs = self.cfg.show_springs + self._viewer.show_com = self.cfg.show_com + + # Configure rendering options + self._viewer.renderer.draw_shadows = self.cfg.enable_shadows + self._viewer.renderer.draw_sky = self.cfg.enable_sky + self._viewer.renderer.draw_wireframe = self.cfg.enable_wireframe + + # Configure colors + self._viewer.renderer.sky_upper = self.cfg.background_color + self._viewer.renderer.sky_lower = self.cfg.ground_color + self._viewer.renderer._light_color = self.cfg.light_color + + self._is_initialized = True + + def step(self, dt: float, state: Any | None = None) -> None: + """Update visualizer for one step.""" + if not self._is_initialized or self._is_closed or self._viewer is None: + return + + self._sim_time += dt + self._step_counter += 1 + + # Fetch updated state from scene data provider + if self._scene_data_provider: + self._state = self._scene_data_provider.get_state() + else: + # Fallback: direct access to NewtonManager + from isaaclab.physics.newton_manager import NewtonManager + + self._state = NewtonManager._state_0 + + # Only update visualizer at the specified frequency + update_frequency = self._viewer._update_frequency if self._viewer else self._update_frequency + if self._step_counter % update_frequency != 0: + return + + with contextlib.suppress(Exception): + if not self._viewer.is_paused(): + self._viewer.begin_frame(self._sim_time) + if self._state is not None: + self._viewer.log_state(self._state) + self._viewer.end_frame() + else: + self._viewer._update() + + def close(self) -> None: + """Close visualizer and clean up resources.""" + if self._is_closed: + return + if self._viewer is not None: + self._viewer = None + self._is_closed = True + + def is_running(self) -> bool: + """Check if visualizer window is still open.""" + if not self._is_initialized or self._is_closed or self._viewer is None: + return False + return self._viewer.is_running() + + def supports_markers(self) -> bool: + """Newton visualizer does not have this feature yet.""" + return False + + def supports_live_plots(self) -> bool: + """Newton visualizer does not have this feature yet.""" + return False + + def is_training_paused(self) -> bool: + """Check if training is paused.""" + if not self._is_initialized or self._viewer is None: + return False + return self._viewer.is_training_paused() + + def is_rendering_paused(self) -> bool: + """Check if rendering is paused.""" + if not self._is_initialized or self._viewer is None: + return False + return self._viewer.is_rendering_paused() diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py new file mode 100644 index 00000000000..922c90c010e --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py @@ -0,0 +1,63 @@ +# 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 + +"""Configuration for Newton OpenGL Visualizer.""" + +from isaaclab.utils import configclass + +from .visualizer_cfg import VisualizerCfg + + +@configclass +class NewtonVisualizerCfg(VisualizerCfg): + """Configuration for Newton OpenGL visualizer. + + Lightweight OpenGL-based visualizer with real-time 3D rendering, interactive + camera controls, and debug visualization (contacts, joints, springs, COM). + + Requires: pyglet >= 2.1.6, imgui_bundle >= 1.92.0 + """ + + visualizer_type: str = "newton" + """Type identifier for Newton visualizer.""" + + window_width: int = 1920 + """Window width in pixels.""" + + window_height: int = 1080 + """Window height in pixels.""" + + update_frequency: int = 1 + """Visualizer update frequency (updates every N frames). Lower = more responsive but slower training.""" + + show_joints: bool = False + """Show joint visualization.""" + + show_contacts: bool = False + """Show contact visualization.""" + + show_springs: bool = False + """Show spring visualization.""" + + show_com: bool = False + """Show center of mass visualization.""" + + enable_shadows: bool = True + """Enable shadow rendering.""" + + enable_sky: bool = True + """Enable sky rendering.""" + + enable_wireframe: bool = False + """Enable wireframe rendering.""" + + background_color: tuple[float, float, float] = (0.53, 0.81, 0.92) + """Background/sky color RGB [0,1].""" + + ground_color: tuple[float, float, float] = (0.18, 0.20, 0.25) + """Ground color RGB [0,1].""" + + light_color: tuple[float, float, float] = (1.0, 1.0, 1.0) + """Light color RGB [0,1].""" diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py new file mode 100644 index 00000000000..0a3109e219c --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py @@ -0,0 +1,586 @@ +# 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 + +"""Omniverse visualizer for PhysX-based SimulationContext.""" + +from __future__ import annotations + +import asyncio +import logging +import enum +from typing import Any +import omni.kit.app +from pxr import UsdGeom + +from .ov_visualizer_cfg import OVVisualizerCfg +from .visualizer import Visualizer + +logger = logging.getLogger(__name__) + + +class RenderMode(enum.IntEnum): + """Different rendering modes for the simulation. + + Render modes correspond to how the viewport and other UI elements (such as listeners to keyboard or mouse + events) are updated. There are three main components that can be updated when the simulation is rendered: + + 1. **UI elements and other extensions**: These are UI elements (such as buttons, sliders, etc.) and other + extensions that are running in the background that need to be updated when the simulation is running. + 2. **Cameras**: These are typically based on Hydra textures and are used to render the scene from different + viewpoints. They can be attached to a viewport or be used independently to render the scene. + 3. **Viewports**: These are windows where you can see the rendered scene. + + Updating each of the above components has a different overhead. For example, updating the viewports is + computationally expensive compared to updating the UI elements. Therefore, it is useful to be able to + control what is updated when the simulation is rendered. This is where the render mode comes in. There are + four different render modes: + + * :attr:`NO_GUI_OR_RENDERING`: The simulation is running without a GUI and off-screen rendering flag is disabled, + so none of the above are updated. + * :attr:`NO_RENDERING`: No rendering, where only 1 is updated at a lower rate. + * :attr:`PARTIAL_RENDERING`: Partial rendering, where only 1 and 2 are updated. + * :attr:`FULL_RENDERING`: Full rendering, where everything (1, 2, 3) is updated. + + .. _Viewports: https://docs.omniverse.nvidia.com/extensions/latest/ext_viewport.html + """ + + NO_GUI_OR_RENDERING = -1 + """The simulation is running without a GUI and off-screen rendering is disabled.""" + NO_RENDERING = 0 + """No rendering, where only other UI elements are updated at a lower rate.""" + PARTIAL_RENDERING = 1 + """Partial rendering, where the simulation cameras and UI elements are updated.""" + FULL_RENDERING = 2 + """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" + + +class OVVisualizer(Visualizer): + """Omniverse visualizer managing viewport/rendering for PhysX workflow. + + This class extends the base :class:`Visualizer` and handles: + - Viewport context and window management + - Render mode switching + - Camera view setup + - Render settings from configuration + - Throttled rendering for UI responsiveness + - Timeline control (play/pause/stop) + + Lifecycle: __init__(cfg) -> initialize(scene_data) -> step() (repeated) -> close() + """ + + def __init__(self, cfg: "OVVisualizerCfg"): + """Initialize OV visualizer with configuration. + + Args: + cfg: Configuration for the visualizer. + """ + super().__init__(cfg) + self.cfg: "OVVisualizerCfg" = cfg + + # Will be set during initialize() + self._simulation_context = None + self._simulation_app = None + self._simulation_app_running = False + self._timeline = None + + # Viewport state + self._viewport_context = None + self._viewport_window = None + self._viewport_api = None + self._render_throttle_counter = 0 + self._render_throttle_period = cfg.render_throttle_period + + # Render mode + self.render_mode = RenderMode.NO_GUI_OR_RENDERING + + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + """Initialize visualizer with simulation context. + + Args: + scene_data: Dictionary containing: + - 'simulation_context': The SimulationContext instance (required) + - 'usd_stage': The USD stage (optional, can get from sim context) + """ + if self._is_initialized: + logger.warning("[OVVisualizer] Already initialized.") + return + + usd_stage = None + if scene_data is not None: + usd_stage = scene_data.get("usd_stage") + self._simulation_context = scene_data.get("simulation_context") + + if usd_stage is None: + raise RuntimeError("OV visualizer requires a USD stage.") + + # Build metadata from simulation context if available + metadata = {} + if self._simulation_context is not None: + # Try to get num_envs from the simulation context's scene if available + num_envs = 0 + if hasattr(self._simulation_context, "scene") and self._simulation_context.scene is not None: + if hasattr(self._simulation_context.scene, "num_envs"): + num_envs = self._simulation_context.scene.num_envs + + # Detect physics backend (could be extended to check actual backend type) + physics_backend = "newton" # Default for now, could be made more sophisticated + + metadata = { + "num_envs": num_envs, + "physics_backend": physics_backend, + "env_prim_pattern": "/World/envs/env_{}", # Standard pattern + } + + self._simulation_context.settings.set_bool("/app/player/playSimulations", False) + self._offscreen_render = bool(self._simulation_context.settings.get("/isaaclab/render/offscreen")) + self._render_viewport = bool(self._simulation_context.settings.get("/isaaclab/render/active_viewport")) + self._has_gui = bool(self._simulation_context.settings.get("/isaaclab/render/active_viewport")) + # Set render mode + if not self._has_gui and not self._offscreen_render: + self.render_mode = RenderMode.NO_GUI_OR_RENDERING + elif not self._has_gui and self._offscreen_render: + self.render_mode = RenderMode.PARTIAL_RENDERING + else: + self.render_mode = RenderMode.FULL_RENDERING + + # enable viewport updates if GUI is enabled + if self._has_gui: + try: + import omni.ui as ui + from omni.kit.viewport.utility import get_active_viewport + self._viewport_context = get_active_viewport() + self._viewport_context.updates_enabled = True + self._viewport_window = ui.Workspace.get_window("Viewport") + except (ImportError, AttributeError): + pass + + # Disable viewport for offscreen-only rendering + if not self._render_viewport and self._offscreen_render: + try: + from omni.kit.viewport.utility import get_active_viewport + get_active_viewport().updates_enabled = False + except (ImportError, AttributeError): + pass + + self._ensure_simulation_app() + self._setup_viewport(usd_stage, metadata) + + num_envs = metadata.get("num_envs", 0) + physics_backend = metadata.get("physics_backend", "unknown") + logger.info(f"[OVVisualizer] Initialized ({num_envs} envs, {physics_backend} physics)") + + self._is_initialized = True + + def step(self, dt: float, state: Any | None = None) -> None: + """Update visualization for one step (render the scene). + + Args: + dt: Time step in seconds. + state: Updated physics state (unused for OV - USD stage is auto-synced). + """ + if not self._is_initialized: + return + + self.render() + + def render(self, mode: RenderMode | None = None) -> None: + """Refreshes the rendering components including UI elements and view-ports depending on the render mode. + + This function is used to refresh the rendering components of the simulation. This includes updating the + view-ports, UI elements, and other extensions (besides physics simulation) that are running in the + background. The rendering components are refreshed based on the render mode. + + Please see :class:`RenderMode` for more information on the different render modes. + + Args: + mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. + """ + if self._simulation_context is None: + return + + # check if we need to change the render mode + if mode is not None: + self.set_render_mode(mode) + + self._simulation_context.settings.set_bool("/app/player/playSimulations", False) + omni.kit.app.get_app().update() + self._simulation_context.settings.set_bool("/app/player/playSimulations", True) + + # app.update() may be changing the cuda device, so we force it back to our desired device here + if "cuda" in self._simulation_context.device: + import torch + + torch.cuda.set_device(self._simulation_context.device) + + def is_running(self) -> bool: + """Check if visualizer is still running.""" + return self.is_playing() + + def is_playing(self) -> bool: + """Check whether the simulation is playing.""" + if self._timeline is None: + return False + return self._timeline.is_playing() + + def is_stopped(self) -> bool: + """Check whether the simulation is stopped.""" + if self._timeline is None: + return True + return self._timeline.is_stopped() + + def play(self) -> None: + """Start playing the simulation.""" + if self._timeline is not None: + self._timeline.play() + + def pause(self) -> None: + """Pause the simulation.""" + if self._timeline is not None: + self._timeline.pause() + + def stop(self) -> None: + """Stop the simulation.""" + if self._timeline is not None: + self._timeline.stop() + + def is_training_paused(self) -> bool: + """Check if training is paused (always False for OV).""" + return False + + def supports_markers(self) -> bool: + """Supports markers via USD prims.""" + return True + + def supports_live_plots(self) -> bool: + """Supports live plots via Isaac Lab UI.""" + return True + + def get_rendering_dt(self) -> float | None: + """Get rendering dt based on OV rate limiting settings.""" + if self._simulation_context is None: + return None + + settings = self._simulation_context.settings + + def _from_frequency(): + freq = settings.get("/app/runLoops/main/rateLimitFrequency") + return 1.0 / freq if freq else None + + if settings.get("/app/runLoops/main/rateLimitEnabled"): + return _from_frequency() + + try: + import omni.kit.loop._loop as omni_loop + + runner = omni_loop.acquire_loop_interface() + return runner.get_manual_step_size() if runner.get_manual_mode() else _from_frequency() + except Exception: + return _from_frequency() + + def set_camera_view( + self, eye: tuple[float, float, float] | list[float], target: tuple[float, float, float] | list[float] + ) -> None: + """Set the location and target of the viewport camera in the stage. + + Note: + This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. + It is provided here for convenience to reduce the amount of imports needed. + + Args: + eye: The location of the camera eye. + target: The location of the camera target. + camera_prim_path: The path to the camera primitive in the stage. Defaults to + the value from config or "/OmniverseKit_Persp". + """ + if not self._is_initialized: + logger.warning("[OVVisualizer] Cannot set camera view - visualizer not initialized.") + return + + try: + # Import Isaac Sim viewport utilities + import isaacsim.core.utils.viewports as vp_utils + + # Get the camera prim path for this viewport + camera_path = self._viewport_api.get_active_camera() + if not camera_path: + camera_path = "/OmniverseKit_Persp" # Default camera + + # Use Isaac Sim utility to set camera view + vp_utils._visualizer_interface.set_camera_view( + eye=list(eye), target=list(target), camera_prim_path=camera_path, viewport_api=self._viewport_api + ) + + logger.info(f"[OVVisualizer] Camera set: pos={eye}, target={target}, camera={camera_path}") + + except Exception as e: + logger.warning(f"[OVVisualizer] Could not set camera: {e}") + + def set_render_mode(self, mode: RenderMode) -> None: + """Change the current render mode of the simulation. + + Please see :class:`RenderMode` for more information on the different render modes. + + .. note:: + When no GUI is available (locally or livestreamed), we do not need to choose whether the viewport + needs to render or not (since there is no GUI). Thus, in this case, calling the function will not + change the render mode. + + Args: + mode: The rendering mode. If different than current rendering mode, + the mode is changed to the new mode. + + Raises: + ValueError: If the input mode is not supported. + """ + # check if mode change is possible -- not possible when no GUI is available + if not getattr(self, "_has_gui", True): + logger.warning( + f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." + ) + return + # check if there is a mode change + # note: this is mostly needed for GUI when we want to switch between full rendering and no rendering. + if mode != self.render_mode: + if mode == RenderMode.FULL_RENDERING: + # display the viewport and enable updates + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = True # pyright: ignore [reportOptionalMemberAccess] + elif mode == RenderMode.PARTIAL_RENDERING: + # hide the viewport and disable updates + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + elif mode == RenderMode.NO_RENDERING: + # hide the viewport and disable updates + if self._viewport_context is not None: + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + # reset the throttle counter + self._render_throttle_counter = 0 + else: + raise ValueError(f"Unsupported render mode: {mode}! Please check `RenderMode` for details.") + # update render mode + self.render_mode = mode + + def close(self) -> None: + """Clean up visualizer resources.""" + # Note: We don't close the SimulationApp here as it's managed by AppLauncher + self._simulation_context = None + self._simulation_app = None + self._viewport_context = None + self._viewport_window = None + self._viewport_api = None + self._is_initialized = False + self._is_closed = True + + # ------------------------------------------------------------------ + # Private methods + # ------------------------------------------------------------------ + + def _ensure_simulation_app(self) -> None: + """Ensure Isaac Sim app is running.""" + try: + # Check if omni.kit.app is available (indicates Isaac Sim is running) + import omni.kit.app + + # Get the running app instance + app = omni.kit.app.get_app() + if app is None or not app.is_running(): + raise RuntimeError( + "[OVVisualizer] No Isaac Sim app is running. " + "OV visualizer requires Isaac Sim to be launched via AppLauncher before initialization. " + "Ensure your script calls AppLauncher before creating the environment." + ) + + # Try to get SimulationApp instance for headless check + try: + from isaacsim import SimulationApp + + # Check various ways SimulationApp might store its instance + sim_app = None + if hasattr(SimulationApp, "_instance") and SimulationApp._instance is not None: + sim_app = SimulationApp._instance + elif hasattr(SimulationApp, "instance") and callable(SimulationApp.instance): + sim_app = SimulationApp.instance() + + if sim_app is not None: + self._simulation_app = sim_app + + # Check if running in headless mode + if self._simulation_app.config.get("headless", False): # pyright: ignore [reportAttributeAccessIssue] + logger.warning( + "[OVVisualizer] Running in headless mode. " + "OV visualizer requires GUI mode (launch with --headless=False) to create viewports." + ) + else: + logger.info("[OVVisualizer] Using existing Isaac Sim app instance.") + else: + # App is running but we couldn't get SimulationApp instance + # This is okay - we can still use omni APIs + logger.info("[OVVisualizer] Isaac Sim app is running (via omni.kit.app).") + self._simulation_app_running = True + except ImportError: + # SimulationApp not available, but omni.kit.app is running + logger.info("[OVVisualizer] Using running Isaac Sim app (SimulationApp module not available).") + + except ImportError as e: + raise ImportError( + f"[OVVisualizer] Could not import omni.kit.app: {e}. Isaac Sim may not be installed or not running." + ) + + def _setup_viewport(self, usd_stage, metadata: dict) -> None: + """Setup viewport with camera and window size.""" + try: + import omni.kit.viewport.utility as vp_utils + from omni.ui import DockPosition + + # Create new viewport or use existing + if self.cfg.create_viewport and self.cfg.viewport_name: + # Map dock position string to enum + dock_position_map = { + "LEFT": DockPosition.LEFT, + "RIGHT": DockPosition.RIGHT, + "BOTTOM": DockPosition.BOTTOM, + "SAME": DockPosition.SAME, + } + dock_pos = dock_position_map.get(self.cfg.dock_position.upper(), DockPosition.SAME) + + # Create new viewport with proper API + self._viewport_window = vp_utils.create_viewport_window( + name=self.cfg.viewport_name, + width=self.cfg.window_width, + height=self.cfg.window_height, + position_x=50, + position_y=50, + docked=True, + ) + + logger.info(f"[OVVisualizer] Created viewport '{self.cfg.viewport_name}'") + + # Dock the viewport asynchronously (needs to wait for window creation) + asyncio.ensure_future(self._dock_viewport_async(self.cfg.viewport_name, dock_pos)) + + # Create dedicated camera for this viewport + if self._viewport_window: + self._create_and_assign_camera(usd_stage) + else: + # Use existing viewport by name, or fall back to active viewport + if self.cfg.viewport_name: + self._viewport_window = vp_utils.get_viewport_window_by_name(self.cfg.viewport_name) + + if self._viewport_window is None: + logger.warning( + f"[OVVisualizer] Viewport '{self.cfg.viewport_name}' not found. " + "Using active viewport instead." + ) + self._viewport_window = vp_utils.get_active_viewport_window() + else: + logger.info(f"[OVVisualizer] Using existing viewport '{self.cfg.viewport_name}'") + else: + self._viewport_window = vp_utils.get_active_viewport_window() + logger.info("[OVVisualizer] Using existing active viewport") + + if self._viewport_window is None: + logger.warning("[OVVisualizer] Could not get/create viewport.") + return + + # Get viewport API for camera control + self._viewport_api = self._viewport_window.viewport_api + + # Set camera pose (uses existing camera if not created above) + self.set_camera_view(self.cfg.camera_position, self.cfg.camera_target) + + logger.info(f"[OVVisualizer] Viewport configured (size: {self.cfg.window_width}x{self.cfg.window_height})") + + except ImportError as e: + logger.warning(f"[OVVisualizer] Viewport utilities unavailable: {e}") + except Exception as e: + logger.error(f"[OVVisualizer] Error setting up viewport: {e}") + + async def _dock_viewport_async(self, viewport_name: str, dock_position) -> None: + """Dock viewport window asynchronously after it's created. + + Args: + viewport_name: Name of the viewport window to dock. + dock_position: DockPosition enum value for where to dock. + """ + try: + import omni.kit.app + import omni.ui + + # Wait for the viewport window to be created in the workspace + viewport_window = None + for i in range(10): # Try up to 10 frames + viewport_window = omni.ui.Workspace.get_window(viewport_name) + if viewport_window: + logger.info(f"[OVVisualizer] Found viewport window '{viewport_name}' after {i} frames") + break + await omni.kit.app.get_app().next_update_async() + + if not viewport_window: + logger.warning( + f"[OVVisualizer] Could not find viewport window '{viewport_name}' in workspace for docking." + ) + return + + # Get the main viewport to dock relative to + main_viewport = omni.ui.Workspace.get_window("Viewport") + if not main_viewport: + # Try alternative viewport names + for alt_name in ["/OmniverseKit/Viewport", "Viewport Next"]: + main_viewport = omni.ui.Workspace.get_window(alt_name) + if main_viewport: + break + + if main_viewport and main_viewport != viewport_window: + # Dock the new viewport relative to the main viewport + viewport_window.dock_in(main_viewport, dock_position, 0.5) + + # Wait a frame for docking to complete + await omni.kit.app.get_app().next_update_async() + + # Make the new viewport the active/focused tab + # Try multiple methods to ensure it becomes active + viewport_window.focus() + viewport_window.visible = True + + # Wait another frame and focus again (sometimes needed for tabs) + await omni.kit.app.get_app().next_update_async() + viewport_window.focus() + + logger.info( + f"[OVVisualizer] Docked viewport '{viewport_name}' at position {self.cfg.dock_position} and set as" + " active" + ) + else: + logger.info( + f"[OVVisualizer] Could not find main viewport for docking. Viewport '{viewport_name}' will remain" + " floating." + ) + + except Exception as e: + logger.warning(f"[OVVisualizer] Error docking viewport: {e}") + + def _create_and_assign_camera(self, usd_stage) -> None: + """Create a dedicated camera for this viewport and assign it.""" + try: + # Create camera prim path based on viewport name + camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera" + + # Check if camera already exists + camera_prim = usd_stage.GetPrimAtPath(camera_path) + if not camera_prim.IsValid(): + # Create camera prim + UsdGeom.Camera.Define(usd_stage, camera_path) + logger.info(f"[OVVisualizer] Created camera: {camera_path}") + else: + logger.info(f"[OVVisualizer] Using existing camera: {camera_path}") + + # Assign camera to viewport + if self._viewport_api: + self._viewport_api.set_active_camera(camera_path) + logger.info(f"[OVVisualizer] Assigned camera '{camera_path}' to viewport '{self.cfg.viewport_name}'") + + except Exception as e: + logger.warning(f"[OVVisualizer] Could not create/assign camera: {e}. Using default camera.") diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py new file mode 100644 index 00000000000..bbe0505b055 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py @@ -0,0 +1,49 @@ +# 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-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Omniverse-based visualizer.""" + +from isaaclab.utils import configclass + +from .visualizer_cfg import VisualizerCfg + + +@configclass +class OVVisualizerCfg(VisualizerCfg): + """Configuration for Omniverse visualizer using Isaac Sim viewport. + + Displays USD stage, VisualizationMarkers, and LivePlots. + Can attach to existing app or launch standalone. + """ + + visualizer_type: str = "omniverse" + """Type identifier for Omniverse visualizer.""" + + render_throttle_period: int = 5 + """Throttle period for rendering updates. + + This controls how frequently UI elements are updated when in NO_RENDERING mode. + A higher value means less frequent UI updates, improving performance. + """ + + viewport_name: str | None = "Visualizer Viewport" + """Viewport name to use. If None, uses active viewport.""" + + create_viewport: bool = True + """Create new viewport with specified name and camera pose.""" + + dock_position: str = "SAME" + """Dock position for new viewport. Options: 'LEFT', 'RIGHT', 'BOTTOM', 'SAME' (tabs with existing).""" + + window_width: int = 1280 + """Viewport width in pixels.""" + + window_height: int = 720 + """Viewport height in pixels.""" diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py new file mode 100644 index 00000000000..e21028a1937 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py @@ -0,0 +1,285 @@ +# 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 + +"""Rerun-based visualizer using rerun-sdk.""" + +from __future__ import annotations + +import logging +from typing import Any + +from .rerun_visualizer_cfg import RerunVisualizerCfg +from .visualizer import Visualizer + +# Set up logger +logger = logging.getLogger(__name__) + +# Try to import rerun and Newton's ViewerRerun +try: + import rerun as rr + import rerun.blueprint as rrb + from newton.viewer import ViewerRerun + + _RERUN_AVAILABLE = True +except ImportError: + rr = None + rrb = None + ViewerRerun = None + _RERUN_AVAILABLE = False + + +class NewtonViewerRerun(ViewerRerun if _RERUN_AVAILABLE else object): + """Isaac Lab wrapper for Newton's ViewerRerun.""" + + def __init__( + self, + app_id: str | None = None, + web_port: int | None = None, + keep_historical_data: bool = False, + keep_scalar_history: bool = True, + record_to_rrd: str | None = None, + metadata: dict | None = None, + ): + """Initialize Newton ViewerRerun wrapper.""" + # Call parent with Newton parameters + super().__init__( + app_id=app_id, + web_port=web_port, + serve_web_viewer=True, # always launch local web viewer in browser + keep_historical_data=keep_historical_data, + keep_scalar_history=keep_scalar_history, + record_to_rrd=record_to_rrd, + ) + + # Isaac Lab state + self._metadata = metadata or {} + + # Log metadata on initialization + self._log_metadata() + + def _log_metadata(self) -> None: + """Log scene metadata to Rerun as text.""" + metadata_text = "# Isaac Lab Scene Metadata\n\n" + + # Physics info + physics_backend = self._metadata.get("physics_backend", "unknown") + metadata_text += f"**Physics Backend:** {physics_backend}\n" + + # Environment info + num_envs = self._metadata.get("num_envs", 0) + metadata_text += f"**Total Environments:** {num_envs}\n" + + # Additional metadata + for key, value in self._metadata.items(): + if key not in ["physics_backend", "num_envs"]: + metadata_text += f"**{key}:** {value}\n" + + # Log to Rerun + rr.log("metadata", rr.TextDocument(metadata_text, media_type=rr.MediaType.MARKDOWN)) + + +class RerunVisualizer(Visualizer): + """Rerun web-based visualizer with time scrubbing, recording, and data inspection. + + Requires Newton physics backend and rerun-sdk (pip install rerun-sdk).""" + + def __init__(self, cfg: RerunVisualizerCfg): + """Initialize Rerun visualizer.""" + super().__init__(cfg) + self.cfg: RerunVisualizerCfg = cfg + + if not _RERUN_AVAILABLE: + raise ImportError("Rerun visualizer requires rerun-sdk and Newton. Install: pip install rerun-sdk") + + self._viewer: NewtonViewerRerun | None = None + self._model = None + self._state = None + self._is_initialized = False + self._sim_time = 0.0 + self._scene_data_provider = None + + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + """Initialize visualizer with Newton Model and State.""" + if self._is_initialized: + logger.warning("[RerunVisualizer] Already initialized. Skipping re-initialization.") + return + + # Import NewtonManager for metadata access + from isaaclab.physics.newton_manager import NewtonManager + + # Store scene data provider for accessing physics state + if scene_data and "scene_data_provider" in scene_data: + self._scene_data_provider = scene_data["scene_data_provider"] + + # Get Newton-specific data from scene data provider + if self._scene_data_provider: + self._model = self._scene_data_provider.get_model() + self._state = self._scene_data_provider.get_state() + else: + # Fallback: direct access to NewtonManager (for backward compatibility) + self._model = NewtonManager._model + self._state = NewtonManager._state_0 + + # Validate required Newton data + if self._model is None: + raise RuntimeError( + "Rerun visualizer requires a Newton Model. " + "Make sure Newton physics is initialized before creating the visualizer." + ) + + if self._state is None: + logger.warning("[RerunVisualizer] No Newton State available. Visualization may not work correctly.") + + # Build metadata from NewtonManager + metadata = { + "physics_backend": "newton", + "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, + "gravity_vector": NewtonManager._gravity_vector, + "clone_physics_only": NewtonManager._clone_physics_only, + } + + # Create Newton ViewerRerun wrapper + try: + if self.cfg.record_to_rrd: + logger.info(f"[RerunVisualizer] Recording enabled to: {self.cfg.record_to_rrd}") + + self._viewer = NewtonViewerRerun( + app_id=self.cfg.app_id, + web_port=self.cfg.web_port, + keep_historical_data=self.cfg.keep_historical_data, + keep_scalar_history=self.cfg.keep_scalar_history, + record_to_rrd=self.cfg.record_to_rrd, + metadata=metadata, + ) + + # Set the model + self._viewer.set_model(self._model) + + # Disable auto world spacing in Newton Viewer to display envs at actual world positions + self._viewer.set_world_offsets((0.0, 0.0, 0.0)) + + # Set initial camera view using Rerun's blueprint system + try: + # Get camera configuration + cam_pos = self.cfg.camera_position + cam_target = self.cfg.camera_target + + # Create blueprint with 3D view and camera settings + blueprint = rrb.Blueprint( + rrb.Spatial3DView( + name="3D View", + origin="/", + eye_controls=rrb.EyeControls3D( + position=cam_pos, + look_target=cam_target, + ), + ), + collapse_panels=True, + ) + rr.send_blueprint(blueprint) + + logger.info(f"[RerunVisualizer] Set initial camera view: position={cam_pos}, target={cam_target}") + except Exception as e: + logger.warning(f"[RerunVisualizer] Could not set initial camera view: {e}") + + # Log initialization + num_envs = metadata.get("num_envs", 0) + physics_backend = metadata.get("physics_backend", "newton") + logger.info(f"[RerunVisualizer] Initialized with {num_envs} environments (physics: {physics_backend})") + + self._is_initialized = True + + except Exception as e: + logger.error(f"[RerunVisualizer] Failed to initialize viewer: {e}") + raise + + def step(self, dt: float, state: Any | None = None) -> None: + """Update visualizer each step. + + This method: + 1. Fetches updated state from NewtonManager + 2. Logs current state to Rerun (transforms, meshes) + 3. Actively logs markers (if enabled) + 4. Actively logs plot data (if enabled) + + Args: + dt: Time step in seconds. + state: Unused (deprecated parameter, kept for API compatibility). + """ + if not self._is_initialized or self._viewer is None: + logger.warning("[RerunVisualizer] Not initialized. Call initialize() first.") + return + + # Fetch updated state from scene data provider + if self._scene_data_provider: + self._state = self._scene_data_provider.get_state() + else: + # Fallback: direct access to NewtonManager + from isaaclab.physics.newton_manager import NewtonManager + + self._state = NewtonManager._state_0 + + # Update internal time + self._sim_time += dt + + # Begin frame with current simulation time + self._viewer.begin_frame(self._sim_time) + + # Log state (transforms) - Newton's ViewerRerun handles this + if self._state is not None: + self._viewer.log_state(self._state) + + # End frame + self._viewer.end_frame() + + def close(self) -> None: + """Clean up Rerun visualizer resources and finalize recordings.""" + if not self._is_initialized or self._viewer is None: + return + + try: + if self.cfg.record_to_rrd: + logger.info(f"[RerunVisualizer] Finalizing recording to: {self.cfg.record_to_rrd}") + self._viewer.close() + logger.info("[RerunVisualizer] Closed successfully.") + if self.cfg.record_to_rrd: + import os + + if os.path.exists(self.cfg.record_to_rrd): + size = os.path.getsize(self.cfg.record_to_rrd) + logger.info(f"[RerunVisualizer] Recording saved: {self.cfg.record_to_rrd} ({size} bytes)") + else: + logger.warning(f"[RerunVisualizer] Recording file not found: {self.cfg.record_to_rrd}") + except Exception as e: + logger.warning(f"[RerunVisualizer] Error during close: {e}") + + self._viewer = None + self._is_initialized = False + + def is_running(self) -> bool: + """Check if visualizer is running. + + Returns: + True if viewer is initialized and running, False otherwise. + """ + if self._viewer is None: + return False + return self._viewer.is_running() + + def is_training_paused(self) -> bool: + """Check if training is paused. + + Note: + Rerun visualizer currently uses Rerun's built-in timeline controls for playback. + """ + return False + + def supports_markers(self) -> bool: + """Rerun visualizer does not have this feature yet.""" + return False + + def supports_live_plots(self) -> bool: + """Rerun visualizer does not have this feature yet.""" + return False diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py new file mode 100644 index 00000000000..3dc4909e4db --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py @@ -0,0 +1,39 @@ +# 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 + +"""Configuration for the Rerun visualizer.""" + +from __future__ import annotations + +from isaaclab.utils import configclass + +from .visualizer_cfg import VisualizerCfg + + +@configclass +class RerunVisualizerCfg(VisualizerCfg): + """Configuration for Rerun visualizer (web-based visualization). + + Provides time scrubbing, 3D navigation, data filtering, and .rrd recording. + Requires Newton physics backend and rerun-sdk: `pip install rerun-sdk` + """ + + visualizer_type: str = "rerun" + """Type identifier for Rerun visualizer.""" + + app_id: str = "isaaclab-simulation" + """Application identifier shown in viewer title.""" + + web_port: int = 9090 + """Port of the local rerun web viewer which is launched in the browser.""" + + keep_historical_data: bool = False + """Keep transform history for time scrubbing (False = constant memory for training).""" + + keep_scalar_history: bool = False + """Keep scalar/plot history in timeline.""" + + record_to_rrd: str | None = None + """Path to save .rrd recording file. None = no recording.""" From 07c057369a30e2bbe20953d665a8f1584ba89bc9 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 07:28:31 -0800 Subject: [PATCH 125/130] refactor step 19: reorganized interface --- .../isaaclab/sim/interface/__init__.py | 9 ++ .../isaaclab/sim/{ => interface}/interface.py | 0 .../sim/{ => interface}/physics_interface.py | 0 .../{ => interface}/visualizer_interface.py | 0 .../isaaclab/sim/physx_ov_visualizer_cfg.py | 83 ------------------- .../isaaclab/sim/simulation_context.py | 4 +- 6 files changed, 11 insertions(+), 85 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/interface/__init__.py rename source/isaaclab/isaaclab/sim/{ => interface}/interface.py (100%) rename source/isaaclab/isaaclab/sim/{ => interface}/physics_interface.py (100%) rename source/isaaclab/isaaclab/sim/{ => interface}/visualizer_interface.py (100%) delete mode 100644 source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py diff --git a/source/isaaclab/isaaclab/sim/interface/__init__.py b/source/isaaclab/isaaclab/sim/interface/__init__.py new file mode 100644 index 00000000000..a681cdc219b --- /dev/null +++ b/source/isaaclab/isaaclab/sim/interface/__init__.py @@ -0,0 +1,9 @@ +from .interface import Interface +from .physics_interface import PhysicsInterface +from .visualizer_interface import VisualizerInterface + +__all__ = [ + "Interface", + "PhysicsInterface", + "VisualizerInterface", +] \ No newline at end of file diff --git a/source/isaaclab/isaaclab/sim/interface.py b/source/isaaclab/isaaclab/sim/interface/interface.py similarity index 100% rename from source/isaaclab/isaaclab/sim/interface.py rename to source/isaaclab/isaaclab/sim/interface/interface.py diff --git a/source/isaaclab/isaaclab/sim/physics_interface.py b/source/isaaclab/isaaclab/sim/interface/physics_interface.py similarity index 100% rename from source/isaaclab/isaaclab/sim/physics_interface.py rename to source/isaaclab/isaaclab/sim/interface/physics_interface.py diff --git a/source/isaaclab/isaaclab/sim/visualizer_interface.py b/source/isaaclab/isaaclab/sim/interface/visualizer_interface.py similarity index 100% rename from source/isaaclab/isaaclab/sim/visualizer_interface.py rename to source/isaaclab/isaaclab/sim/interface/visualizer_interface.py diff --git a/source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py b/source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py deleted file mode 100644 index 7243118e0e5..00000000000 --- a/source/isaaclab/isaaclab/sim/physx_ov_visualizer_cfg.py +++ /dev/null @@ -1,83 +0,0 @@ -# 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 - -"""Configuration for Omniverse visualizer in PhysX-based SimulationContext.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from isaaclab.utils import configclass -from isaaclab.visualizers import VisualizerCfg - -from .physx_ov_visualizer import RenderMode - -if TYPE_CHECKING: - from .physx_ov_visualizer import PhysxOVVisualizer - - -@configclass -class PhysxOVVisualizerCfg(VisualizerCfg): - """Configuration for Omniverse visualizer in PhysX-based SimulationContext. - - This configuration extends :class:`VisualizerCfg` and is used by the - :class:`PhysxOVVisualizer` class which manages viewport/rendering for - PhysX-based SimulationContext workflows. - """ - - visualizer_type: str = "omniverse" - """Type identifier for Omniverse visualizer.""" - - default_render_mode: RenderMode | None = None - """Default render mode for the visualizer. - - If None, the render mode will be automatically determined based on GUI availability: - - NO_GUI_OR_RENDERING: When no GUI and offscreen rendering is disabled - - PARTIAL_RENDERING: When no GUI but offscreen rendering is enabled - - FULL_RENDERING: When GUI is available (local, livestreamed, or XR) - - See :class:`RenderMode` for available options. - """ - - render_throttle_period: int = 5 - """Throttle period for rendering updates. - - This controls how frequently UI elements are updated when in NO_RENDERING mode. - A higher value means less frequent UI updates, improving performance. - """ - - camera_prim_path: str = "/OmniverseKit_Persp" - """Path to the camera primitive in the USD stage.""" - - warmup_renders: int = 2 - """Number of warmup renders to perform on hard reset. - - This is used to initialize replicator buffers before the simulation starts. - """ - - viewport_name: str | None = "Viewport" - """Viewport name to use. If None, uses active viewport.""" - - create_viewport: bool = False - """Create new viewport with specified name and camera pose.""" - - dock_position: str = "SAME" - """Dock position for new viewport. Options: 'LEFT', 'RIGHT', 'BOTTOM', 'SAME' (tabs with existing).""" - - window_width: int = 1280 - """Viewport width in pixels.""" - - window_height: int = 720 - """Viewport height in pixels.""" - - def create_visualizer(self) -> "PhysxOVVisualizer": - """Create PhysxOVVisualizer instance from this config. - - Returns: - PhysxOVVisualizer instance configured with this config. - """ - from .physx_ov_visualizer import PhysxOVVisualizer - - return PhysxOVVisualizer(self) diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 14da3be3731..f3a4cc9e5b2 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -21,10 +21,10 @@ from isaaclab.utils.logger import configure_logging from isaaclab.utils.version import get_isaac_sim_version -from .physics_interface import PhysicsInterface from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg -from .visualizer_interface import VisualizerInterface +from .interface.physics_interface import PhysicsInterface +from .interface.visualizer_interface import VisualizerInterface logger = logging.getLogger(__name__) From 81c91d129e7abdb4aa022e8bf8cab77f6de8b8f0 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 07:58:47 -0800 Subject: [PATCH 126/130] refactor step 20: gets simulation_context tests to pass --- .../isaaclab/physics/physx_manager.py | 2 +- .../sim/interface/visualizer_interface.py | 4 +- source/isaaclab/isaaclab/sim/utils/stage.py | 2 +- .../isaaclab/visualizers/newton_visualizer.py | 6 + .../visualizers/physx_ov_visualizer.py | 8 +- .../isaaclab/visualizers/rerun_visualizer.py | 6 + .../isaaclab/visualizers/visualizer.py | 5 + .../test/sim/test_simulation_context.py | 108 ++++++++++-------- 8 files changed, 87 insertions(+), 54 deletions(-) diff --git a/source/isaaclab/isaaclab/physics/physx_manager.py b/source/isaaclab/isaaclab/physics/physx_manager.py index 1432bcf3c31..cb056536a35 100644 --- a/source/isaaclab/isaaclab/physics/physx_manager.py +++ b/source/isaaclab/isaaclab/physics/physx_manager.py @@ -895,7 +895,7 @@ def _load_fabric_interface(cls) -> None: cls._fabric_iface = None # set carb settings for fabric - carb_settings.set_bool("/physics/fabricEnabled", cls._cfg.use_fabric) + carb_settings.set_bool("/isaaclab/physics/fabric_enabled", cls._cfg.use_fabric) carb_settings.set_bool("/physics/updateToUsd", not cls._cfg.use_fabric) carb_settings.set_bool("/physics/updateParticlesToUsd", not cls._cfg.use_fabric) carb_settings.set_bool("/physics/updateVelocitiesToUsd", not cls._cfg.use_fabric) diff --git a/source/isaaclab/isaaclab/sim/interface/visualizer_interface.py b/source/isaaclab/isaaclab/sim/interface/visualizer_interface.py index 2866174eb9d..3d051416d87 100644 --- a/source/isaaclab/isaaclab/sim/interface/visualizer_interface.py +++ b/source/isaaclab/isaaclab/sim/interface/visualizer_interface.py @@ -137,9 +137,9 @@ def is_playing(self) -> bool: return True # physics is always playing when there is no visualizer, basically headless mode def is_stopped(self) -> bool: - """Check whether the simulation is stopped.""" + """Check whether the simulation is stopped (not paused).""" for viz in self.visualizers: - return not viz.is_running() + return viz.is_stopped() return False def forward(self) -> None: diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index ce7fa6b9ca5..6c32fa1416d 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -471,7 +471,7 @@ def attach_stage_to_usd_context(attaching_early: bool = False): PhysxManager.enable_stage_open_callback(False) # enable physics fabric - SimulationContext.instance().carb_settings.set_bool("/physics/fabricEnabled", True) + SimulationContext.instance().carb_settings.set_bool("/isaaclab/physics/fabric_enabled", True) # attach stage to usd context omni.usd.get_context().attach_stage_with_callback(stage_id) diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py index be6b5e54d06..c689df4f7b7 100644 --- a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py @@ -372,6 +372,12 @@ def is_running(self) -> bool: return False return self._viewer.is_running() + def is_stopped(self) -> bool: + """Check if visualizer window is closed.""" + if not self._is_initialized or self._is_closed or self._viewer is None: + return True + return False + def supports_markers(self) -> bool: """Newton visualizer does not have this feature yet.""" return False diff --git a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py index 27e44b47aa6..73d6de3f81a 100644 --- a/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/physx_ov_visualizer.py @@ -447,11 +447,9 @@ def stop(self) -> None: def is_running(self) -> bool: """Check if visualizer is still running.""" - return self.is_playing() - - def is_training_paused(self) -> bool: - """Check if training is paused (always False for OV).""" - return False + if self._timeline is None: + return False + return self._timeline.is_playing() def supports_markers(self) -> bool: """Supports markers via USD prims.""" diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py index e21028a1937..fcad36a7ccd 100644 --- a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py @@ -268,6 +268,12 @@ def is_running(self) -> bool: return False return self._viewer.is_running() + def is_stopped(self) -> bool: + """Check if visualizer is stopped.""" + if self._viewer is None: + return True + return False + def is_training_paused(self) -> bool: """Check if training is paused. diff --git a/source/isaaclab/isaaclab/visualizers/visualizer.py b/source/isaaclab/isaaclab/visualizers/visualizer.py index 46c1c299dd0..69015b4fbb3 100644 --- a/source/isaaclab/isaaclab/visualizers/visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/visualizer.py @@ -51,6 +51,11 @@ def is_running(self) -> bool: """Check if visualizer is still running (e.g., window not closed).""" pass + @abstractmethod + def is_stopped(self) -> bool: + """Check if visualizer is stopped (e.g., window closed).""" + pass + def is_training_paused(self) -> bool: """Check if training is paused by visualizer controls.""" return False diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 420fff13138..b20c016e9ab 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -18,10 +18,10 @@ import weakref import omni.timeline -from isaaclab.sim import IsaacEvents, PhysxManager import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext +from isaaclab.physics import PhysxManager, PhysxManagerCfg, IsaacEvents @pytest.fixture(autouse=True) @@ -49,14 +49,20 @@ def test_setup_teardown(): @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) + from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg + + physics_manager_cfg = PhysxManagerCfg( + physics_prim_path="/Physics/PhysX", + gravity=(0.0, -0.5, -0.5), + device=device, + physics_material=RigidBodyMaterialCfg(), + ) + cfg = SimulationCfg(physics_manager_cfg=physics_manager_cfg, render_interval=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) - # verify app interface is valid - assert sim._visualizer.app is not None # verify stage is valid assert sim.stage is not None # verify device property @@ -64,14 +70,19 @@ def test_init(device): # verify no RTX sensors are available assert not sim.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") - # obtain physics scene api - physx_scene_api = sim._physx_scene_api # type: ignore - physics_scene = sim._physics_scene # type: ignore + # obtain physics scene from USD + from pxr import UsdPhysics + from pxr.PhysxSchema import PhysxSceneAPI + + physics_scene_prim = sim.stage.GetPrimAtPath("/Physics/PhysX") + assert physics_scene_prim.IsValid() + physics_scene = UsdPhysics.Scene(physics_scene_prim) + physx_scene_api = PhysxSceneAPI(physics_scene_prim) # check valid settings physics_hz = physx_scene_api.GetTimeStepsPerSecondAttr().Get() physics_dt = 1.0 / physics_hz - assert physics_dt == cfg.dt + assert physics_dt == cfg.physics_manager_cfg.dt # check valid paths assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() @@ -82,7 +93,7 @@ def test_init(device): 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.physics_manager_cfg.gravity) @pytest.mark.isaacsim_ci @@ -197,7 +208,7 @@ def test_timeline_pause(): @pytest.mark.isaacsim_ci def test_reset(): """Test simulation reset.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create a simple cube to test with @@ -211,13 +222,13 @@ def test_reset(): assert sim.is_playing() # check that physics sim view is created - assert sim._physics_backend.physics_sim_view is not None + assert sim._physics_interface.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) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create a simple cube @@ -238,7 +249,7 @@ def test_reset_soft(): @pytest.mark.isaacsim_ci def test_forward(): """Test forward propagation for fabric updates.""" - cfg = SimulationCfg(dt=0.01, use_fabric=True) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01, use_fabric=True)) sim = SimulationContext(cfg) # create simple scene @@ -258,7 +269,7 @@ def test_forward(): @pytest.mark.parametrize("render", [True, False]) def test_step(render): """Test stepping simulation with and without rendering.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -278,7 +289,7 @@ def test_step(render): @pytest.mark.isaacsim_ci def test_render(): """Test rendering simulation.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -335,7 +346,7 @@ def test_clear_stage(): 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_manager_cfg.physics_prim_path).IsValid() + assert sim.stage.GetPrimAtPath(sim.cfg.physics_manager_cfg.physics_prim_path).IsValid() # type: ignore[union-attr] """ @@ -347,13 +358,14 @@ def test_clear_stage(): @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 + from pxr.PhysxSchema import PhysxSceneAPI - cfg = SimulationCfg(physx=PhysxCfg(solver_type=solver_type)) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(solver_type=solver_type)) sim = SimulationContext(cfg) - # obtain physics scene api - physx_scene_api = sim._physx_scene_api # type: ignore + # obtain physics scene api from USD + physics_scene_prim = sim.stage.GetPrimAtPath(cfg.physics_manager_cfg.physics_prim_path) + physx_scene_api = PhysxSceneAPI(physics_scene_prim) # check solver type is set solver_type_str = "PGS" if solver_type == 0 else "TGS" assert physx_scene_api.GetSolverTypeAttr().Get() == solver_type_str @@ -363,22 +375,25 @@ def test_solver_type(solver_type): @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) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(use_fabric=use_fabric)) sim = SimulationContext(cfg) - # check fabric is enabled - assert sim.carb_settings.get_as_bool("/isaaclab/fabric_enabled") == use_fabric + # check fabric is enabled via physics setting + assert sim.carb_settings.get_as_bool("/isaaclab/physics/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) + from pxr.PhysxSchema import PhysxSceneAPI + + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=dt)) sim = SimulationContext(cfg) - # obtain physics scene api - physx_scene_api = sim._physx_scene_api # type: ignore + # obtain physics scene api from USD + physics_scene_prim = sim.stage.GetPrimAtPath(cfg.physics_manager_cfg.physics_prim_path) + physx_scene_api = PhysxSceneAPI(physics_scene_prim) # check physics dt physics_hz = physx_scene_api.GetTimeStepsPerSecondAttr().Get() physics_dt = 1.0 / physics_hz @@ -389,18 +404,21 @@ def test_physics_dt(dt): @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) + from pxr import UsdPhysics + + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(gravity=gravity)) sim = SimulationContext(cfg) - # obtain physics scene api - physics_scene = sim._physics_scene # type: ignore + # obtain physics scene from USD + physics_scene_prim = sim.stage.GetPrimAtPath(cfg.physics_manager_cfg.physics_prim_path) + physics_scene = UsdPhysics.Scene(physics_scene_prim) 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, decimal=6) + actual_gravity = np.array(gravity_dir) * gravity_mag + np.testing.assert_almost_equal(actual_gravity, cfg.physics_manager_cfg.gravity, decimal=6) """ @@ -411,7 +429,7 @@ def test_custom_gravity(gravity): @pytest.mark.isaacsim_ci def test_timeline_callbacks_on_play(): """Test that timeline callbacks are triggered on play event.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create a simple scene @@ -473,7 +491,7 @@ def on_stop_callback(event): def test_timeline_callbacks_with_weakref(): """Test that timeline callbacks work correctly with weak references (similar to asset_base.py).""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create a simple scene @@ -560,7 +578,7 @@ 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.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create tracking for multiple callbacks @@ -612,7 +630,7 @@ def callback3(event): @pytest.mark.isaacsim_ci def test_callback_execution_order(): """Test that callbacks are executed in the correct order based on priority.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # track execution order @@ -662,7 +680,7 @@ def callback_high_priority(event): @pytest.mark.isaacsim_ci def test_callback_unsubscribe(): """Test that unsubscribing callbacks works correctly.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create callback counter @@ -705,7 +723,7 @@ def on_play_callback(event): @pytest.mark.isaacsim_ci def test_pause_event_callback(): """Test that pause event callbacks are triggered correctly.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create callback tracker @@ -749,7 +767,7 @@ def on_pause_callback(event): ) def test_isaac_event_triggered_on_reset(event_type): """Test that Isaac events are triggered during reset.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -784,7 +802,7 @@ def on_event(event): @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) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -829,7 +847,7 @@ def on_prim_deletion(event): @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) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create callback tracker @@ -871,7 +889,7 @@ def on_timeline_stop(event): @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) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -945,7 +963,7 @@ def safe_callback(callback_name, event, obj_ref): @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) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -1000,7 +1018,7 @@ def callback3(event): def test_exception_in_callback_on_reset(): """Test that exceptions in callbacks during reset are properly captured and raised.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create simple scene @@ -1039,7 +1057,7 @@ def on_physics_ready_with_exception(event): def test_exception_in_callback_on_play(): """Test that exceptions in callbacks during play are properly captured and raised.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # create callback tracker @@ -1081,7 +1099,7 @@ def on_play_with_exception(event): def test_exception_in_callback_on_stop(): """Test that exceptions in callbacks during stop are properly captured and raised.""" - cfg = SimulationCfg(dt=0.01) + cfg = SimulationCfg(physics_manager_cfg=PhysxManagerCfg(dt=0.01)) sim = SimulationContext(cfg) # start the simulation first From 1538f48b436ff27687d543842b4776cafd3fcf17 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 08:22:03 -0800 Subject: [PATCH 127/130] made simulation manager a lot nicer --- .../isaaclab/physics/physx_manager.py | 357 ++++++++---------- 1 file changed, 159 insertions(+), 198 deletions(-) diff --git a/source/isaaclab/isaaclab/physics/physx_manager.py b/source/isaaclab/isaaclab/physics/physx_manager.py index cb056536a35..27856f052dd 100644 --- a/source/isaaclab/isaaclab/physics/physx_manager.py +++ b/source/isaaclab/isaaclab/physics/physx_manager.py @@ -19,7 +19,7 @@ from collections import OrderedDict from datetime import datetime from enum import Enum -from typing import TYPE_CHECKING, Any, Callable +from typing import TYPE_CHECKING, Any, Callable, ClassVar import carb import omni.kit @@ -159,6 +159,8 @@ def _finish(self) -> bool: # Invoke pvd interface to create recording stage_filename = "baked_animation_recording.usda" + if self._physx_pvd_interface is None: + return False result = self._physx_pvd_interface.ovd_to_usd_over_with_layer_creation( input_ovd_path, stage_path, @@ -217,47 +219,47 @@ class PhysxManager(PhysicsManager): Lifecycle: initialize() -> reset() -> step() (repeated) -> close() """ - # Core interfaces (names must match Isaac Sim's expected attributes) - _timeline = omni.timeline.get_timeline_interface() - _message_bus = carb.eventdispatcher.get_eventdispatcher() - _physx_interface = omni.physx.get_physx_interface() - _physx_sim_interface = omni.physx.get_physx_simulation_interface() - _carb_settings = carb.settings.get_settings() - - # State - _view: omni.physics.tensors.SimulationView | None = None - _view_warp: omni.physics.tensors.SimulationView | None = None - _backend: str = "torch" - _warmup_needed: bool = True - _view_created: bool = False - _assets_loaded: bool = True - - # Physics scenes (name must match Isaac Sim's expected attribute) - _physics_scene_apis: OrderedDict = OrderedDict() - - # Callbacks: id -> subscription - _callbacks: dict = {} - _callback_id: int = 0 - _handles: dict = {} # Named internal handles - - # Simulation context reference (for stage, carb_settings, logger access) - _sim: "SimulationContext | None" = None - - # Manager configuration (contains all physics settings) - _cfg: "PhysxManagerCfg | None" = None - - # Device and fabric state - _physics_device: str = "cpu" - _fabric_iface = None - _update_fabric = None - _anim_recorder: AnimationRecorder | None = None + # ==================== Omniverse Interfaces ==================== + _timeline: ClassVar[omni.timeline.ITimeline] = omni.timeline.get_timeline_interface() + _message_bus: ClassVar[carb.eventdispatcher.IEventDispatcher] = carb.eventdispatcher.get_eventdispatcher() + _physx_interface: ClassVar[omni.physx.IPhysx] = omni.physx.get_physx_interface() + _physx_sim_interface: ClassVar[omni.physx.IPhysxSimulation] = omni.physx.get_physx_simulation_interface() + _carb_settings: ClassVar[carb.settings.ISettings] = carb.settings.get_settings() + + # ==================== Simulation Views ==================== + _view: ClassVar[omni.physics.tensors.SimulationView | None] = None + _view_warp: ClassVar[omni.physics.tensors.SimulationView | None] = None + _backend: ClassVar[str] = "torch" + + # ==================== State Flags ==================== + _warmup_needed: ClassVar[bool] = True + _view_created: ClassVar[bool] = False + _assets_loaded: ClassVar[bool] = True + + # ==================== Physics Scenes ==================== + _physics_scene_apis: ClassVar[OrderedDict[str, PhysxSchema.PhysxSceneAPI]] = OrderedDict() + + # ==================== Callbacks ==================== + _callbacks: ClassVar[dict[int, Any]] = {} + _callback_id: ClassVar[int] = 0 + _handles: ClassVar[dict[str, Any]] = {} + + # ==================== Context & Configuration ==================== + _sim: ClassVar["SimulationContext | None"] = None + _cfg: ClassVar["PhysxManagerCfg | None"] = None + + # ==================== Device & Fabric ==================== + _physics_device: ClassVar[str] = "cpu" + _fabric_iface: ClassVar[Any] = None + _update_fabric: ClassVar[Callable[[float, float], None] | None] = None + _anim_recorder: ClassVar[AnimationRecorder | None] = None # Compatibility stub for Isaac Sim code that calls _simulation_manager_interface class _PhysxManagerInterfaceStub: - """Minimal stub for Isaac Sim compatibility.""" + """Minimal stub providing Isaac Sim compatibility interface.""" @staticmethod - def reset(): + def reset() -> None: pass @staticmethod @@ -274,24 +276,9 @@ def is_simulating() -> bool: except Exception: return False - # No-ops for unused methods - get_num_physics_steps = staticmethod(lambda: 0) - is_paused = staticmethod(lambda: False) - get_callback_iter = staticmethod(lambda: 0) - set_callback_iter = staticmethod(lambda v: None) - register_deletion_callback = staticmethod(lambda cb: None) - register_physics_scene_addition_callback = staticmethod(lambda cb: None) - deregister_callback = staticmethod(lambda id: False) - enable_usd_notice_handler = staticmethod(lambda f: None) - enable_fabric_usd_notice_handler = staticmethod(lambda s, f: None) - is_fabric_usd_notice_handler_enabled = staticmethod(lambda s: False) - get_sample_count = staticmethod(lambda: 0) - get_all_samples = staticmethod(lambda: []) - get_buffer_capacity = staticmethod(lambda: 1000) - get_current_time = staticmethod(lambda: carb.RationalTime(-1, 1)) - get_simulation_time_at_time = staticmethod(lambda t: 0.0) - get_sample_range = staticmethod(lambda: None) - log_statistics = staticmethod(lambda: None) + def __getattr__(self, name: str) -> Callable[..., Any]: + """Return no-op callable for any undefined method.""" + return lambda *args, **kwargs: None _simulation_manager_interface = _PhysxManagerInterfaceStub() @@ -337,7 +324,7 @@ def reset(cls, soft: bool = False) -> None: if not soft: # initialize the physics simulation if cls._view is None: - cls.initialize_physics() + cls._initialize_physics() # app.update() may be changing the cuda device in reset, so we force it back to our desired device here if "cuda" in cls._physics_device: @@ -380,7 +367,7 @@ def step(cls) -> None: def close(cls) -> None: """Clean up physics resources.""" # clear the simulation manager state (notifies assets to cleanup) - cls.clear() + cls._clear() # detach the stage from physx if cls._physx_sim_interface is not None: cls._physx_sim_interface.detach_stage() @@ -390,10 +377,10 @@ def close(cls) -> None: cls._anim_recorder = None @classmethod - def clear(cls) -> None: - """Clear all state and callbacks.""" + def _clear(cls) -> None: + """Clear all state and callbacks (internal use only).""" # Notify assets to clean up (PRIM_DELETION with "/" = clear all) - cls.dispatch_prim_deletion("/") + cls._message_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": "/"}) # Properly unsubscribe handles before clearing # Timeline subscriptions are auto-cleaned by omni.timeline # Message bus observers just need to be deleted @@ -416,8 +403,8 @@ def clear(cls) -> None: cls._update_fabric = None @classmethod - def initialize_physics(cls) -> None: - """Warm-start physics and create simulation views.""" + def _initialize_physics(cls) -> None: + """Warm-start physics and create simulation views (internal use only).""" if not cls._warmup_needed: return cls._physx_interface.force_load_physics_from_usd() @@ -433,18 +420,6 @@ def get_physics_sim_view(cls) -> omni.physics.tensors.SimulationView | None: """Get the physics simulation view.""" return cls._view - @classmethod - def get_backend(cls) -> str: - """Get the tensor backend ("torch" or "warp").""" - return cls._backend - - @classmethod - def set_backend(cls, backend: str) -> None: - """Set the tensor backend.""" - if backend not in ("torch", "warp"): - raise ValueError(f"Backend must be 'torch' or 'warp', got '{backend}'") - cls._backend = backend - @classmethod def get_physics_dt(cls) -> float: """Get the physics timestep in seconds.""" @@ -460,13 +435,13 @@ def get_physics_dt(cls) -> float: @classmethod def get_device(cls) -> str: - """Get the physics simulation device.""" + """Get the physics simulation device (e.g., 'cuda:0' or 'cpu').""" return cls._physics_device @classmethod - def get_physics_sim_device(cls) -> str: - """Get the physics simulation device (alias for get_device).""" - return cls._physics_device + def _is_gpu_device(cls) -> bool: + """Check if configured for GPU physics.""" + return cls._cfg is not None and "cuda" in cls._cfg.device @classmethod def is_fabric_enabled(cls) -> bool: @@ -474,8 +449,8 @@ def is_fabric_enabled(cls) -> bool: return cls._fabric_iface is not None @classmethod - def set_physics_sim_device(cls, device: str) -> None: - """Set the physics simulation device.""" + def _set_physics_sim_device(cls, device: str) -> None: + """Set the physics simulation device (internal use only).""" if "cuda" in device: parts = device.split(":") device_id = int(parts[1]) if len(parts) > 1 else max(0, cls._carb_settings.get_as_int("/physics/cudaDevice")) @@ -494,11 +469,6 @@ def assets_loading(cls) -> bool: """Check if assets are currently loading.""" return not cls._assets_loaded - @classmethod - def dispatch_prim_deletion(cls, prim_path: str) -> None: - """Dispatch prim deletion event.""" - cls._message_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": prim_path}) - # ------------------------------------------------------------------ # Callback Registration # ------------------------------------------------------------------ @@ -523,7 +493,9 @@ def register_callback(cls, callback: Callable, event: IsaacEvents, order: int = if hasattr(callback, "__self__"): obj_ref = weakref.proxy(callback.__self__) method_name = callback.__name__ - cb = lambda e, o=obj_ref, m=method_name: getattr(o, m)(e) + + def cb(e: Any, o: Any = obj_ref, m: str = method_name) -> Any: + return getattr(o, m)(e) else: cb = callback @@ -590,7 +562,7 @@ def _setup_callbacks(cls) -> None: def _on_play(cls, event: Any) -> None: """Handle timeline play.""" if cls._carb_settings.get_as_bool("/app/player/playSimulations"): - cls.initialize_physics() + cls._initialize_physics() @classmethod def _on_stop(cls, event: Any) -> None: @@ -636,9 +608,11 @@ def _create_views(cls) -> None: stage_id = get_current_stage_id() cls._view = omni.physics.tensors.create_simulation_view(cls._backend, stage_id=stage_id) - cls._view.set_subspace_roots("/") cls._view_warp = omni.physics.tensors.create_simulation_view("warp", stage_id=stage_id) - cls._view_warp.set_subspace_roots("/") + if cls._view is not None: + cls._view.set_subspace_roots("/") + if cls._view_warp is not None: + cls._view_warp.set_subspace_roots("/") cls._physx_interface.update_simulation(cls.get_physics_dt(), 0.0) cls._view_created = True @@ -654,16 +628,6 @@ def _track_physics_scenes(cls) -> None: if prim.GetTypeName() == "PhysicsScene": cls._physics_scene_apis[prim.GetPath().pathString] = PhysxSchema.PhysxSceneAPI.Apply(prim) - @classmethod - def _is_gpu_enabled(cls) -> bool: - """Check if GPU dynamics is enabled.""" - if cls._physics_scene_apis: - api = list(cls._physics_scene_apis.values())[0] - bp = api.GetBroadphaseTypeAttr().Get() - gpu_dyn = api.GetEnableGPUDynamicsAttr().Get() - return bp == "GPU" and gpu_dyn - return False - @classmethod def _set_gpu_dynamics(cls, enable: bool) -> None: """Enable/disable GPU dynamics on all physics scenes.""" @@ -725,138 +689,135 @@ def _configure_simulation_dt(cls) -> None: @classmethod def _apply_physics_settings(cls) -> None: - """Sets various carb physics settings.""" + """Apply all physics configuration settings.""" if cls._sim is None or cls._cfg is None: return - carb_settings = cls._sim.carb_settings + cls._apply_carb_settings() + cls._apply_device_settings() + cls._create_default_material() + cls._apply_physx_scene_settings() + cls._log_config_warnings() - # Get physics scene API from the physics interface - stage = cls._sim.stage - physics_scene_prim = stage.GetPrimAtPath(cls._cfg.physics_prim_path) - physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) + @classmethod + def _apply_carb_settings(cls) -> None: + """Apply Carbonite physics settings.""" + if cls._sim is None: + return + settings = cls._sim.carb_settings + + # Enable hydra scene-graph instancing for GUI rendering + settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) + # Use PhysX SDK dispatcher instead of carb tasking + settings.set_bool("/physics/physxDispatcher", True) + # Disable contact processing by default (enabled when contact sensors are created) + settings.set_bool("/physics/disableContactProcessing", True) + # Enable contact reporting for cylinder/cone shapes (not natively supported by PhysX) + settings.set_bool("/physics/collisionConeCustomGeometry", False) + settings.set_bool("/physics/collisionCylinderCustomGeometry", False) + # Hide simulation output window + settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) + + @classmethod + def _apply_device_settings(cls) -> None: + """Configure GPU/CPU device settings.""" + if cls._sim is None or cls._cfg is None: + return + settings = cls._sim.carb_settings - # -------------------------- - # Carb Physics API settings - # -------------------------- - - # enable hydra scene-graph instancing - # note: this allows rendering of instanceable assets on the GUI - carb_settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) - # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking - # note: dispatcher handles how threads are launched for multi-threaded physics - carb_settings.set_bool("/physics/physxDispatcher", True) - # disable contact processing in omni.physx - # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. - # The physics flag gets enabled when a contact sensor is created. - # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts - # are always processed. The issue is reported to the PhysX team by @mmittal. - carb_settings.set_bool("/physics/disableContactProcessing", True) - # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them - # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags - # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry - carb_settings.set_bool("/physics/collisionConeCustomGeometry", False) - carb_settings.set_bool("/physics/collisionCylinderCustomGeometry", False) - # hide the Simulation Settings window - carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) - - # handle device settings - if "cuda" in cls._cfg.device: - parsed_device = cls._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 = carb_settings.get_as_int("/physics/cudaDevice") - if device_id < 0: - carb_settings.set_int("/physics/cudaDevice", 0) - device_id = 0 + if cls._is_gpu_device(): + device_parts = cls._cfg.device.split(":") + if len(device_parts) == 1: + # "cuda" without ID - use carb setting or default to 0 + device_id = max(0, settings.get_as_int("/physics/cudaDevice")) else: - # if users specified "cuda:N", we use the provided device id - device_id = int(parsed_device[1]) - carb_settings.set_int("/physics/cudaDevice", device_id) - # suppress readback for GPU physics - carb_settings.set_bool("/physics/suppressReadback", True) - # save the device + device_id = int(device_parts[1]) + + settings.set_int("/physics/cudaDevice", device_id) + settings.set_bool("/physics/suppressReadback", True) cls._physics_device = f"cuda:{device_id}" else: - # enable USD read/write operations for CPU physics - carb_settings.set_int("/physics/cudaDevice", -1) - carb_settings.set_bool("/physics/suppressReadback", False) - # save the device + settings.set_int("/physics/cudaDevice", -1) + settings.set_bool("/physics/suppressReadback", False) cls._physics_device = "cpu" - # Configure simulation manager backend - # Isaac Lab always uses torch tensors for consistency, even on CPU - cls.set_backend("torch") - cls.set_physics_sim_device(cls._physics_device) - - # -------------------------- - # USDPhysics API settings - # -------------------------- - - # create the default physics material - # this material is used when no material is specified for a primitive - if cls._cfg.physics_material is not None: - material_path = f"{cls._cfg.physics_prim_path}/defaultMaterial" - cls._cfg.physics_material.func(material_path, cls._cfg.physics_material) - # bind the physics material to the scene - sim_utils.bind_physics_material(cls._cfg.physics_prim_path, material_path) - - # -------------------------- - # PhysX API settings - # -------------------------- - - # set broadphase type - broadphase_type = "GPU" if "cuda" in cls._cfg.device else "MBP" - physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) - # set gpu dynamics - enable_gpu_dynamics = "cuda" in cls._cfg.device - 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 cls._cfg.enable_ccd: - cls._cfg.enable_ccd = False + cls._backend = "torch" + cls._set_physics_sim_device(cls._physics_device) + + @classmethod + def _create_default_material(cls) -> None: + """Create and bind default physics material if configured.""" + if cls._cfg is None or cls._cfg.physics_material is None: + return + + material_path = f"{cls._cfg.physics_prim_path}/defaultMaterial" + cls._cfg.physics_material.func(material_path, cls._cfg.physics_material) + sim_utils.bind_physics_material(cls._cfg.physics_prim_path, material_path) + + @classmethod + def _apply_physx_scene_settings(cls) -> None: + """Apply PhysX scene API settings.""" + if cls._sim is None or cls._cfg is None: + return + + stage = cls._sim.stage + physics_scene_prim = stage.GetPrimAtPath(cls._cfg.physics_prim_path) + physx_scene_api = PhysxSchema.PhysxSceneAPI(physics_scene_prim) + + is_gpu = cls._is_gpu_device() + + # Broadphase and GPU dynamics + physx_scene_api.CreateBroadphaseTypeAttr("GPU" if is_gpu else "MBP") + physx_scene_api.CreateEnableGPUDynamicsAttr(is_gpu) + + # CCD (disabled for GPU dynamics) + enable_ccd = cls._cfg.enable_ccd + if is_gpu and enable_ccd: + enable_ccd = False cls._sim.logger.warning( - "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxManagerCfg to avoid this" - " warning." + "CCD is disabled when GPU dynamics is enabled. Disable CCD in PhysxManagerCfg to suppress this warning." ) - physx_scene_api.CreateEnableCCDAttr(cls._cfg.enable_ccd) + physx_scene_api.CreateEnableCCDAttr(enable_ccd) - # set solver type + # Solver type solver_type = "PGS" if cls._cfg.solver_type == 0 else "TGS" physx_scene_api.CreateSolverTypeAttr(solver_type) - # set solve articulation contact last + # Articulation contact ordering attr = physx_scene_api.GetPrim().CreateAttribute( "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool ) attr.Set(cls._cfg.solve_articulation_contact_last) - # iterate over all the settings and set them + # Apply remaining settings from config + skip_keys = { + "solver_type", "enable_ccd", "solve_articulation_contact_last", + "dt", "device", "render_interval", "gravity", + "physics_prim_path", "use_fabric", "physics_material" + } for key, value in cls._cfg.to_dict().items(): # type: ignore - # Skip non-PhysX settings and already-handled settings - if key in ["solver_type", "enable_ccd", "solve_articulation_contact_last", - "dt", "device", "render_interval", "gravity", - "physics_prim_path", "use_fabric", "physics_material"]: + if key in skip_keys: continue if key == "bounce_threshold_velocity": key = "bounce_threshold" sim_utils.safe_set_attribute_on_usd_schema(physx_scene_api, key, value, camel_case=True) - # throw warnings for helpful guidance + @classmethod + def _log_config_warnings(cls) -> None: + """Log helpful configuration warnings.""" + if cls._cfg is None or cls._sim is None: + return + if cls._cfg.solver_type == 1 and not cls._cfg.enable_external_forces_every_iteration: logger.warning( - "The `enable_external_forces_every_iteration` parameter in PhysxManagerCfg 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." + "enable_external_forces_every_iteration is False with TGS solver. " + "If experiencing noisy velocities, enable this flag and increase velocity iterations to 1-2." ) if not cls._cfg.enable_stabilization and cls._cfg.dt > 0.0333: cls._sim.logger.warning( - "Large simulation step size (> 0.0333 seconds) is not recommended without enabling stabilization." - " Consider setting the `enable_stabilization` flag to True in PhysxManagerCfg, or reducing the" - " simulation step size if you run into physics issues." + "Large timestep (>0.0333s) without stabilization may cause physics issues. " + "Consider enabling enable_stabilization or reducing dt." ) @classmethod From aaa3d8ba5afc2fc53e5fccb7eb505ddd9b2e5882 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 09:59:23 -0800 Subject: [PATCH 128/130] synchronized newton visualizer --- .../isaaclab/visualizers/ov_visualizer.py | 4 +--- .../isaaclab/isaaclab/visualizers/visualizer.py | 16 ++++++++++++++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py index 0a3109e219c..234e2aee88f 100644 --- a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py @@ -57,7 +57,7 @@ class RenderMode(enum.IntEnum): class OVVisualizer(Visualizer): - """Omniverse visualizer managing viewport/rendering for PhysX workflow. + """Omniverse visualizer managing viewport/rendering. This class extends the base :class:`Visualizer` and handles: - Viewport context and window management @@ -65,7 +65,6 @@ class OVVisualizer(Visualizer): - Camera view setup - Render settings from configuration - Throttled rendering for UI responsiveness - - Timeline control (play/pause/stop) Lifecycle: __init__(cfg) -> initialize(scene_data) -> step() (repeated) -> close() """ @@ -83,7 +82,6 @@ def __init__(self, cfg: "OVVisualizerCfg"): self._simulation_context = None self._simulation_app = None self._simulation_app_running = False - self._timeline = None # Viewport state self._viewport_context = None diff --git a/source/isaaclab/isaaclab/visualizers/visualizer.py b/source/isaaclab/isaaclab/visualizers/visualizer.py index 69015b4fbb3..79d3bf3606d 100644 --- a/source/isaaclab/isaaclab/visualizers/visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/visualizer.py @@ -89,3 +89,19 @@ def get_rendering_dt(self) -> float | None: def set_camera_view(self, eye: tuple, target: tuple) -> None: """Set camera view position. No-op by default.""" pass + + def reset(self, soft: bool = False) -> None: + """Reset visualizer state. No-op by default.""" + pass + + def play(self) -> None: + """Handle simulation play/start. No-op by default.""" + pass + + def pause(self) -> None: + """Handle simulation pause. No-op by default.""" + pass + + def stop(self) -> None: + """Handle simulation stop. No-op by default.""" + pass From 8a115900c63425fff1f2437363291406bb0e8278 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 10:52:13 -0800 Subject: [PATCH 129/130] refactore step 21: ensure simulation context consistency --- scripts/benchmarks/benchmark_cameras.py | 2 +- scripts/benchmarks/benchmark_load_robot.py | 2 +- .../benchmarks/benchmark_view_comparison.py | 2 - .../benchmarks/benchmark_xform_prim_view.py | 1 - scripts/demos/arms.py | 2 +- scripts/demos/bin_packing.py | 2 +- scripts/demos/bipeds.py | 2 +- scripts/demos/deformables.py | 2 +- scripts/demos/hands.py | 2 +- scripts/demos/haply_teleoperation.py | 2 +- scripts/demos/markers.py | 2 +- scripts/demos/multi_asset.py | 2 +- scripts/demos/procedural_terrain.py | 2 +- scripts/demos/quadcopter.py | 2 +- scripts/demos/quadrupeds.py | 2 +- scripts/demos/sensors/cameras.py | 2 +- scripts/demos/sensors/contact_sensor.py | 2 +- .../demos/sensors/frame_transformer_sensor.py | 2 +- scripts/demos/sensors/imu_sensor.py | 2 +- scripts/demos/sensors/multi_mesh_raycaster.py | 2 +- .../sensors/multi_mesh_raycaster_camera.py | 2 +- scripts/demos/sensors/raycaster_sensor.py | 2 +- scripts/tutorials/00_sim/create_empty.py | 2 +- scripts/tutorials/00_sim/launch_app.py | 2 +- scripts/tutorials/00_sim/log_time.py | 2 +- .../tutorials/00_sim/set_rendering_mode.py | 2 +- scripts/tutorials/00_sim/spawn_prims.py | 2 +- scripts/tutorials/01_assets/add_new_robot.py | 2 +- .../tutorials/01_assets/run_articulation.py | 2 +- .../01_assets/run_deformable_object.py | 2 +- .../tutorials/01_assets/run_rigid_object.py | 2 +- .../01_assets/run_surface_gripper.py | 2 +- scripts/tutorials/02_scene/create_scene.py | 2 +- .../04_sensors/add_sensors_on_robot.py | 2 +- .../04_sensors/run_frame_transformer.py | 2 +- .../tutorials/04_sensors/run_ray_caster.py | 2 +- .../04_sensors/run_ray_caster_camera.py | 2 +- .../tutorials/04_sensors/run_usd_camera.py | 2 +- .../tutorials/05_controllers/run_diff_ik.py | 2 +- scripts/tutorials/05_controllers/run_osc.py | 2 +- .../isaaclab/isaaclab/envs/direct_marl_env.py | 1 - .../isaaclab/isaaclab/envs/direct_rl_env.py | 1 - .../isaaclab/envs/manager_based_env.py | 1 - .../isaaclab/envs/ui/base_env_window.py | 4 +- .../isaaclab/isaaclab/envs/ui/empty_window.py | 2 +- .../envs/ui/viewport_camera_controller.py | 2 +- .../isaaclab/sim/simulation_context.py | 536 ++++++------------ .../isaaclab/isaaclab/sim/utils/__init__.py | 14 + .../test/assets/check_external_force.py | 2 +- .../test/assets/check_fixed_base_assets.py | 2 +- .../test/assets/check_ridgeback_franka.py | 2 +- .../test/controllers/test_differential_ik.py | 1 - .../controllers/test_operational_space.py | 1 - .../isaaclab/test/controllers/test_pink_ik.py | 2 +- .../test/deps/isaacsim/check_ref_count.py | 2 +- .../isaacsim/check_rep_texture_randomizer.py | 2 +- .../test/markers/check_markers_visibility.py | 2 +- .../test/scene/check_interactive_scene.py | 2 +- .../test/scene/test_interactive_scene.py | 1 - .../test/sensors/check_contact_sensor.py | 2 +- .../sensors/check_multi_mesh_ray_caster.py | 2 +- .../isaaclab/test/sensors/check_ray_caster.py | 2 +- .../test/sensors/test_frame_transformer.py | 2 +- source/isaaclab/test/sim/check_meshes.py | 2 +- .../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 | 2 +- .../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 | 2 +- .../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 - .../test/terrains/check_terrain_importer.py | 2 +- .../test/terrains/test_terrain_importer.py | 2 +- .../check_scene_xr_visualization.py | 2 +- .../envs/g1_locomanipulation_sdg_env.py | 2 +- 81 files changed, 244 insertions(+), 448 deletions(-) diff --git a/scripts/benchmarks/benchmark_cameras.py b/scripts/benchmarks/benchmark_cameras.py index b31f6655c0e..e87a8f74e7b 100644 --- a/scripts/benchmarks/benchmark_cameras.py +++ b/scripts/benchmarks/benchmark_cameras.py @@ -754,7 +754,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) scene_entities = design_scene( num_tiled_cams=args_cli.num_tiled_cameras, num_standard_cams=args_cli.num_standard_cameras, diff --git a/scripts/benchmarks/benchmark_load_robot.py b/scripts/benchmarks/benchmark_load_robot.py index 45d066162c8..8555bfda0b4 100644 --- a/scripts/benchmarks/benchmark_load_robot.py +++ b/scripts/benchmarks/benchmark_load_robot.py @@ -146,7 +146,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device="cuda:0") sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + sim._visualizer_interface.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) # Start the timer for creating the scene setup_time_begin = time.perf_counter_ns() diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 6db3b540190..fbfa310f088 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -180,8 +180,6 @@ def benchmark_view(view_type: str, num_iterations: int) -> tuple[dict[str, float computed_results["world_orientations_after_set"] = orientations_after_set.clone() # close simulation - sim.clear() - sim.clear_all_callbacks() sim.clear_instance() return timing_results, computed_results diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 5c34474e39d..596aed8e106 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -215,7 +215,6 @@ def benchmark_xform_prim_view( timing_results["get_both"] = (time.perf_counter() - start_time) / num_iterations # close simulation - sim.clear() sim.clear_instance() return timing_results, computed_results diff --git a/scripts/demos/arms.py b/scripts/demos/arms.py index 92bd4499d6d..81e1f323032 100644 --- a/scripts/demos/arms.py +++ b/scripts/demos/arms.py @@ -212,7 +212,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([3.5, 0.0, 3.2], [0.0, 0.0, 0.5]) + sim._visualizer_interface.set_camera_view([3.5, 0.0, 3.2], [0.0, 0.0, 0.5]) # design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/demos/bin_packing.py b/scripts/demos/bin_packing.py index 192026c81d9..fcb100ceec7 100644 --- a/scripts/demos/bin_packing.py +++ b/scripts/demos/bin_packing.py @@ -331,7 +331,7 @@ def main() -> None: sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view((2.5, 0.0, 4.0), (0.0, 0.0, 2.0)) + sim._visualizer_interface.set_camera_view((2.5, 0.0, 4.0), (0.0, 0.0, 2.0)) # Design scene scene_cfg = MultiObjectSceneCfg(num_envs=args_cli.num_envs, env_spacing=1.0, replicate_physics=False) diff --git a/scripts/demos/bipeds.py b/scripts/demos/bipeds.py index 1da3677cb5b..6ed349bd527 100644 --- a/scripts/demos/bipeds.py +++ b/scripts/demos/bipeds.py @@ -115,7 +115,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.0, 0.0, 2.25], target=[0.0, 0.0, 1.0]) + sim._visualizer_interface.set_camera_view(eye=[3.0, 0.0, 2.25], target=[0.0, 0.0, 1.0]) # design scene robots, origins = design_scene(sim) diff --git a/scripts/demos/deformables.py b/scripts/demos/deformables.py index bebaa51a4e8..8e3e589fe9a 100644 --- a/scripts/demos/deformables.py +++ b/scripts/demos/deformables.py @@ -180,7 +180,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([4.0, 4.0, 3.0], [0.5, 0.5, 0.0]) + sim._visualizer_interface.set_camera_view([4.0, 4.0, 3.0], [0.5, 0.5, 0.0]) # Design scene by adding assets to it scene_entities, scene_origins = design_scene() diff --git a/scripts/demos/hands.py b/scripts/demos/hands.py index a0fa04e0fbf..c9f8f3bf513 100644 --- a/scripts/demos/hands.py +++ b/scripts/demos/hands.py @@ -146,7 +146,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[0.0, -0.5, 1.5], target=[0.0, -0.2, 0.5]) + sim._visualizer_interface.set_camera_view(eye=[0.0, -0.5, 1.5], target=[0.0, -0.2, 0.5]) # design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/demos/haply_teleoperation.py b/scripts/demos/haply_teleoperation.py index b6d02900baf..d3d9846e1ae 100644 --- a/scripts/demos/haply_teleoperation.py +++ b/scripts/demos/haply_teleoperation.py @@ -336,7 +336,7 @@ def main(): sim = sim_utils.SimulationContext(sim_cfg) # set the simulation view - sim.set_camera_view([1.6, 1.0, 1.70], [0.4, 0.0, 1.0]) + sim._visualizer_interface.set_camera_view([1.6, 1.0, 1.70], [0.4, 0.0, 1.0]) scene_cfg = FrankaHaplySceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/markers.py b/scripts/demos/markers.py index 6152dcf5226..042ceed5bdd 100644 --- a/scripts/demos/markers.py +++ b/scripts/demos/markers.py @@ -97,7 +97,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([0.0, 18.0, 12.0], [0.0, 3.0, 0.0]) + sim._visualizer_interface.set_camera_view([0.0, 18.0, 12.0], [0.0, 3.0, 0.0]) # Spawn things into stage # Lights diff --git a/scripts/demos/multi_asset.py b/scripts/demos/multi_asset.py index d104eb161d3..a5aab0488e9 100644 --- a/scripts/demos/multi_asset.py +++ b/scripts/demos/multi_asset.py @@ -278,7 +278,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + sim._visualizer_interface.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) # Design scene scene_cfg = MultiObjectSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0, replicate_physics=False) diff --git a/scripts/demos/procedural_terrain.py b/scripts/demos/procedural_terrain.py index e409ee4f091..0fe30d7cc28 100644 --- a/scripts/demos/procedural_terrain.py +++ b/scripts/demos/procedural_terrain.py @@ -156,7 +156,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[5.0, 5.0, 5.0], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[5.0, 5.0, 5.0], target=[0.0, 0.0, 0.0]) # design scene scene_entities, scene_origins = design_scene() # Play the simulator diff --git a/scripts/demos/quadcopter.py b/scripts/demos/quadcopter.py index 52d8eefc140..a7f4852a495 100644 --- a/scripts/demos/quadcopter.py +++ b/scripts/demos/quadcopter.py @@ -50,7 +50,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[0.5, 0.5, 1.0], target=[0.0, 0.0, 0.5]) + sim._visualizer_interface.set_camera_view(eye=[0.5, 0.5, 1.0], target=[0.0, 0.0, 0.5]) # Spawn things into stage # Ground-plane diff --git a/scripts/demos/quadrupeds.py b/scripts/demos/quadrupeds.py index b9935de30da..a19070713ce 100644 --- a/scripts/demos/quadrupeds.py +++ b/scripts/demos/quadrupeds.py @@ -172,7 +172,7 @@ def main(): # Initialize the simulation context sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01)) # Set main camera - sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) # design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/demos/sensors/cameras.py b/scripts/demos/sensors/cameras.py index 5c1effcc214..e5db9fadc20 100644 --- a/scripts/demos/sensors/cameras.py +++ b/scripts/demos/sensors/cameras.py @@ -282,7 +282,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device, use_fabric=not args_cli.disable_fabric) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = SensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/sensors/contact_sensor.py b/scripts/demos/sensors/contact_sensor.py index 547d7b2b1b0..6110caa9f95 100644 --- a/scripts/demos/sensors/contact_sensor.py +++ b/scripts/demos/sensors/contact_sensor.py @@ -158,7 +158,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = ContactSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/sensors/frame_transformer_sensor.py b/scripts/demos/sensors/frame_transformer_sensor.py index c20bc23afdb..7a3d8857c21 100644 --- a/scripts/demos/sensors/frame_transformer_sensor.py +++ b/scripts/demos/sensors/frame_transformer_sensor.py @@ -152,7 +152,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = FrameTransformerSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/sensors/imu_sensor.py b/scripts/demos/sensors/imu_sensor.py index e6a922c8455..ccd3e542212 100644 --- a/scripts/demos/sensors/imu_sensor.py +++ b/scripts/demos/sensors/imu_sensor.py @@ -125,7 +125,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = ImuSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/sensors/multi_mesh_raycaster.py b/scripts/demos/sensors/multi_mesh_raycaster.py index 8868ad20861..7af4bd3e7b8 100644 --- a/scripts/demos/sensors/multi_mesh_raycaster.py +++ b/scripts/demos/sensors/multi_mesh_raycaster.py @@ -280,7 +280,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = RaycasterSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0, replicate_physics=False) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/sensors/multi_mesh_raycaster_camera.py b/scripts/demos/sensors/multi_mesh_raycaster_camera.py index 1f676f3f0b0..475ca228583 100644 --- a/scripts/demos/sensors/multi_mesh_raycaster_camera.py +++ b/scripts/demos/sensors/multi_mesh_raycaster_camera.py @@ -306,7 +306,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = RaycasterSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0, replicate_physics=False) scene = InteractiveScene(scene_cfg) diff --git a/scripts/demos/sensors/raycaster_sensor.py b/scripts/demos/sensors/raycaster_sensor.py index 302fff8ddf5..bf6ad65380c 100644 --- a/scripts/demos/sensors/raycaster_sensor.py +++ b/scripts/demos/sensors/raycaster_sensor.py @@ -143,7 +143,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = RaycasterSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/tutorials/00_sim/create_empty.py b/scripts/tutorials/00_sim/create_empty.py index 6fa283a68f1..51506800c12 100644 --- a/scripts/tutorials/00_sim/create_empty.py +++ b/scripts/tutorials/00_sim/create_empty.py @@ -41,7 +41,7 @@ def main(): sim_cfg = SimulationCfg(dt=0.01) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) # Play the simulator sim.reset() diff --git a/scripts/tutorials/00_sim/launch_app.py b/scripts/tutorials/00_sim/launch_app.py index 1622d3ba956..9e2f77b9e68 100644 --- a/scripts/tutorials/00_sim/launch_app.py +++ b/scripts/tutorials/00_sim/launch_app.py @@ -73,7 +73,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + sim._visualizer_interface.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) # Design scene by adding assets to it design_scene() diff --git a/scripts/tutorials/00_sim/log_time.py b/scripts/tutorials/00_sim/log_time.py index 2e4445c3d47..4370107bba9 100644 --- a/scripts/tutorials/00_sim/log_time.py +++ b/scripts/tutorials/00_sim/log_time.py @@ -56,7 +56,7 @@ def main(): sim_cfg = SimulationCfg(dt=0.01) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) # Play the simulator sim.reset() diff --git a/scripts/tutorials/00_sim/set_rendering_mode.py b/scripts/tutorials/00_sim/set_rendering_mode.py index 220b5b765be..00244b893a8 100644 --- a/scripts/tutorials/00_sim/set_rendering_mode.py +++ b/scripts/tutorials/00_sim/set_rendering_mode.py @@ -58,7 +58,7 @@ def main(): sim = sim_utils.SimulationContext(sim_cfg) # Pose camera in the hospital lobby area - sim.set_camera_view([-11, -0.5, 2], [0, 0, 0.5]) + sim._visualizer_interface.set_camera_view([-11, -0.5, 2], [0, 0, 0.5]) # Load hospital scene hospital_usd_path = f"{ISAAC_NUCLEUS_DIR}/Environments/Hospital/hospital.usd" diff --git a/scripts/tutorials/00_sim/spawn_prims.py b/scripts/tutorials/00_sim/spawn_prims.py index 7c120bd308d..73b224a59c0 100644 --- a/scripts/tutorials/00_sim/spawn_prims.py +++ b/scripts/tutorials/00_sim/spawn_prims.py @@ -93,7 +93,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + sim._visualizer_interface.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) # Design scene design_scene() # Play the simulator diff --git a/scripts/tutorials/01_assets/add_new_robot.py b/scripts/tutorials/01_assets/add_new_robot.py index bc322d10947..fa1ccfda96b 100644 --- a/scripts/tutorials/01_assets/add_new_robot.py +++ b/scripts/tutorials/01_assets/add_new_robot.py @@ -162,7 +162,7 @@ def main(): # Initialize the simulation context sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) - sim.set_camera_view([3.5, 0.0, 3.2], [0.0, 0.0, 0.5]) + sim._visualizer_interface.set_camera_view([3.5, 0.0, 3.2], [0.0, 0.0, 0.5]) # Design scene scene_cfg = NewRobotsSceneCfg(args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/tutorials/01_assets/run_articulation.py b/scripts/tutorials/01_assets/run_articulation.py index bc4254cbae1..d5ea801e536 100644 --- a/scripts/tutorials/01_assets/run_articulation.py +++ b/scripts/tutorials/01_assets/run_articulation.py @@ -122,7 +122,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + sim._visualizer_interface.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) # Design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/tutorials/01_assets/run_deformable_object.py b/scripts/tutorials/01_assets/run_deformable_object.py index 3623bb3d8a1..2eaac68b89a 100644 --- a/scripts/tutorials/01_assets/run_deformable_object.py +++ b/scripts/tutorials/01_assets/run_deformable_object.py @@ -147,7 +147,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.0, 0.0, 1.0], target=[0.0, 0.0, 0.5]) + sim._visualizer_interface.set_camera_view(eye=[3.0, 0.0, 1.0], target=[0.0, 0.0, 0.5]) # Design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/tutorials/01_assets/run_rigid_object.py b/scripts/tutorials/01_assets/run_rigid_object.py index dc1a88c57eb..fc10a595b7b 100644 --- a/scripts/tutorials/01_assets/run_rigid_object.py +++ b/scripts/tutorials/01_assets/run_rigid_object.py @@ -127,7 +127,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[1.5, 0.0, 1.0], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[1.5, 0.0, 1.0], target=[0.0, 0.0, 0.0]) # Design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/tutorials/01_assets/run_surface_gripper.py b/scripts/tutorials/01_assets/run_surface_gripper.py index 066dd9a077d..6a2c2d0970e 100644 --- a/scripts/tutorials/01_assets/run_surface_gripper.py +++ b/scripts/tutorials/01_assets/run_surface_gripper.py @@ -162,7 +162,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.75, 7.5, 10.0], [2.75, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.75, 7.5, 10.0], [2.75, 0.0, 0.0]) # Design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/scripts/tutorials/02_scene/create_scene.py b/scripts/tutorials/02_scene/create_scene.py index 82b5b21c009..8e056f8cf80 100644 --- a/scripts/tutorials/02_scene/create_scene.py +++ b/scripts/tutorials/02_scene/create_scene.py @@ -113,7 +113,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + sim._visualizer_interface.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) # Design scene scene_cfg = CartpoleSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/tutorials/04_sensors/add_sensors_on_robot.py b/scripts/tutorials/04_sensors/add_sensors_on_robot.py index 5cc6de6778b..4b57489fc20 100644 --- a/scripts/tutorials/04_sensors/add_sensors_on_robot.py +++ b/scripts/tutorials/04_sensors/add_sensors_on_robot.py @@ -160,7 +160,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # Design scene scene_cfg = SensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/tutorials/04_sensors/run_frame_transformer.py b/scripts/tutorials/04_sensors/run_frame_transformer.py index 8f951e93639..a8a24e095f7 100644 --- a/scripts/tutorials/04_sensors/run_frame_transformer.py +++ b/scripts/tutorials/04_sensors/run_frame_transformer.py @@ -167,7 +167,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device) sim = SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) # Design scene scene_entities = design_scene() # Play the simulator diff --git a/scripts/tutorials/04_sensors/run_ray_caster.py b/scripts/tutorials/04_sensors/run_ray_caster.py index 51780accbdd..a62ed0b596a 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster.py +++ b/scripts/tutorials/04_sensors/run_ray_caster.py @@ -131,7 +131,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([0.0, 15.0, 15.0], [0.0, 0.0, -2.5]) + sim._visualizer_interface.set_camera_view([0.0, 15.0, 15.0], [0.0, 0.0, -2.5]) # Design scene scene_entities = design_scene() # Play simulator diff --git a/scripts/tutorials/04_sensors/run_ray_caster_camera.py b/scripts/tutorials/04_sensors/run_ray_caster_camera.py index d9979965a0b..d210c66a77d 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster_camera.py +++ b/scripts/tutorials/04_sensors/run_ray_caster_camera.py @@ -165,7 +165,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg() sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 3.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 3.5], [0.0, 0.0, 0.0]) # Design scene scene_entities = design_scene() # Play simulator diff --git a/scripts/tutorials/04_sensors/run_usd_camera.py b/scripts/tutorials/04_sensors/run_usd_camera.py index 0dd99bb335a..ffc36a8c9ba 100644 --- a/scripts/tutorials/04_sensors/run_usd_camera.py +++ b/scripts/tutorials/04_sensors/run_usd_camera.py @@ -270,7 +270,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) # Design scene scene_entities = design_scene() # Play simulator diff --git a/scripts/tutorials/05_controllers/run_diff_ik.py b/scripts/tutorials/05_controllers/run_diff_ik.py index 22d17963235..01d6fad8e76 100644 --- a/scripts/tutorials/05_controllers/run_diff_ik.py +++ b/scripts/tutorials/05_controllers/run_diff_ik.py @@ -193,7 +193,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) # Design scene scene_cfg = TableTopSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/scripts/tutorials/05_controllers/run_osc.py b/scripts/tutorials/05_controllers/run_osc.py index 98b2532a0a2..b3e329a6e85 100644 --- a/scripts/tutorials/05_controllers/run_osc.py +++ b/scripts/tutorials/05_controllers/run_osc.py @@ -465,7 +465,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) # Design scene scene_cfg = SceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index acd493270c1..e99fa75440e 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -545,7 +545,6 @@ def close(self): # detach physx stage omni.physx.get_physx_simulation_interface().detach_stage() self.sim.stop() - self.sim.clear() self.sim.clear_instance() diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index a9c861d7057..9234b581b0b 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -514,7 +514,6 @@ def close(self): # detach physx stage omni.physx.get_physx_simulation_interface().detach_stage() self.sim.stop() - self.sim.clear() self.sim.clear_instance() diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 324dd9ae01d..6d629ae766a 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -528,7 +528,6 @@ def close(self): # detach physx stage omni.physx.get_physx_simulation_interface().detach_stage() self.sim.stop() - self.sim.clear() self.sim.clear_instance() diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 0ca18b50143..1ac9e0ac613 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -131,7 +131,7 @@ def _build_sim_frame(self): else: render_value = RenderMode.FULL_RENDERING - for visualizer in self.env.sim._visualizer._visualizers: + for visualizer in self.env.sim._visualizer_interface._visualizers: if hasattr(visualizer, "set_render_mode"): set_render_mode_fn = lambda value: visualizer.set_render_mode(RenderMode[value]) @@ -461,7 +461,7 @@ async def _dock_window(self, window_title: str): for _ in range(5): if omni.ui.Workspace.get_window(window_title): break - await self.env.sim._visualizer.app.next_update_async() + await omni.kit.app.get_app().next_update_async() # dock next to properties window custom_window = omni.ui.Workspace.get_window(window_title) diff --git a/source/isaaclab/isaaclab/envs/ui/empty_window.py b/source/isaaclab/isaaclab/envs/ui/empty_window.py index 3b6ecd80aaf..4302a51677d 100644 --- a/source/isaaclab/isaaclab/envs/ui/empty_window.py +++ b/source/isaaclab/isaaclab/envs/ui/empty_window.py @@ -69,7 +69,7 @@ async def _dock_window(self, window_title: str): for _ in range(5): if omni.ui.Workspace.get_window(window_title): break - await self.env.sim._visualizer.app.next_update_async() + await omni.kit.app.get_app().next_update_async() # dock next to properties window custom_window = omni.ui.Workspace.get_window(window_title) diff --git a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py index 6eb332dcdc4..97d1304c927 100644 --- a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py +++ b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py @@ -216,7 +216,7 @@ def update_view_location(self, eye: Sequence[float] | None = None, lookat: Seque cam_target = viewer_origin + self.default_cam_lookat # set the camera view - self._env.sim.set_camera_view(eye=cam_eye, target=cam_target) + self._env.sim._visualizer_interface.set_camera_view(eye=cam_eye, target=cam_target) """ Private Functions diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index f3a4cc9e5b2..2389b75a8dd 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -3,442 +3,258 @@ # # SPDX-License-Identifier: BSD-3-Clause -from __future__ import annotations - import builtins +import gc import logging import traceback from collections.abc import Iterator from contextlib import contextmanager -from typing import Any, ClassVar +from typing import Any import carb -import omni.kit.app -import omni.usd from pxr import Usd, UsdUtils +import isaaclab.sim.utils.stage as stage_utils + import isaaclab.sim as sim_utils -from isaaclab.utils.logger import configure_logging +from isaaclab.sim.utils import create_new_stage_in_memory, raise_callback_exception_if_any from isaaclab.utils.version import get_isaac_sim_version - +from .interface import PhysicsInterface, VisualizerInterface, Interface from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg -from .interface.physics_interface import PhysicsInterface -from .interface.visualizer_interface import VisualizerInterface +# import logger logger = logging.getLogger(__name__) -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: - - * configure the simulator with different settings such as the physics time-step, the number of physics substeps, - and the physics solver parameters (for more information, see :class:`isaaclab.sim.SimulationCfg`) - * playing, pausing, stepping and stopping the simulation - * adding and removing callbacks to different simulation events such as physics stepping, rendering, etc. - - This class provides a standalone simulation context for Isaac Lab, with additional functionalities - such as setting up the simulation context with a configuration object, exposing commonly used - simulator-related functions, and performing version checks of Isaac Sim to ensure compatibility - between releases. - - The simulation context is a singleton object. This means that there can only be one instance - of the simulation context at any given time. This is enforced by the parent class. Therefore, it is - not possible to create multiple instances of the simulation context. Instead, the simulation context - can be accessed using the ``instance()`` method. - - .. attention:: - Since we only support the `PyTorch `_ backend for simulation, the - simulation context is configured to use the ``torch`` backend by default. This means that - all the data structures used in the simulation are ``torch.Tensor`` objects. - - The simulation context can be used in two different modes of operations: - - 1. **Standalone python script**: In this mode, the user has full control over the simulation and - can trigger stepping events synchronously (i.e. as a blocking call). In this case the user - has to manually call :meth:`step` step the physics simulation and :meth:`render` to - render the scene. - 2. **Omniverse extension**: In this mode, the user has limited control over the simulation stepping - and all the simulation events are triggered asynchronously (i.e. as a non-blocking call). In this - case, the user can only trigger the simulation to start, pause, and stop. The simulation takes - care of stepping the physics simulation and rendering the scene. - - Based on above, for most functions in this class there is an equivalent function that is suffixed - with ``_async``. The ``_async`` functions are used in the Omniverse extension mode and - the non-``_async`` functions are used in the standalone python script mode. - """ - - _instance: SimulationContext | None = None - """The singleton instance of the simulation context.""" +class SettingsHelper: + """Helper for typed Carbonite settings access.""" - _is_initialized: ClassVar[bool] = False - """Whether the simulation context is initialized.""" + def __init__(self, settings: "carb.settings.ISettings"): + self._settings = settings - def __init__(self, cfg: SimulationCfg | None = None): - """Creates a simulation context to control the simulator. + def set(self, name: str, value: Any) -> None: + """Set a Carbonite setting with automatic type routing. Args: - cfg: The configuration of the simulation. Defaults to None, - in which case the default configuration is used. + name: The setting name (e.g., "/physics/cudaDevice"). + value: The value to set (bool, int, float, str, list, or tuple). """ - # 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() # type: ignore - # store configuration - self.cfg = cfg - - # setup logger - self.logger = configure_logging( - logging_level=self.cfg.logging_level, - save_logs_to_file=self.cfg.save_logs_to_file, - log_dir=self.cfg.log_dir, - ) - - # create stage in memory if requested - if self.cfg.create_stage_in_memory: - self._initial_stage = sim_utils.create_new_stage_in_memory() + if isinstance(value, bool): + self._settings.set_bool(name, value) + elif isinstance(value, int): + self._settings.set_int(name, value) + elif isinstance(value, float): + self._settings.set_float(name, value) + elif isinstance(value, str): + self._settings.set_string(name, value) + elif isinstance(value, (list, tuple)): + self._settings.set(name, value) 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 + raise ValueError(f"Unsupported value type for setting '{name}': {type(value)}") - # acquire settings interface - self.carb_settings = carb.settings.get_settings() + def get(self, name: str) -> Any: + """Get a Carbonite setting value.""" + return self._settings.get(name) - # initialize visualizer interface (handles viewport, render mode, render settings) - self._visualizer = VisualizerInterface(self) - # define a global variable to store the exceptions raised in the callback stack - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - # initialize physics interface (handles scene creation, delegates to backends) - self._physics_interface = PhysicsInterface(self) +class SimulationContext: + """Controls simulation lifecycle including physics stepping and rendering. - def __new__(cls, *args, **kwargs) -> SimulationContext: - """Returns the instance of the simulation context. + This singleton class manages: - 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: - logger.debug("Returning the previously defined instance of Simulation Context.") - return cls._instance # type: ignore + * Physics configuration (time-step, solver parameters via :class:`isaaclab.sim.SimulationCfg`) + * Simulation state (play, pause, step, stop) + * Rendering and visualization - """ - Operations - Singleton. + The singleton instance can be accessed using the ``instance()`` class method. """ - @classmethod - def instance(cls) -> SimulationContext | None: - """Returns the instance of the simulation context. + # Singleton instance + _instance: "SimulationContext | None" = None - Returns: - The instance of the simulation context. None if the instance is not initialized. - """ + def __new__(cls, cfg: SimulationCfg | None = None): + """Enforce singleton pattern.""" + if cls._instance is None: + cls._instance = super().__new__(cls) + cls._instance._initialized = False return cls._instance @classmethod - 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 - # close visualizer (unsubscribes stop handle) - cls._instance._visualizer.close() - # close physics interface (clears PhysxManager, detaches physx stage) - cls._instance._physics_interface.close() - # 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 - - """ - Properties. - """ - - @property - def stage(self) -> Usd.Stage: - """USD Stage.""" - return self._initial_stage + def instance(cls) -> "SimulationContext | None": + """Get the singleton instance, or None if not created.""" + return cls._instance - @property - def device(self) -> str: - """Device used by the simulation. + def __init__(self, cfg: SimulationCfg | None = None): + """Initialize the simulation context. - Note: - In Omniverse, it is possible to configure multiple GPUs for rendering, while physics engine - operates on a single GPU. This function returns the device that is used for physics simulation. + Args: + cfg: Simulation configuration. Defaults to None (uses default config). """ - return self._physics_interface.device - - """ - Operations - Simulation Information. - """ - def get_version(self) -> tuple[int, int, int]: - """Returns the version of the simulator. - - The returned tuple contains the following information: - - * Major version: This is the year of the release (e.g. 2022). - * Minor version: This is the half-year of the release (e.g. 1 or 2). - * Patch version: This is the patch number of the release (e.g. 0). - - .. attention:: - This function is deprecated and will be removed in the future. - We recommend using :func:`isaaclab.utils.version.get_isaac_sim_version` - instead of this function. + # Skip initialization if already initialized + if not self._initialized: + # store input + self.cfg = SimulationCfg() if cfg is None else cfg + self.device = self.cfg.device - Returns: - A tuple containing the major, minor, and patch versions. + # get existing stage or create new one in memory + stage_cache = UsdUtils.StageCache.Get() + all_stages = stage_cache.GetAllStages() if stage_cache.Size() > 0 else [] + self.stage = all_stages[0] if all_stages else create_new_stage_in_memory() - Example: - >>> sim = SimulationContext() - >>> sim.get_version() - (2022, 1, 0) - """ - return get_isaac_sim_version().major, get_isaac_sim_version().minor, get_isaac_sim_version().micro + # Cache stage in USD cache + stage_id = stage_cache.GetId(self.stage).ToLongInt() # type: ignore[union-attr] + if stage_id < 0: + stage_cache.Insert(self.stage) # type: ignore[union-attr] - def get_physics_dt(self) -> float: - """Returns the physics time step of the simulation. + # Acquire settings interface and create helper + self.carb_settings = carb.settings.get_settings() + self._settings_helper = SettingsHelper(self.carb_settings) - Returns: - The physics time step of the simulation. - """ - return self.cfg.physics_manager_cfg.dt + # Initialize interfaces (order matters: visualizer first for config, then physics) + self._visualizer_interface = VisualizerInterface(self) + self._physics_interface = PhysicsInterface(self) - @property - def physics_prim_path(self) -> str: - """The path to the physics scene prim.""" - return self.cfg.physics_manager_cfg.physics_prim_path + # List of interfaces for common operations + self._interfaces: list[Interface] = [ + self._physics_interface, + self._visualizer_interface, + ] - def get_rendering_dt(self) -> float: - """Returns the rendering time step of the simulation. + # define a global variable to store the exceptions raised in the callback stack + builtins.ISAACLAB_CALLBACK_EXCEPTION = None - Returns: - The rendering time step of the simulation. - """ - return self.cfg.physics_manager_cfg.dt * self.cfg.render_interval + self._is_playing = False + self._initialized = True - """ - Operations - Utilities. - """ - - def set_camera_view( - self, - eye: tuple[float, float, float], - target: tuple[float, float, float], - camera_prim_path: str = "/OmniverseKit_Persp", - ): - """Set the location and target of the viewport camera in the stage. + def _call_interfaces(self, method: str, **kwargs) -> None: + """Call a method on all interfaces.""" + for interface in self._interfaces: + getattr(interface, method)(**kwargs) + raise_callback_exception_if_any() - Note: - This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. - It is provided here for convenience to reduce the amount of imports needed. + def get_version(self) -> tuple[int, int, int]: + """Returns the version of the simulator (major, minor, patch).""" + ver = get_isaac_sim_version() + return ver.major, ver.minor, ver.micro - Args: - eye: The location of the camera eye. - target: The location of the camera target. - camera_prim_path: The path to the camera primitive in the stage. Defaults to - "/OmniverseKit_Persp". - """ - self._visualizer.set_camera_view(eye, target) + def set_setting(self, name: str, value: Any) -> None: + """Set a Carbonite setting value.""" + self._settings_helper.set(name, value) - def set_setting(self, name: str, value: Any): - """Set simulation settings using the Carbonite SDK. + def get_setting(self, name: str) -> Any: + """Get a Carbonite setting value.""" + return self._settings_helper.get(name) - .. note:: - If the input setting name does not exist, it will be created. If it does exist, the value will be - overwritten. Please make sure to use the correct setting name. + def forward(self) -> None: + """Update kinematics and sync scene data without stepping physics.""" + self._call_interfaces("forward") - To understand the settings interface, please refer to the - `Carbonite SDK `_ - documentation. + def reset(self, soft: bool = False) -> None: + """Reset the simulation. Args: - name: The name of the setting. - value: The value of the setting. + soft: If True, skip full reinitialization. """ - # Route through typed setters for correctness and consistency for common scalar types. - if isinstance(value, bool): - self.carb_settings.set_bool(name, value) - elif isinstance(value, int): - self.carb_settings.set_int(name, value) - elif isinstance(value, float): - self.carb_settings.set_float(name, value) - elif isinstance(value, str): - self.carb_settings.set_string(name, value) - elif isinstance(value, (list, tuple)): - self.carb_settings.set(name, value) - else: - raise ValueError(f"Unsupported value type for setting '{name}': {type(value)}") + self._call_interfaces("reset", soft=soft) + self._is_playing = True - def get_setting(self, name: str) -> Any: - """Read the simulation setting using the Carbonite SDK. + def step(self, render: bool = True) -> None: + """Step physics, update visualizers, and optionally render. Args: - name: The name of the setting. - - Returns: - The value of the setting. + render: Whether to render the scene after stepping. Defaults to True. """ - return self.carb_settings.get(name) - - """ - Operations- Timeline. - """ + self._call_interfaces("step", render=render) def is_playing(self) -> bool: - """Check whether the simulation is playing. - - Returns: - True if the simulator is playing. - """ - return self._visualizer.is_playing() + """Returns True if simulation is playing.""" + return self._is_playing def is_stopped(self) -> bool: - """Check whether the simulation is stopped. - - Returns: - True if the simulator is stopped. - """ - return self._visualizer.is_stopped() + """Returns True if simulation is stopped.""" + return not self._is_playing def play(self) -> None: - """Start playing the simulation.""" - self._visualizer.play() - # check for callback exceptions - self._check_for_callback_exceptions() + """Start the simulation.""" + self._call_interfaces("play") + self._is_playing = True def pause(self) -> None: """Pause the simulation.""" - self._visualizer.pause() - # check for callback exceptions - self._check_for_callback_exceptions() + self._call_interfaces("pause") def stop(self) -> None: - """Stop the simulation. - - Note: - Stopping the simulation will lead to the simulation state being lost. - """ - self._visualizer.stop() - # check for callback exceptions - self._check_for_callback_exceptions() - - """ - Operations - Override (standalone) - """ - - def reset(self, soft: bool = False): - """Reset the simulation. + """Stop the simulation.""" + self._call_interfaces("stop") + self._is_playing = False - .. 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 + def render(self, mode: int | None = None) -> None: + """Refresh rendering components (viewports, UI). 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. - + mode: Render mode. Defaults to None (use current mode). """ - self._physics_interface.reset(soft) - self._visualizer.reset(soft) - self._check_for_callback_exceptions() - - def forward(self) -> None: - """Updates articulation kinematics and fabric for rendering.""" self._physics_interface.forward() + self._visualizer_interface.render() - def step(self, render: bool = True): - """Steps the simulation. + def get_physics_dt(self) -> float: + """Returns the physics time step.""" + return self._physics_interface.physics_dt - .. note:: - This function blocks if the timeline is paused. It only returns when the timeline is playing. + def get_rendering_dt(self) -> float: + """Returns the rendering time step.""" + return self._visualizer_interface.get_rendering_dt() + + @classmethod + def clear_instance(cls, clear_stage: bool = True) -> None: + """Clean up resources and clear the singleton instance. Args: - 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. + clear_stage: If True, clear stage prims (preserving /World and PhysicsScene) + before closing. Defaults to True. """ - self._visualizer.step(render) - if self.is_playing(): - self._physics_interface.step(render) + if cls._instance is None: + return - def render(self, mode: RenderMode | None = None): - """Refreshes the rendering components including UI elements and view-ports depending on the render mode. + if not cls._instance._initialized: + logger.warning("Simulation context not initialized.") + return - This function is used to refresh the rendering components of the simulation. This includes updating the - view-ports, UI elements, and other extensions (besides physics simulation) that are running in the - background. The rendering components are refreshed based on the render mode. + # Optionally clear stage contents first + if clear_stage: + cls.clear_stage() + # Close all interfaces + cls._instance._call_interfaces("close") - Please see :class:`RenderMode` for more information on the different render modes. + # Remove stage from cache + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(cls._instance.stage).ToLongInt() # type: ignore[union-attr] + if stage_id > 0: + stage_cache.Erase(cls._instance.stage) # type: ignore[union-attr] - Args: - mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. - """ - self._physics_interface.forward() - self._visualizer.render() + # Clear instance + cls._instance._initialized = False + cls._instance = None + + gc.collect() @classmethod - def clear(cls): - """Clear the current USD stage.""" + def clear_stage(cls) -> None: + """Clear the current USD stage (preserving /World and PhysicsScene). - 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 - - # 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.") + Note: + This clears stage prims but keeps the context alive. For full cleanup, + use :meth:`clear_instance` instead. + """ + if cls._instance is None: + return - """ - Helper Functions - """ + def _predicate(prim: Usd.Prim) -> bool: + path = prim.GetPath().pathString # type: ignore[union-attr] + return path != "/World" and prim.GetTypeName() != "PhysicsScene" - 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. - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # type: ignore - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore - raise exception_to_raise + sim_utils.clear_stage(stage=cls._instance.stage, predicate=_predicate) @contextmanager @@ -490,20 +306,15 @@ def build_simulation_context( """ try: if create_new_stage: - # Clear any existing simulation context before creating a new stage - # This ensures proper resource cleanup and allows a fresh initialization - SimulationContext.clear_instance() - sim_utils.create_new_stage() + stage_utils.create_new_stage() if sim_cfg is None: - # Construct one and overwrite the dt, gravity, and device in physics_manager_cfg from isaaclab.physics.physx_manager_cfg import PhysxManagerCfg gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) physics_manager_cfg = PhysxManagerCfg(dt=dt, device=device, gravity=gravity) sim_cfg = SimulationCfg(physics_manager_cfg=physics_manager_cfg) - # Construct simulation context sim = SimulationContext(sim_cfg) if add_ground_plane: @@ -511,13 +322,10 @@ def build_simulation_context( cfg = GroundPlaneCfg() cfg.func("/World/defaultGroundPlane", cfg) - if add_lighting or (auto_add_lighting and sim.carb_settings.get("/isaaclab/has_gui")): + if add_lighting or (auto_add_lighting and sim.get_setting("/isaaclab/has_gui")): # Lighting cfg = DomeLightCfg( - color=(0.1, 0.1, 0.1), - enable_color_temperature=True, - color_temperature=5500, - intensity=10000, + color=(0.1, 0.1, 0.1), enable_color_temperature=True, color_temperature=5500, intensity=10000 ) # Dome light named specifically to avoid conflicts cfg.func(prim_path="/World/defaultDomeLight", cfg=cfg, translation=(0.0, 0.0, 10.0)) @@ -525,15 +333,9 @@ def build_simulation_context( yield sim except Exception: - sim.logger.error(traceback.format_exc()) + logger.error(traceback.format_exc()) raise finally: - if not sim.carb_settings.get("/isaaclab/has_gui"): - # Stop simulation only if we aren't rendering otherwise the app will hang indefinitely + if not sim.get_setting("/isaaclab/has_gui"): sim.stop() - - # 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: # type: ignore - raise builtins.ISAACLAB_CALLBACK_EXCEPTION # type: ignore diff --git a/source/isaaclab/isaaclab/sim/utils/__init__.py b/source/isaaclab/isaaclab/sim/utils/__init__.py index 3a85ae44c2f..00c7ad5007a 100644 --- a/source/isaaclab/isaaclab/sim/utils/__init__.py +++ b/source/isaaclab/isaaclab/sim/utils/__init__.py @@ -5,9 +5,23 @@ """Utilities built around USD operations.""" +import builtins + from .legacy import * # noqa: F401, F403 from .prims import * # noqa: F401, F403 from .queries import * # noqa: F401, F403 from .semantics import * # noqa: F401, F403 from .stage import * # noqa: F401, F403 from .transforms import * # noqa: F401, F403 + + +def raise_callback_exception_if_any() -> None: + """Check for and re-raise any exception stored in the callback exception global. + + This is used to propagate exceptions from callbacks (like physics or render callbacks) + back to the main thread where they can be properly handled. + """ + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None + raise exception_to_raise diff --git a/source/isaaclab/test/assets/check_external_force.py b/source/isaaclab/test/assets/check_external_force.py index f666135ba3b..0c6e2ec02a4 100644 --- a/source/isaaclab/test/assets/check_external_force.py +++ b/source/isaaclab/test/assets/check_external_force.py @@ -54,7 +54,7 @@ def main(): # Load kit helper sim = SimulationContext(sim_utils.SimulationCfg(dt=0.005)) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # Spawn things into stage # Ground-plane diff --git a/source/isaaclab/test/assets/check_fixed_base_assets.py b/source/isaaclab/test/assets/check_fixed_base_assets.py index c62c5a3334d..c4e5994524b 100644 --- a/source/isaaclab/test/assets/check_fixed_base_assets.py +++ b/source/isaaclab/test/assets/check_fixed_base_assets.py @@ -145,7 +145,7 @@ def main(): # Initialize the simulation context sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01)) # Set main camera - sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0]) # design scene scene_entities, scene_origins = design_scene() scene_origins = torch.tensor(scene_origins, device=sim.device) diff --git a/source/isaaclab/test/assets/check_ridgeback_franka.py b/source/isaaclab/test/assets/check_ridgeback_franka.py index 724cd8b2a08..53bc559d60f 100644 --- a/source/isaaclab/test/assets/check_ridgeback_franka.py +++ b/source/isaaclab/test/assets/check_ridgeback_franka.py @@ -149,7 +149,7 @@ def main(): # Initialize the simulation context sim = sim_utils.SimulationContext(sim_utils.SimulationCfg()) # Set main camera - sim.set_camera_view([1.5, 1.5, 1.5], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([1.5, 1.5, 1.5], [0.0, 0.0, 0.0]) # design scene robot = design_scene() # Play the simulator diff --git a/source/isaaclab/test/controllers/test_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py index 4c4cc60a055..6774c1b97fa 100644 --- a/source/isaaclab/test/controllers/test_differential_ik.py +++ b/source/isaaclab/test/controllers/test_differential_ik.py @@ -77,7 +77,6 @@ def sim(): # Cleanup sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 3c309c14f20..b2e16c3c961 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -202,7 +202,6 @@ def sim(): # Cleanup sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index e16e0a0f549..ff10b34ee37 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -118,7 +118,7 @@ def env_and_cfg(request): right_eef_urdf_link_name = right_candidates[0] # Set up camera view - env.sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 1.0]) + env.sim._visualizer_interface.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 1.0]) # Create test parameters from test_cfg test_params = { diff --git a/source/isaaclab/test/deps/isaacsim/check_ref_count.py b/source/isaaclab/test/deps/isaacsim/check_ref_count.py index b0d5127e5c8..ff471d84402 100644 --- a/source/isaaclab/test/deps/isaacsim/check_ref_count.py +++ b/source/isaaclab/test/deps/isaacsim/check_ref_count.py @@ -142,7 +142,7 @@ def main(): print("---" * 10) # Clean up - sim.clear() + sim.clear_instance() print("Reference count of the robot view: ", ctypes.c_long.from_address(id(robot)).value) print("Referrers of the robot view: ", gc.get_referrers(robot)) diff --git a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py index ffb89eafb55..6dee007a51b 100644 --- a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py +++ b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py @@ -91,7 +91,7 @@ def main(): env_positions = cloner.clone( source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True, copy_from_source=True ) - physics_scene_path = sim.physics_prim_path + physics_scene_path = sim.cfg.physics_manager_cfg.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/markers/check_markers_visibility.py b/source/isaaclab/test/markers/check_markers_visibility.py index 98dbee8ddcd..ac5c734994a 100644 --- a/source/isaaclab/test/markers/check_markers_visibility.py +++ b/source/isaaclab/test/markers/check_markers_visibility.py @@ -132,7 +132,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0]) # design scene scene_cfg = SensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) scene = InteractiveScene(scene_cfg) diff --git a/source/isaaclab/test/scene/check_interactive_scene.py b/source/isaaclab/test/scene/check_interactive_scene.py index 5b2463b315a..6fc8b4d7479 100644 --- a/source/isaaclab/test/scene/check_interactive_scene.py +++ b/source/isaaclab/test/scene/check_interactive_scene.py @@ -82,7 +82,7 @@ def main(): # Load kit helper sim = SimulationContext(sim_utils.SimulationCfg(dt=0.005)) # Set main camera - sim.set_camera_view(eye=[5, 5, 5], target=[0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view(eye=[5, 5, 5], target=[0.0, 0.0, 0.0]) # Spawn things into stage with Timer("Setup scene"): diff --git a/source/isaaclab/test/scene/test_interactive_scene.py b/source/isaaclab/test/scene/test_interactive_scene.py index cee8cc8b694..2eab9f3f070 100644 --- a/source/isaaclab/test/scene/test_interactive_scene.py +++ b/source/isaaclab/test/scene/test_interactive_scene.py @@ -67,7 +67,6 @@ def make_scene(num_envs: int, env_spacing: float = 1.0): yield make_scene, sim sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index f01ba84b1ce..56d1872e9f9 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -110,7 +110,7 @@ def main(): ) contact_sensor = ContactSensor(cfg=contact_sensor_cfg) # filter collisions within each environment instance - physics_scene_path = sim.physics_prim_path + physics_scene_path = sim.cfg.physics_manager_cfg.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"] ) diff --git a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py index 2b1a084abcc..07d60f96c9b 100644 --- a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py +++ b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py @@ -102,7 +102,7 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048): cloner.define_base_env("/World/envs") envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) - physics_scene_path = sim.physics_prim_path + physics_scene_path = sim.cfg.physics_manager_cfg.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index 4554c18e508..12d25aa0ebf 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -79,7 +79,7 @@ def design_scene(sim: SimulationContext, num_envs: int = 2048): cloner.define_base_env("/World/envs") envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs) cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) - physics_scene_path = sim.physics_prim_path + physics_scene_path = sim.cfg.physics_manager_cfg.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 6c479a87949..4a18bdc87c1 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -78,7 +78,7 @@ def sim(): # Load kit helper sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.005, device="cpu")) # Set main camera - sim.set_camera_view(eye=(5.0, 5.0, 5.0), target=(0.0, 0.0, 0.0)) + sim._visualizer_interface.set_camera_view(eye=(5.0, 5.0, 5.0), target=(0.0, 0.0, 0.0)) yield sim # Cleanup sim.clear_instance() diff --git a/source/isaaclab/test/sim/check_meshes.py b/source/isaaclab/test/sim/check_meshes.py index c99f56ec820..9771330fcce 100644 --- a/source/isaaclab/test/sim/check_meshes.py +++ b/source/isaaclab/test/sim/check_meshes.py @@ -143,7 +143,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.01) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view([8.0, 8.0, 6.0], [0.0, 0.0, 0.0]) + sim._visualizer_interface.set_camera_view([8.0, 8.0, 6.0], [0.0, 0.0, 0.0]) # Design scene by adding assets to it design_scene() diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index d207be3b16f..498cd0cff36 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -70,7 +70,6 @@ def sim(): # stop simulation sim.stop() # cleanup stage and context - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_mjcf_converter.py b/source/isaaclab/test/sim/test_mjcf_converter.py index d8d2e99dd1d..f67df0d0fd7 100644 --- a/source/isaaclab/test/sim/test_mjcf_converter.py +++ b/source/isaaclab/test/sim/test_mjcf_converter.py @@ -47,7 +47,6 @@ def test_setup_teardown(): # Teardown: Cleanup simulation sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 595e5984818..919cd3a0127 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -74,7 +74,6 @@ def setup_simulation(): yield sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg # Teardown sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index b20c016e9ab..74d30bd497b 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -340,7 +340,7 @@ def test_clear_stage(): assert sim.stage.GetPrimAtPath("/World/Cube2").IsValid() # clear the stage - sim.clear() + sim.clear_stage() # verify objects are removed but World and Physics remain assert not sim.stage.GetPrimAtPath("/World/Cube1").IsValid() 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 84c6d68bfcc..59c9d73ae32 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(): yield sim omni.physx.get_physx_simulation_interface().detach_stage() 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/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index 451aa6b0c8b..7a095b2ce2d 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -39,7 +39,6 @@ def sim(): # cleanup after test sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_lights.py b/source/isaaclab/test/sim/test_spawn_lights.py index e0acb6e01de..6b6b4b7dce1 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_instance() diff --git a/source/isaaclab/test/sim/test_spawn_materials.py b/source/isaaclab/test/sim/test_spawn_materials.py index f5f097aa4f2..162f57f2fc3 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(): sim_utils.update_stage() yield sim sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index 353a6a42bb5..25221b91e8f 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -34,7 +34,7 @@ def sim(): yield sim # Cleanup sim.stop() - sim.clear() + () sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_sensors.py b/source/isaaclab/test/sim/test_spawn_sensors.py index bade39d5ba4..3606baa7df3 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(): sim_utils.update_stage() yield sim sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index ebbf8684737..b86c343f62a 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(): sim_utils.update_stage() yield sim sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index 6b238611cf9..ed9366e9681 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(): sim_utils.update_stage() yield sim sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_urdf_converter.py b/source/isaaclab/test/sim/test_urdf_converter.py index 06f3ecdd4ff..089728330bd 100644 --- a/source/isaaclab/test/sim/test_urdf_converter.py +++ b/source/isaaclab/test/sim/test_urdf_converter.py @@ -56,7 +56,6 @@ def sim_config(): yield sim, config # Teardown sim.stop() - sim.clear() sim.clear_instance() diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index 7bb766c7130..e85d960f0d5 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -158,7 +158,7 @@ def main(): cloner.define_base_env("/World/envs") envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_balls) cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True) - physics_scene_path = sim.physics_prim_path + physics_scene_path = sim.cfg.physics_manager_cfg.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index 2f0d33482b4..17e4cdf733c 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -317,7 +317,7 @@ def _populate_scene(sim: SimulationContext, num_balls: int = 2048, geom_sphere: prim_paths=envs_prim_paths, replicate_physics=True, ) - physics_scene_path = sim.physics_prim_path + physics_scene_path = sim.cfg.physics_manager_cfg.physics_prim_path cloner.filter_collisions( physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) diff --git a/source/isaaclab/test/visualization/check_scene_xr_visualization.py b/source/isaaclab/test/visualization/check_scene_xr_visualization.py index 11fe31998dd..2d43d54b3de 100644 --- a/source/isaaclab/test/visualization/check_scene_xr_visualization.py +++ b/source/isaaclab/test/visualization/check_scene_xr_visualization.py @@ -238,7 +238,7 @@ def main(): sim_cfg = sim_utils.SimulationCfg(dt=0.005) sim = sim_utils.SimulationContext(sim_cfg) # Set main camera - sim.set_camera_view(eye=(8, 0, 4), target=(0.0, 0.0, 0.0)) + sim._visualizer_interface.set_camera_view(eye=(8, 0, 4), target=(0.0, 0.0, 0.0)) # design scene scene = InteractiveScene(SimpleSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)) # Play the simulator diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py index e40820054d5..28bd1594bf1 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py @@ -144,7 +144,7 @@ class G1LocomanipulationSDGEnv(LocomanipulationSDGEnv): def __init__(self, cfg: G1LocomanipulationSDGEnvCfg, **kwargs): super().__init__(cfg) - self.sim.set_camera_view([10.5, 10.5, 10.5], [0.0, 0.0, 0.5]) + self.sim._visualizer_interface.set_camera_view([10.5, 10.5, 10.5], [0.0, 0.0, 0.5]) self._upper_body_dim = self.action_manager.get_term("upper_body_ik").action_dim self._waist_dim = 0 # self._env.action_manager.get_term("waist_joint_pos").action_dim self._lower_body_dim = self.action_manager.get_term("lower_body_joint_pos").action_dim From deb84dca4713f15cb13bc47f9e3ff0811276bbde Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 3 Feb 2026 11:54:27 -0800 Subject: [PATCH 130/130] passing simulation context test --- .../isaaclab/physics/newton_manager.py | 4 ++-- .../isaaclab/physics/physx_manager.py | 9 ++++---- .../isaaclab/sim/interface/interface.py | 4 ++++ .../isaaclab/sim/simulation_context.py | 23 ++++++++++++------- .../test/sim/test_simulation_context.py | 6 +---- 5 files changed, 27 insertions(+), 19 deletions(-) diff --git a/source/isaaclab/isaaclab/physics/newton_manager.py b/source/isaaclab/isaaclab/physics/newton_manager.py index bfd8a04d21a..c734358f740 100644 --- a/source/isaaclab/isaaclab/physics/newton_manager.py +++ b/source/isaaclab/isaaclab/physics/newton_manager.py @@ -104,9 +104,9 @@ def initialize(cls, sim_context: "SimulationContext") -> None: cls._sim = sim_context cls._cfg = sim_context.cfg.physics_manager_cfg # type: ignore[assignment] - # Set simulation parameters from config + # Set simulation parameters from config (device comes from sim_context, not cfg) cls._dt = cls._cfg.dt - cls._device = cls._cfg.device + cls._device = sim_context.device cls._gravity_vector = cls._cfg.gravity # USD fabric sync only needed for OV rendering diff --git a/source/isaaclab/isaaclab/physics/physx_manager.py b/source/isaaclab/isaaclab/physics/physx_manager.py index 27856f052dd..3cf936a9adc 100644 --- a/source/isaaclab/isaaclab/physics/physx_manager.py +++ b/source/isaaclab/isaaclab/physics/physx_manager.py @@ -441,7 +441,7 @@ def get_device(cls) -> str: @classmethod def _is_gpu_device(cls) -> bool: """Check if configured for GPU physics.""" - return cls._cfg is not None and "cuda" in cls._cfg.device + return cls._sim is not None and "cuda" in cls._sim.device @classmethod def is_fabric_enabled(cls) -> bool: @@ -720,13 +720,14 @@ def _apply_carb_settings(cls) -> None: @classmethod def _apply_device_settings(cls) -> None: - """Configure GPU/CPU device settings.""" - if cls._sim is None or cls._cfg is None: + """Configure GPU/CPU device settings based on SimulationContext.device.""" + if cls._sim is None: return settings = cls._sim.carb_settings + device = cls._sim.device # Device comes from SimulationContext if cls._is_gpu_device(): - device_parts = cls._cfg.device.split(":") + device_parts = device.split(":") if len(device_parts) == 1: # "cuda" without ID - use carb setting or default to 0 device_id = max(0, settings.get_as_int("/physics/cudaDevice")) diff --git a/source/isaaclab/isaaclab/sim/interface/interface.py b/source/isaaclab/isaaclab/sim/interface/interface.py index e8bdb4423cd..ae4bc8afdb1 100644 --- a/source/isaaclab/isaaclab/sim/interface/interface.py +++ b/source/isaaclab/isaaclab/sim/interface/interface.py @@ -60,6 +60,10 @@ def play(self) -> None: """Handle simulation start.""" pass + def pause(self) -> None: + """Handle simulation pause.""" + pass + def stop(self) -> None: """Handle simulation stop.""" pass diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 2389b75a8dd..0809535f37d 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -99,7 +99,7 @@ def __init__(self, cfg: SimulationCfg | None = None): # get existing stage or create new one in memory stage_cache = UsdUtils.StageCache.Get() - all_stages = stage_cache.GetAllStages() if stage_cache.Size() > 0 else [] + all_stages = stage_cache.GetAllStages() if stage_cache.Size() > 0 else [] # type: ignore[union-attr] self.stage = all_stages[0] if all_stages else create_new_stage_in_memory() # Cache stage in USD cache @@ -122,9 +122,11 @@ def __init__(self, cfg: SimulationCfg | None = None): ] # define a global variable to store the exceptions raised in the callback stack - builtins.ISAACLAB_CALLBACK_EXCEPTION = None + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore[attr-defined] + # Simulation state: playing, paused, or stopped self._is_playing = False + self._is_stopped = True self._initialized = True def _call_interfaces(self, method: str, **kwargs) -> None: @@ -158,6 +160,7 @@ def reset(self, soft: bool = False) -> None: """ self._call_interfaces("reset", soft=soft) self._is_playing = True + self._is_stopped = False def step(self, render: bool = True) -> None: """Step physics, update visualizers, and optionally render. @@ -168,26 +171,30 @@ def step(self, render: bool = True) -> None: self._call_interfaces("step", render=render) def is_playing(self) -> bool: - """Returns True if simulation is playing.""" + """Returns True if simulation is playing (not paused or stopped).""" return self._is_playing def is_stopped(self) -> bool: - """Returns True if simulation is stopped.""" - return not self._is_playing + """Returns True if simulation is stopped (not just paused).""" + return self._is_stopped def play(self) -> None: - """Start the simulation.""" + """Start or resume the simulation.""" self._call_interfaces("play") self._is_playing = True + self._is_stopped = False def pause(self) -> None: - """Pause the simulation.""" + """Pause the simulation (can be resumed with play).""" self._call_interfaces("pause") + self._is_playing = False + # Note: _is_stopped remains False - paused is different from stopped def stop(self) -> None: - """Stop the simulation.""" + """Stop the simulation completely.""" self._call_interfaces("stop") self._is_playing = False + self._is_stopped = True def render(self, mode: int | None = None) -> None: """Refresh rendering components (viewports, UI). diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index 74d30bd497b..1efcbd7e7e6 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -34,9 +34,6 @@ def test_setup_teardown(): # Yield for the test yield - # Teardown: Clear the simulation context after each test - if SimulationContext.instance() is not None: - SimulationContext.clear() SimulationContext.clear_instance() @@ -54,10 +51,9 @@ def test_init(device): physics_manager_cfg = PhysxManagerCfg( physics_prim_path="/Physics/PhysX", gravity=(0.0, -0.5, -0.5), - device=device, physics_material=RigidBodyMaterialCfg(), ) - cfg = SimulationCfg(physics_manager_cfg=physics_manager_cfg, render_interval=5) + cfg = SimulationCfg(device=device, physics_manager_cfg=physics_manager_cfg, render_interval=5) # sim = SimulationContext(cfg) # TODO: Figure out why keyword argument doesn't work. # note: added a fix in Isaac Sim 2023.1 for this.