From ef821d927986f4d91c85399a1e882d968293a158 Mon Sep 17 00:00:00 2001 From: Piotr Barejko Date: Wed, 23 Sep 2026 17:25:05 -0700 Subject: [PATCH 01/11] Remove camera_path_relative_to_env_0 (#7983) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit (cherry picked from commit e5f1bdbe917af29e70fd9fa0c6b24344465285a0) # Description Remove confusing field from Camera spec Fixes # (issue) ## Type of change - Bug fix (non-breaking change which fixes an issue) - New feature (non-breaking change which adds functionality) - Breaking change (existing functionality will not work without user modification) - Documentation update ## Release backport - [ ] Backport this pull request to the active release branch after it merges into `develop` ## Screenshots Please attach before and after screenshots of the change if applicable. ## Checklist Docker and GPU tests run on demand. Push the commits you want tested, then comment `run-ci` on the pull request. - [ ] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [ ] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works - [ ] I have added a changelog fragment under `source//changelog.d/` for every touched package (do **not** edit `CHANGELOG.rst` or bump `extension.toml` — CI handles that) - [ ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --- .../camera-render-spec-paths.major.rst | 6 ++ .../isaaclab/renderers/camera_render_spec.py | 9 +-- .../isaaclab/sensors/camera/camera.py | 5 -- .../changelog.d/ovrtx-camera-source-path.rst | 7 ++ .../isaaclab_ov/renderers/ovrtx_renderer.py | 77 ++++++++++--------- .../isaaclab_ov/renderers/ovrtx_usd.py | 2 +- .../isaaclab_ov/test/test_ovrtx_clone_plan.py | 3 +- .../test/test_ovrtx_deformable_bindings.py | 2 +- .../test/test_ovrtx_renderer_contract.py | 34 +++++++- source/isaaclab_ov/test/test_ovrtx_usd.py | 1 - 10 files changed, 90 insertions(+), 56 deletions(-) create mode 100644 source/isaaclab/changelog.d/camera-render-spec-paths.major.rst create mode 100644 source/isaaclab_ov/changelog.d/ovrtx-camera-source-path.rst diff --git a/source/isaaclab/changelog.d/camera-render-spec-paths.major.rst b/source/isaaclab/changelog.d/camera-render-spec-paths.major.rst new file mode 100644 index 000000000000..dd40abb0ae92 --- /dev/null +++ b/source/isaaclab/changelog.d/camera-render-spec-paths.major.rst @@ -0,0 +1,6 @@ +Removed +^^^^^^^ + +* **Breaking:** Removed ``CameraRenderSpec.camera_path_relative_to_env_0``. Remove this argument + from render-spec constructors and use the absolute paths in ``camera_prim_paths`` instead. + OVRTX derived cloned camera paths from the authored source camera internally. diff --git a/source/isaaclab/isaaclab/renderers/camera_render_spec.py b/source/isaaclab/isaaclab/renderers/camera_render_spec.py index 9797c97baa61..f30cc2c31e7f 100644 --- a/source/isaaclab/isaaclab/renderers/camera_render_spec.py +++ b/source/isaaclab/isaaclab/renderers/camera_render_spec.py @@ -23,10 +23,10 @@ class CameraRenderSpec: cfg: Camera configuration (data types, resolution, filters, etc.). device: Torch device string (e.g. ``"cuda:0"``) used by GPU annotators and Warp. num_instances: Number of tiled camera instances (environments). - camera_prim_paths: Absolute USD paths for each environment's camera prim. - view_count: Number of camera prims (must match ``len(camera_prim_paths)``). - camera_path_relative_to_env_0: Camera prim path with ``/World/envs/env_0/`` prefix - stripped; required by OVRTX. Empty string if the first camera is not under env 0. + camera_prim_paths: Absolute paths of the authored camera prims. When the renderer + clones environments internally, this may contain only the source camera path; + the renderer resolves its logical per-environment paths during registration. + view_count: Number of logical camera instances in the sensor view. """ cfg: CameraCfg @@ -34,4 +34,3 @@ class CameraRenderSpec: num_instances: int camera_prim_paths: tuple[str, ...] view_count: int - camera_path_relative_to_env_0: str diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 68b24a8cf77c..1b6d2b801f11 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -652,10 +652,6 @@ def _initialize_impl(self): # any renderer-side per-camera setup) and ``create_render_data`` consume # it, and the prims are already authored at this point. cam_paths = tuple(str(p.GetPath()) for p in sim_utils.find_matching_prims(self.cfg.prim_path, self.stage)) - env_0_prefix = "/World/envs/env_0/" - rel_under_env0 = ( - cam_paths[0].removeprefix(env_0_prefix) if cam_paths and cam_paths[0].startswith(env_0_prefix) else "" - ) device_str = self._device if isinstance(self._device, str) else str(self._device) render_spec = CameraRenderSpec( cfg=self.cfg, @@ -663,7 +659,6 @@ def _initialize_impl(self): num_instances=self._num_envs, camera_prim_paths=cam_paths, view_count=self._num_envs, - camera_path_relative_to_env_0=rel_under_env0, ) # Delegate per-camera USD setup to the renderer — must run **before** diff --git a/source/isaaclab_ov/changelog.d/ovrtx-camera-source-path.rst b/source/isaaclab_ov/changelog.d/ovrtx-camera-source-path.rst new file mode 100644 index 000000000000..fe9f940d34b6 --- /dev/null +++ b/source/isaaclab_ov/changelog.d/ovrtx-camera-source-path.rst @@ -0,0 +1,7 @@ +Changed +^^^^^^^ + +* Derived OVRTX cloned camera paths from the authored absolute source path in + ``CameraRenderSpec.camera_prim_paths``. Remove the ``camera_path_relative_to_env_0`` argument + when constructing render specs; OVRTX validated the source path under + ``/World/envs/env_0/`` and resolved the per-environment paths internally. diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py index 8c026265a9ed..426933899eb5 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py @@ -211,6 +211,30 @@ def _gpu_side_render_var_sync_enabled() -> bool: return value == "1" +def _get_cloned_camera_paths(camera_prim_path: str, num_instances: int) -> list[str]: + """Return paths for the source camera in env_0 and its clones in every other environment. + + Cloned cameras may be absent from the authored USD. OVRTX still needs one path per + environment; these can be synthesized because :meth:`OVRTXRenderer.prepare_stage` + requires environment ids ordered from zero. + + Args: + camera_prim_path: Absolute path of the source camera under ``/World/envs/env_0/``. + num_instances: Number of environments the camera is replicated into. + + Returns: + One absolute camera prim path per environment, in environment id order. + + Raises: + ValueError: If the source camera does not live under ``/World/envs/env_0/``. + """ + env_0_prefix = "/World/envs/env_0/" + camera_rel_path = camera_prim_path.removeprefix(env_0_prefix) + if not camera_prim_path.startswith(env_0_prefix) or not camera_rel_path: + raise ValueError(f"OVRTX cameras must be under {env_0_prefix}, got {camera_prim_path!r}.") + return [f"/World/envs/env_{i}/{camera_rel_path}" for i in range(num_instances)] + + def _write_file(output_dir: Path, file_name: str, content: str) -> None: """Write ``content`` to ``output_dir / file_name``. @@ -229,31 +253,6 @@ def _write_file(output_dir: Path, file_name: str, content: str) -> None: logger.info("Wrote USD file: %s", output_path) -def _env_camera_prim_paths(camera_path_relative_to_env_0: str | None, num_instances: int) -> list[str]: - """Per-env camera prim paths derived from the env 0 prototype. - - ``CameraRenderSpec.camera_prim_paths`` names only the camera prims authored on the USD stage, - which is one prototype per spawn variant whenever USD replication does not run. That is the - kitless case: the clone plan routes ``UsdReplicateContext`` only under Kit, so OvPhysx, Newton - and OVRTX each replicate the prototype themselves. OVRTX still needs one path per environment, - which is safe to synthesize because :meth:`OVRTXRenderer.prepare_stage` requires env ids - ordered from zero. - - Args: - camera_path_relative_to_env_0: Camera prim path with the ``/World/envs/env_0/`` prefix stripped. - num_instances: Number of environments the camera is replicated into. - - Returns: - One absolute camera prim path per environment, in env id order. - - Raises: - ValueError: If the camera prototype does not live under ``/World/envs/env_0/``. - """ - if not camera_path_relative_to_env_0: - raise ValueError("OVRTX cameras must be under /World/envs/env_0/.") - return [f"/World/envs/env_{i}/{camera_path_relative_to_env_0}" for i in range(num_instances)] - - def _write_combined_stage(output_dir: Path, scene_usd: str, render_product_usd: str) -> None: """Write the scene and render product prims in one debug layer, preserving scene metadata.""" from pxr import Sdf @@ -405,7 +404,7 @@ def __init__(self, cfg: OVRTXRendererCfg): self._cable_points: wp.array | None = None self._initialized_scene = False self._exported_usd_string: str | None = None - self._camera_rel_path: str | None = None + self._camera_prim_path: str | None = None self._output_id_color_buffers: dict[str, wp.array] = {} self._clone_plan: ClonePlan | None = None self._visual_material_writer_ref: weakref.ReferenceType[OVRTXVisualMaterialWriter] | None = None @@ -588,7 +587,7 @@ def _initialize_camera_render_data_from_spec_legacy( first_cam_path = spec.camera_prim_paths[0] if not first_cam_path.startswith(env_0_prefix): raise RuntimeError(f"Expected camera prim under '{env_0_prefix}', got '{first_cam_path}'") - self._camera_rel_path = spec.camera_path_relative_to_env_0 + self._camera_prim_path = first_cam_path logger.info("Injecting camera definitions...") @@ -616,7 +615,7 @@ def _initialize_camera_render_data_from_spec_legacy( render_data.resources.callback(self.backend.renderer.remove_usd, reference) logger.info("OVRTX loaded USD from string successfully") - camera_paths = _env_camera_prim_paths(self._camera_rel_path, num_envs) + camera_paths = _get_cloned_camera_paths(self._camera_prim_path, num_envs) if num_envs > 1: self._clone_sources_in_ovrtx() self._update_scene_partitions_after_clone(num_envs) @@ -697,7 +696,7 @@ def _update_scene_partitions_after_clone(self, num_envs: int): logger.info("Writing scene partitions for %d environments...", num_envs) partition_tokens = [f"env_{i}" for i in range(num_envs)] env_prim_paths = [f"/World/envs/env_{i}" for i in range(num_envs)] - camera_prim_paths = _env_camera_prim_paths(self._camera_rel_path, num_envs) + camera_prim_paths = _get_cloned_camera_paths(self._camera_prim_path, num_envs) self.backend.renderer.write_attribute( env_prim_paths, @@ -739,8 +738,9 @@ def _setup_xform_bindings_legacy(self): object_paths = [] newton_indices = [] + camera_rel_path = self._camera_prim_path.removeprefix("/World/envs/env_0/") for idx, path in enumerate(all_body_paths): - if "/World/envs/" in path and self._camera_rel_path not in path and "GroundPlane" not in path: + if "/World/envs/" in path and camera_rel_path not in path and "GroundPlane" not in path: object_paths.append(path) newton_indices.append(idx) @@ -958,6 +958,7 @@ def create_render_data(self, spec: CameraRenderSpec) -> OVRTXCameraRenderData: Performs OVRTX initialization (stage export, USD load, bindings) on first call, matching the interface of Isaac RTX and Newton Warp which need no separate initialize(). """ + camera_paths = _get_cloned_camera_paths(spec.camera_prim_paths[0], spec.num_instances) # Normalize aliases such as "cuda" before comparing cameras sharing this renderer. warp_device = wp.get_device(spec.device) if self._initialized_scene and str(warp_device) != self._device: @@ -982,10 +983,9 @@ def create_render_data(self, spec: CameraRenderSpec) -> OVRTXCameraRenderData: else: self._register_camera(spec, render_data) if not self._use_ovstage: - intrinsic_prim_paths = _env_camera_prim_paths(spec.camera_path_relative_to_env_0, spec.num_instances) for name in _CAMERA_INTRINSIC_ATTRIBUTES: binding = self.backend.renderer.bind_attribute( - prim_paths=intrinsic_prim_paths, + prim_paths=camera_paths, attribute_name=name, dtype="float32", prim_mode=PrimMode.EXISTING_ONLY, @@ -1002,7 +1002,9 @@ def create_render_data(self, spec: CameraRenderSpec) -> OVRTXCameraRenderData: def _register_camera(self, spec: CameraRenderSpec, render_data: OVRTXCameraRenderData) -> None: """Add another tiled product and camera binding without reloading the shared scene.""" - camera_paths = _env_camera_prim_paths(spec.camera_path_relative_to_env_0, spec.num_instances) + camera_paths = _get_cloned_camera_paths(spec.camera_prim_paths[0], spec.num_instances) + if not camera_paths: + raise ValueError("OVRTX cameras must be under /World/envs/env_0/.") scope = render_data.render_scope_name product_path = render_data.render_product_path usd = build_render_product_as_string( @@ -1944,7 +1946,7 @@ def _initialize_camera_render_data_from_spec_ovstage( first_cam_path = spec.camera_prim_paths[0] if not first_cam_path.startswith(env_0_prefix): raise RuntimeError(f"Expected camera prim under '{env_0_prefix}', got '{first_cam_path}'") - self._camera_rel_path = spec.camera_path_relative_to_env_0 + self._camera_prim_path = first_cam_path logger.info("Injecting camera definitions...") @@ -1987,7 +1989,7 @@ def _initialize_camera_render_data_from_spec_ovstage( self._initialized_scene = True - camera_paths = _env_camera_prim_paths(self._camera_rel_path, num_envs) + camera_paths = _get_cloned_camera_paths(self._camera_prim_path, num_envs) # Re-author the RenderProduct's camera relationship after clone. ``stage.clone`` recreates the per-env # cameras, so the RenderProduct must be pointed at the freshly-interned camera path ids to discover every @@ -2088,7 +2090,7 @@ def _update_scene_partitions_after_clone_ovstage(self, num_envs: int): """Update scene partition attributes on cloned environments and cameras (ovstage path).""" logger.info("Writing scene partitions for %d environments...", num_envs) env_prim_paths = [f"/World/envs/env_{i}" for i in range(num_envs)] - camera_prim_paths = _env_camera_prim_paths(self._camera_rel_path, num_envs) + camera_prim_paths = _get_cloned_camera_paths(self._camera_prim_path, num_envs) # TOKEN_ID semantic tells ovstage the uint64 values are interned string tokens, not raw integers; # the renderer resolves them back to the original "env_N" strings for scene-partition lookup. token_ids = np.array([self.backend.paths.intern_token(f"env_{i}") for i in range(num_envs)], dtype=np.uint64) @@ -2145,8 +2147,9 @@ def _setup_xform_bindings_ovstage(self) -> None: object_paths = [] newton_indices = [] + camera_rel_path = self._camera_prim_path.removeprefix("/World/envs/env_0/") for idx, path in enumerate(all_body_paths): - if "/World/envs/" in path and self._camera_rel_path not in path and "GroundPlane" not in path: + if "/World/envs/" in path and camera_rel_path not in path and "GroundPlane" not in path: object_paths.append(path) newton_indices.append(idx) diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py index b641dc047551..c5261f638ae7 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py @@ -208,7 +208,7 @@ def build_render_scope_usd( if spec.cfg.isp_cfg is not None and "rgb_hdr" not in data_types: data_types.append("rgb_hdr") tiled_width, tiled_height = _tiled_resolution(spec.num_instances, spec.cfg.width, spec.cfg.height) - camera_path = f"/World/envs/env_0/{spec.camera_path_relative_to_env_0}" + camera_path = spec.camera_prim_paths[0] render_var_configs = get_render_var_configs(data_types, render_data.render_scope_name) minimal_mode = next( (_RTX_MINIMAL_MODES[data_type] for data_type in data_types if data_type in _RTX_MINIMAL_MODES), None diff --git a/source/isaaclab_ov/test/test_ovrtx_clone_plan.py b/source/isaaclab_ov/test/test_ovrtx_clone_plan.py index 28c8e2a722e9..bb4b5a9ded6a 100644 --- a/source/isaaclab_ov/test/test_ovrtx_clone_plan.py +++ b/source/isaaclab_ov/test/test_ovrtx_clone_plan.py @@ -114,7 +114,7 @@ def _make_ovrtx_renderer_without_backend() -> OVRTXRenderer: renderer._device = "cuda:0" # __init__'s default, replaced by create_render_data(spec) # create_render_data resolves this from the spec; tests that bypass it get the default. renderer._warp_device = SimpleNamespace(ordinal=0) - renderer._camera_rel_path = "Camera" + renderer._camera_prim_path = "/World/envs/env_0/Camera" renderer._render_product_paths = [] renderer._camera_render_data = [] renderer._next_camera_id = 0 @@ -147,7 +147,6 @@ def _make_camera_render_spec(num_envs: int = 1, device: str = "cpu") -> CameraRe num_instances=num_envs, camera_prim_paths=camera_paths, view_count=num_envs, - camera_path_relative_to_env_0="Camera", ) diff --git a/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py b/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py index fafa64992b2c..de45923463ef 100644 --- a/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py +++ b/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py @@ -97,7 +97,7 @@ def _make_renderer_without_backend(device: str = "cpu") -> tuple[OVRTXRenderer, renderer.cfg = OVRTXRendererCfg() renderer.backend = SimpleNamespace() renderer._device = device - renderer._camera_rel_path = "Camera" + renderer._camera_prim_path = "/World/envs/env_0/Camera" renderer._clone_plan = None renderer.backend.renderer = _FakeOVRTXBackend() renderer._deformable_points_binding = None diff --git a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py index b28cb582a637..166c78857870 100644 --- a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py +++ b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py @@ -257,7 +257,6 @@ def check_depth(rd, data, expected, label): num_instances=2, camera_prim_paths=tuple(f"/World/envs/env_{i}/cam{index}" for i in range(2)), view_count=2, - camera_path_relative_to_env_0=f"cam{index}", ) rd = renderer.create_render_data(spec) data = CameraData.allocate( @@ -455,7 +454,6 @@ def fake_map(self, render_var): device="cpu", num_instances=2, camera_prim_paths=[f"/World/envs/env_{i}/cam{camera_id}" for i in range(2)], - camera_path_relative_to_env_0=f"cam{camera_id}", ) ) stage = stages[render_data.render_product_path] @@ -785,6 +783,36 @@ def test_ovrtx_cleanup_without_render_data_keeps_renderer_state(): assert renderer._initialized_scene is True +@pytest.mark.parametrize( + "camera_path", + [ + "/World/Camera", + "/World/envs/env_1/Camera", + "/World/envs/env_00/Camera", + "/World/envs/env_0", + "/World/envs/env_0/", + ], +) +def test_create_render_data_rejects_cameras_outside_source_environment(camera_path): + """Camera registration requires a source camera beneath env_0 before touching the backend.""" + from isaaclab.renderers.camera_render_spec import CameraRenderSpec + + renderer = _make_ovrtx_renderer_without_backend() + renderer.backend.renderer = MagicMock() + spec = CameraRenderSpec( + cfg=_make_camera_cfg(["depth"]), + device="cpu", + num_instances=2, + camera_prim_paths=(camera_path,), + view_count=2, + ) + + with pytest.raises(ValueError, match="/World/envs/env_0/"): + renderer.create_render_data(spec) + + assert not renderer.backend.renderer.mock_calls + + @pytest.mark.parametrize("use_ovstage", [False, True]) def test_intrinsic_updates_target_the_given_camera(monkeypatch, use_ovstage): """Cameras sharing a renderer must bind and update distinct native camera paths.""" @@ -811,7 +839,6 @@ def test_intrinsic_updates_target_the_given_camera(monkeypatch, use_ovstage): device="cpu", num_instances=2, camera_prim_paths=camera_paths, - camera_path_relative_to_env_0=camera_paths[0].rsplit("/", 1)[1], ) ) for camera_paths in paths @@ -869,7 +896,6 @@ def test_registered_camera_expands_env_0_prototype_to_every_env(monkeypatch, use device="cpu", num_instances=3, camera_prim_paths=(f"/World/envs/env_0/{relative_path}",), - camera_path_relative_to_env_0=relative_path, ) ) expected_paths = [f"/World/envs/env_{i}/{relative_path}" for i in range(3)] diff --git a/source/isaaclab_ov/test/test_ovrtx_usd.py b/source/isaaclab_ov/test/test_ovrtx_usd.py index dbad2eb43664..bd04c7b0f50e 100644 --- a/source/isaaclab_ov/test/test_ovrtx_usd.py +++ b/source/isaaclab_ov/test/test_ovrtx_usd.py @@ -67,7 +67,6 @@ def camera_spec(): num_instances=4, camera_prim_paths=tuple(f"/World/envs/env_{i}/Robot/head_cam" for i in range(4)), view_count=4, - camera_path_relative_to_env_0="Robot/head_cam", ) From ddc6ec4b4fec7e321b188bdb89dc991e611580b5 Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Wed, 23 Sep 2026 17:28:58 -0700 Subject: [PATCH 02/11] [Core] Refactor and cleanup core subpackage for 3.0 [3/N] (#7979) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description Third PR in a series that splits the core cleanup in #7949 into small, reviewable pieces. This one fixes five small bugs in `isaaclab.utils` and the camera utilities. Each fix has a regression test that fails without it. **Fixes** - `utils.math.unproject_depth`: homogeneous pixel coordinates were built as `(1, u, v)` instead of `(u, v, 1)` (`pad(..., (0, 0, 1, 0))` instead of `(0, 0, 0, 1)`), so every unprojected point was wrong and the first image row became `inf`/`nan`. This regressed in #4437 (quaternion convention change), where the padding was flipped together with the quaternion code. It also affects `create_pointcloud_from_depth` / `create_pointcloud_from_rgbd` and the `run_usd_camera.py` / `run_ray_caster_camera.py` tutorials. This fix is in its own commit. - `sensors.camera.utils.create_pointcloud_from_rgbd`: a color tuple or `rgb=None` called `torch.Tensor(data, device=..., dtype=...)`, which always raises `TypeError`. The uniform color is now built with `torch.tensor(...).repeat(...)` on the point cloud's device. The comment and docstring claiming the default color is white are corrected (it is black). - `utils.math.quat_slerp`: `q2 *= -1.0` negated the caller's tensor in place when taking the shorter arc; it is now `q2 = -q2`. - `utils.sensors.convert_camera_intrinsics_to_usd`: `abs()` was applied to the boolean `(c_x - w/2) > 1e-4 or ...`, so a principal point left of or above the image center never triggered the aperture-offset warning. - `utils.datasets.HDF5DatasetFileHandler.create`: a bare file name gave `os.path.dirname(...) == ""` and `os.makedirs("")` raised. The directory is now only created when there is one. **Tests** (focused checks, no new test files; together they run in about 2.5 s). The aperture-offset warning fix is a one-line `abs()` correction and is not covered by a dedicated test. - `test/utils/test_math.py`: new `test_unproject_depth` (compares against the pinhole model); the existing `test_quat_slerp` now also asserts that `q2` is left unchanged. - `test/utils/test_hdf5_dataset_file_handler.py`: `test_create_dataset_file` also covers a bare file name. - `test/sensors/test_opencv_distortion.py` (kit-less): `test_pointcloud_from_rgbd_uniform_color` (color tuple and `None`). All of these fail against `develop` sources and pass with the fixes; the three touched test files pass in full. ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` ## Type of change - Bug fix (non-breaking change which fixes an issue) ## Checklist - [x] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added a changelog fragment under `source//changelog.d/` for every touched package (do **not** edit `CHANGELOG.rst` or bump `extension.toml` — CI handles that) - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --- .../changelog.d/core-cleanup-utils-fixes.rst | 15 +++++++++++ .../isaaclab/isaaclab/sensors/camera/utils.py | 10 +++---- .../datasets/hdf5_dataset_file_handler.py | 4 +-- source/isaaclab/isaaclab/utils/math.py | 4 +-- source/isaaclab/isaaclab/utils/sensors.py | 2 +- .../test/sensors/test_opencv_distortion.py | 12 +++++++++ .../utils/test_hdf5_dataset_file_handler.py | 12 ++++++++- source/isaaclab/test/utils/test_math.py | 27 ++++++++++++++++++- 8 files changed, 74 insertions(+), 12 deletions(-) create mode 100644 source/isaaclab/changelog.d/core-cleanup-utils-fixes.rst diff --git a/source/isaaclab/changelog.d/core-cleanup-utils-fixes.rst b/source/isaaclab/changelog.d/core-cleanup-utils-fixes.rst new file mode 100644 index 000000000000..bcb79400ea28 --- /dev/null +++ b/source/isaaclab/changelog.d/core-cleanup-utils-fixes.rst @@ -0,0 +1,15 @@ +Fixed +^^^^^ + +* Fixed :func:`~isaaclab.utils.math.unproject_depth` building homogeneous pixel coordinates as + ``(1, u, v)`` instead of ``(u, v, 1)``, which returned wrong points and turned the first image row + into ``inf``/``nan``. This also affected :func:`~isaaclab.sensors.camera.utils.create_pointcloud_from_depth` + and :func:`~isaaclab.sensors.camera.utils.create_pointcloud_from_rgbd`. +* Fixed :func:`~isaaclab.sensors.camera.utils.create_pointcloud_from_rgbd` raising a ``TypeError`` when + ``rgb`` is a color tuple or ``None``. +* Fixed :func:`~isaaclab.utils.math.quat_slerp` negating the caller's ``q2`` tensor in place when + taking the shorter arc. +* Fixed :func:`~isaaclab.utils.sensors.convert_camera_intrinsics_to_usd` not warning about aperture + offsets when the principal point is left of or above the image center. +* Fixed :meth:`~isaaclab.utils.datasets.HDF5DatasetFileHandler.create` failing when ``file_path`` is a + bare file name without a directory. diff --git a/source/isaaclab/isaaclab/sensors/camera/utils.py b/source/isaaclab/isaaclab/sensors/camera/utils.py index 5870f4743282..548ac47035fc 100644 --- a/source/isaaclab/isaaclab/sensors/camera/utils.py +++ b/source/isaaclab/isaaclab/sensors/camera/utils.py @@ -173,7 +173,7 @@ def create_pointcloud_from_rgbd( - If a ``np.array``/``wp.array``/``torch.tensor`` of shape (H, W, 3), then the corresponding channels encode the RGB values. - If a tuple, then the point cloud has a single color specified by the values (r, g, b). - - If None, then default color is white, i.e. (0, 0, 0). + - If None, then default color is black, i.e. (0, 0, 0). If the input ``normalize_rgb`` is set to :obj:`True`, then the RGB values are normalized to be in the range [0, 1]. @@ -232,12 +232,12 @@ def create_pointcloud_from_rgbd( points_rgb = rgb.permute(1, 0, 2).reshape(-1, 3) elif isinstance(rgb, (tuple, list)): # same color for all points - points_rgb = torch.Tensor((rgb,) * num_points, device=device, dtype=torch.uint8) + points_rgb = torch.tensor(rgb, device=points_xyz.device, dtype=torch.uint8).repeat(num_points, 1) else: - # default color is white - points_rgb = torch.Tensor(((0, 0, 0),) * num_points, device=device, dtype=torch.uint8) + # default color is black + points_rgb = torch.zeros((num_points, 3), device=points_xyz.device, dtype=torch.uint8) else: - points_rgb = torch.Tensor(((0, 0, 0),) * num_points, device=device, dtype=torch.uint8) + points_rgb = torch.zeros((num_points, 3), device=points_xyz.device, dtype=torch.uint8) # normalize color values if normalize_rgb: points_rgb = points_rgb.float() / 255 diff --git a/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py b/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py index d2963c94c593..c682e56346c4 100644 --- a/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py +++ b/source/isaaclab/isaaclab/utils/datasets/hdf5_dataset_file_handler.py @@ -69,8 +69,8 @@ def create(self, file_path: str, env_name: str = None): if not file_path.endswith(".hdf5"): file_path += ".hdf5" dir_path = os.path.dirname(file_path) - if not os.path.isdir(dir_path): - os.makedirs(dir_path) + if dir_path: + os.makedirs(dir_path, exist_ok=True) self._hdf5_file_stream = h5py.File(file_path, "w") # Set the dataset format version diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index 851bde397c76..1c298a9d856c 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -1238,7 +1238,7 @@ def unproject_depth(depth: torch.Tensor, intrinsics: torch.Tensor, is_ortho: boo indices_u = torch.arange(im_width, device=depth.device, dtype=depth.dtype) indices_v = torch.arange(im_height, device=depth.device, dtype=depth.dtype) img_indices = torch.stack(torch.meshgrid([indices_u, indices_v], indexing="ij"), dim=0).reshape(2, -1) - pixels = torch.nn.functional.pad(img_indices, (0, 0, 1, 0), mode="constant", value=1.0) + pixels = torch.nn.functional.pad(img_indices, (0, 0, 0, 1), mode="constant", value=1.0) pixels = pixels.unsqueeze(0) # (3, H x W) -> (1, 3, H x W) # unproject points into 3D space @@ -1789,7 +1789,7 @@ def quat_slerp(q1: torch.Tensor, q2: torch.Tensor, tau: float) -> torch.Tensor: if d < 0.0: # Invert rotation d = -d - q2 *= -1.0 + q2 = -q2 angle = torch.acos(torch.clamp(d, -1, 1)) if abs(angle) < torch.finfo(q1.dtype).eps * 4.0: return q1 diff --git a/source/isaaclab/isaaclab/utils/sensors.py b/source/isaaclab/isaaclab/utils/sensors.py index ce5d193dde7e..064a4cf47d02 100644 --- a/source/isaaclab/isaaclab/utils/sensors.py +++ b/source/isaaclab/isaaclab/utils/sensors.py @@ -36,7 +36,7 @@ def convert_camera_intrinsics_to_usd( logger.warning("Camera non square pixels are not supported by Omniverse. The average of f_x and f_y are used.") # warn about aperture offsets - if abs((c_x - float(width) / 2) > 1e-4 or (c_y - float(height) / 2) > 1e-4): + if abs(c_x - float(width) / 2) > 1e-4 or abs(c_y - float(height) / 2) > 1e-4: logger.warning( "Camera aperture offsets are not supported by Omniverse. c_x and c_y will be half of width and height" ) diff --git a/source/isaaclab/test/sensors/test_opencv_distortion.py b/source/isaaclab/test/sensors/test_opencv_distortion.py index 137cc4f2666f..275525088ca2 100644 --- a/source/isaaclab/test/sensors/test_opencv_distortion.py +++ b/source/isaaclab/test/sensors/test_opencv_distortion.py @@ -48,6 +48,7 @@ import isaaclab.sim as sim_utils from isaaclab.sensors.camera.camera import Camera, _camera_select_intrinsics_kernel, _camera_set_intrinsics_kernel from isaaclab.sensors.camera.camera_data import CameraData + from isaaclab.sensors.camera.utils import create_pointcloud_from_rgbd from isaaclab.sim.spawners.sensors.sensors import spawn_camera from isaaclab.sim.spawners.sensors.sensors_cfg import ( FisheyeCameraCfg, @@ -304,6 +305,17 @@ def test_readback_distinct_image_size_mismatches_each_warn(): assert any("(1280, 720)" in message for message in mismatch_warnings) +def test_pointcloud_from_rgbd_uniform_color(): + """A color tuple, or no color, gives every point the same color.""" + depth = torch.ones(4, 5) + intrinsics = torch.tensor([[20.0, 0.0, 2.5], [0.0, 20.0, 2.0], [0.0, 0.0, 1.0]]) + + for rgb, color in [((255, 0, 128), (255, 0, 128)), (None, (0, 0, 0))]: + points_xyz, points_rgb = create_pointcloud_from_rgbd(intrinsics, depth, rgb=rgb) + expected = torch.tensor(color, dtype=torch.uint8).expand(points_xyz.shape[0], 3) + torch.testing.assert_close(points_rgb, expected) + + @pytest.mark.parametrize("device", ["cpu", "cuda:0"]) def test_set_intrinsic_matrices_skips_only_distortion_cameras_in_batch(device): """In a mixed batch only the distortion camera is skipped (with a warning); a plain camera is updated. diff --git a/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py b/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py index c6117660480f..3cfa7a016d41 100644 --- a/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py +++ b/source/isaaclab/test/utils/test_hdf5_dataset_file_handler.py @@ -46,7 +46,7 @@ def temp_dir(): shutil.rmtree(temp_dir) -def test_create_dataset_file(temp_dir): +def test_create_dataset_file(temp_dir, monkeypatch): """Test creating a new dataset file.""" # create a dataset file given a file name with extension dataset_file_path = os.path.join(temp_dir, f"{uuid.uuid4()}.hdf5") @@ -66,6 +66,16 @@ def test_create_dataset_file(temp_dir): # check if the dataset is created assert os.path.exists(dataset_file_path + ".hdf5") + # create a dataset file given a bare file name in the current working directory + monkeypatch.chdir(temp_dir) + dataset_file_name = f"{uuid.uuid4()}.hdf5" + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.create(dataset_file_name, "test_env_name") + dataset_file_handler.close() + + # check if the dataset is created + assert os.path.exists(os.path.join(temp_dir, dataset_file_name)) + def test_add_env_args_preserves_existing_args_after_reopen(temp_dir): """Test extending environment arguments after reopening a dataset.""" diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index 4070c6da1140..7aaefb69b627 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -518,6 +518,28 @@ def test_orthogonalize_perspective_depth(device): torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth) +@pytest.mark.parametrize("device", test_devices()) +def test_unproject_depth(device): + """Test unproject_depth against the pinhole camera model, including the first image row and column.""" + height, width = 3, 4 + fx, fy, cx, cy = 50.0, 40.0, 1.5, 1.0 + depth = torch.rand(height, width, device=device) + 0.5 + intrinsics = torch.tensor([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], device=device) + + points = math_utils.unproject_depth(depth, intrinsics) + + # points are ordered column by column: index = u * height + v + expected = torch.tensor( + [ + [(u - cx) * depth[v, u].item() / fx, (v - cy) * depth[v, u].item() / fy, depth[v, u].item()] + for u in range(width) + for v in range(height) + ], + device=device, + ) + torch.testing.assert_close(points, expected) + + @pytest.mark.parametrize("device", test_devices()) def test_combine_frame_transform(device): """Test combine_frame_transforms function.""" @@ -870,11 +892,14 @@ def test_quat_slerp(device): key_times = [0, 1] slerp = scipy_tf.Slerp(key_times, key_rots) + q2_tensor = torch.tensor(q2, device=device) for tau in tau_values: expected = slerp(tau).as_quat() # (x, y, z, w) - result = math_utils.quat_slerp(torch.tensor(q1, device=device), torch.tensor(q2, device=device), tau) + result = math_utils.quat_slerp(torch.tensor(q1, device=device), q2_tensor, tau) # Assert that the result is almost equal to the expected quaternion np.testing.assert_array_almost_equal(result.cpu(), expected, decimal=DECIMAL_PRECISION) + # the input quaternion is not modified when interpolating along the shorter arc + np.testing.assert_array_equal(q2_tensor.cpu().numpy(), q2) @pytest.mark.parametrize("device", test_devices()) From 2fbc47049a80db08dd770688dc7a04a262f97845 Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Wed, 23 Sep 2026 18:22:41 -0700 Subject: [PATCH 03/11] [Multigpu] Support --check_success in multi-GPU training benchmarks (#7982) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Sum each rank's success samples across the process group at every iteration boundary so all ranks record the same global history and stop at the same iteration. Remove the validation that rejected --check_success for distributed runs. # Description Please include a summary of the change and which issue is fixed. Please also include relevant motivation and context. List any dependencies that are required for this change. Fixes # (issue) ## Type of change - Bug fix (non-breaking change which fixes an issue) - New feature (non-breaking change which adds functionality) - Breaking change (existing functionality will not work without user modification) - Documentation update ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` ## Screenshots Please attach before and after screenshots of the change if applicable. ## Checklist Docker and GPU tests run on demand. Push the commits you want tested, then comment `run-ci` on the pull request. - [ ] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [ ] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works - [ ] I have added a changelog fragment under `source//changelog.d/` for every touched package (do **not** edit `CHANGELOG.rst` or bump `extension.toml` — CI handles that) - [ ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --- docs/source/features/multi_gpu.rst | 8 +++++--- scripts/benchmarks/test/test_early_stop.py | 2 ++ .../multigpu-benchmark-check-success.rst | 6 ++++++ .../isaaclab/benchmark/distributed.py | 7 +++---- .../benchmark/entrypoints/early_stop.py | 3 +++ source/isaaclab/isaaclab/benchmark/metrics.py | 20 +++++++++++++++++++ .../test/benchmark/test_distributed.py | 12 +++++++++-- .../isaaclab/test/benchmark/test_metrics.py | 2 ++ 8 files changed, 51 insertions(+), 9 deletions(-) create mode 100644 source/isaaclab/changelog.d/multigpu-benchmark-check-success.rst diff --git a/docs/source/features/multi_gpu.rst b/docs/source/features/multi_gpu.rst index 809a8d266879..12b39e65a2b8 100644 --- a/docs/source/features/multi_gpu.rst +++ b/docs/source/features/multi_gpu.rst @@ -153,9 +153,11 @@ Run each benchmark with the same launcher options used by ``train_multigpu``: --task Isaac-Cartpole --num_envs 4096 --max_iterations 100 ``training_multigpu`` supports RSL-RL, RL-Games, and skrl with PyTorch. It does -not support skrl with JAX or SB3. It also rejects ``--video``, -``--capture_env_sensors``, and ``--check_success``, which do not produce a -meaningful aggregate result across ranks. +not support skrl with JAX or SB3. It also rejects ``--video`` and +``--capture_env_sensors``, which do not produce a meaningful aggregate result +across ranks. ``--check_success`` is supported with RSL-RL and RL-Games: the +success metric is averaged over the environments of all ranks, so every rank +stops at the same iteration. For multi-node benchmarks, pass the same ``--nnodes``, ``--node_rank``, and rendezvous options described in :ref:`multi-node-training` on every node. diff --git a/scripts/benchmarks/test/test_early_stop.py b/scripts/benchmarks/test/test_early_stop.py index feb63dca8ade..82d5d1dec82b 100644 --- a/scripts/benchmarks/test/test_early_stop.py +++ b/scripts/benchmarks/test/test_early_stop.py @@ -40,6 +40,7 @@ def stop_logging_writer(self): class _FakeRunner: def __init__(self, has_writer: bool = True): self.logger = _FakeLogger(has_writer=has_writer) + self.device = "cpu" self.current_learning_iteration = 7 self.saved: list[str] = [] @@ -96,6 +97,7 @@ class _FakeAlgo: def __init__(self, horizon_length: int | None = None, config_horizon: int | None = 16, epoch_num: int = 0): self.max_epochs = 999 self.epoch_num = epoch_num + self.ppo_device = "cpu" if horizon_length is not None: self.horizon_length = horizon_length self.config = {"horizon_length": config_horizon} if config_horizon is not None else {} diff --git a/source/isaaclab/changelog.d/multigpu-benchmark-check-success.rst b/source/isaaclab/changelog.d/multigpu-benchmark-check-success.rst new file mode 100644 index 000000000000..6e1be8500275 --- /dev/null +++ b/source/isaaclab/changelog.d/multigpu-benchmark-check-success.rst @@ -0,0 +1,6 @@ +Changed +^^^^^^^ + +* Changed ``isaaclab benchmark training_multigpu`` to accept ``--check_success`` for RSL-RL and RL-Games. + The success metric is now summed across ranks before each convergence check, so every rank stops at + the same iteration. diff --git a/source/isaaclab/isaaclab/benchmark/distributed.py b/source/isaaclab/isaaclab/benchmark/distributed.py index ddf434a30e6a..6289bc561503 100644 --- a/source/isaaclab/isaaclab/benchmark/distributed.py +++ b/source/isaaclab/isaaclab/benchmark/distributed.py @@ -172,8 +172,9 @@ def add_distributed_arg(parser: argparse.ArgumentParser) -> None: def validate_distributed_args(parser: argparse.ArgumentParser, args_cli: argparse.Namespace) -> None: """Reject training benchmark options that a distributed run cannot honor. - Recording video, capturing sensor frames, and stopping early on success all act on one rank's - environments, which neither describes the job nor stays in step with the other ranks. + Recording video and capturing sensor frames act on one rank's environments, which does not + describe the job. Success-based early stopping is allowed because the success metric is summed + across ranks before every convergence check, so all ranks stop together. Args: parser: Parser used to report the error. @@ -185,5 +186,3 @@ def validate_distributed_args(parser: argparse.ArgumentParser, args_cli: argpars parser.error("Video recording is not supported by multi-GPU training benchmarks.") if getattr(args_cli, "capture_env_sensors", 0) > 0: parser.error("Environment sensor capture is not supported by multi-GPU training benchmarks.") - if getattr(args_cli, "check_success", False): - parser.error("Success-based early stopping is not supported by multi-GPU training benchmarks.") diff --git a/source/isaaclab/isaaclab/benchmark/entrypoints/early_stop.py b/source/isaaclab/isaaclab/benchmark/entrypoints/early_stop.py index 2b20f9e0a78c..f20af8eb8459 100644 --- a/source/isaaclab/isaaclab/benchmark/entrypoints/early_stop.py +++ b/source/isaaclab/isaaclab/benchmark/entrypoints/early_stop.py @@ -82,6 +82,8 @@ def _step(self, actions) -> tuple: result = self._orig_step(actions) self.tracker.record_step(result[3]) # rsl_rl: (obs, rew, dones, extras) if self.tracker.at_iteration_boundary: + # Ranks must agree on convergence, or a rank that stops alone deadlocks the others. + self.tracker.all_reduce_iteration(self.runner.device) self.tracker.end_iteration() if self.stop_on_convergence and self.tracker.converged: raise EarlyStopConverged() @@ -185,6 +187,7 @@ def after_steps(self) -> None: self._base.after_steps() if self.tracker is None: return + self.tracker.all_reduce_iteration(self.algo.ppo_device) self.tracker.end_iteration() if self.stop_on_convergence and self.tracker.converged and self.algo is not None: print( diff --git a/source/isaaclab/isaaclab/benchmark/metrics.py b/source/isaaclab/isaaclab/benchmark/metrics.py index 677819d18f41..8af8012ac891 100644 --- a/source/isaaclab/isaaclab/benchmark/metrics.py +++ b/source/isaaclab/isaaclab/benchmark/metrics.py @@ -15,6 +15,8 @@ from collections.abc import Sequence from dataclasses import dataclass +import torch +import torch.distributed as dist from tensorboard.backend.event_processing import event_accumulator from .schema import Framework, MeanStd @@ -191,6 +193,24 @@ def record_step(self, extras: dict) -> None: self._iter_count += 1 self._step_count += 1 + def all_reduce_iteration(self, device: str) -> None: + """Sum the current iteration's success samples across distributed ranks. + + Every rank must call this at the same iteration boundary, before :meth:`end_iteration`, so + all ranks record the same global history and reach the same convergence decision. Does + nothing when no :mod:`torch.distributed` process group is initialized. + + Args: + device: Device of the tensor exchanged by the collective, which must match the + process-group backend (a CUDA device for NCCL). + """ + if not (dist.is_available() and dist.is_initialized()): + return + stats = torch.tensor([self._iter_sum, float(self._iter_count)], dtype=torch.float64, device=device) + dist.all_reduce(stats, op=dist.ReduceOp.SUM) + self._iter_sum = stats[0].item() + self._iter_count = int(stats[1].item()) + def end_iteration(self) -> float | None: """Finalize the current iteration. Returns mean metric, or ``None`` if no data.""" if self._iter_count == 0: diff --git a/source/isaaclab/test/benchmark/test_distributed.py b/source/isaaclab/test/benchmark/test_distributed.py index f68db73104f2..e42d855ed435 100644 --- a/source/isaaclab/test/benchmark/test_distributed.py +++ b/source/isaaclab/test/benchmark/test_distributed.py @@ -98,7 +98,6 @@ def _parser() -> argparse.ArgumentParser: [ (["--distributed", "--video"], "Video recording"), (["--distributed", "--capture_env_sensors", "1"], "sensor capture"), - (["--distributed", "--check_success"], "early stopping"), ], ) def test_unsupported_options_are_rejected_for_distributed_runs( @@ -114,7 +113,16 @@ def test_unsupported_options_are_rejected_for_distributed_runs( assert message in capsys.readouterr().err -@pytest.mark.parametrize("argv", [[], ["--video"], ["--distributed"], ["--distributed", "--capture_env_sensors", "0"]]) +@pytest.mark.parametrize( + "argv", + [ + [], + ["--video"], + ["--distributed"], + ["--distributed", "--capture_env_sensors", "0"], + ["--distributed", "--check_success"], + ], +) def test_supported_option_combinations_pass_validation(argv: list[str]): """Single-process runs keep every feature, and distributed runs keep the rank-safe ones.""" parser = _parser() diff --git a/source/isaaclab/test/benchmark/test_metrics.py b/source/isaaclab/test/benchmark/test_metrics.py index d3595bc3f089..f78d22cc2dd2 100644 --- a/source/isaaclab/test/benchmark/test_metrics.py +++ b/source/isaaclab/test/benchmark/test_metrics.py @@ -98,6 +98,8 @@ def test_success_rate_tracker_convergence(): t = SuccessRateTracker(threshold=0.5, window=2, num_steps_per_env=1) for v in (0.6, 0.7): t.record_step({"log": {"Metrics/success_rate": v}}) + # No process group is initialized, so the reduction leaves the local samples unchanged. + t.all_reduce_iteration("cpu") t.end_iteration() assert t.converged is True assert t.tail_mean == pytest.approx(0.65) From f7e528f3a780990d69459e6880111d3387c4965b Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Wed, 23 Sep 2026 18:29:13 -0700 Subject: [PATCH 04/11] [Core] Refactor and cleanup core subpackage for 3.0 [4/N] (#7980) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description Fourth PR in a series that splits the core cleanup in #7949 into small, reviewable pieces. This one fixes four small bugs in `isaaclab.terrains` and `isaaclab.sim`, and adds a spawner for meshes (e.g. terrain from an `.obj` file) ported from #5407 by @ooctipus (closed as stale). The PhysX `useEnvIds` and clone-spawner changes from that PR are not included. Each fix has a regression test that fails without it. ## Fixes - `terrains.trimesh.mesh_terrains.repeated_objects_terrain`: the built-in `MeshRepeatedBoxesTerrainCfg`, `MeshRepeatedCylindersTerrainCfg`, and `MeshRepeatedPyramidsTerrainCfg` default `object_type` to strings such as `"{DIR}.utils:make_box"`, which configclass turns into a `ResolvableString` (a callable `str` subclass). The function checked `isinstance(str)` before `callable`, looked up `make_{DIR}.utils:make_box` in the module, got `None`, and raised `ValueError`. Callables are now checked first; plain names such as `"box"` still resolve to `make_box`. The error message now shows the configured value instead of `None`. - `sim.schemas.modify_articulation_root_properties`: with `fix_root_link` set, the existing fixed joint was searched for on the current stage instead of the `stage` argument. - `sim.utils.queries.find_global_fixed_joint_prim`: the signature accepts `Sdf.Path`, but `prim_path.startswith(...)` raised `AttributeError` for one. The path is converted to `str` first, like the sibling query functions. - `sim.converters.MeshConverter`: the prim name was taken from `basename.split(".")`, which raised `ValueError` for file names with more than one dot (e.g. `duck.v2.obj`). It now uses `os.path.splitext`; the existing invalid-identifier fallback turns `duck.v2` into `duck_v2`. The unused `mesh_file_format` variable is removed. ## Mesh spawner **Added** - `sim_utils.MeshFileCfg` / `sim_utils.spawn_from_mesh` spawn a mesh from: - a mesh file path (`.obj`, `.stl`, `.fbx`, ...), converted with `MeshConverter` and referenced, or - in-memory triangle data: `MeshFileCfg.TriangleMeshCfg(vertices, faces, vertex_colors)` or a `trimesh.Trimesh` through `MeshFileCfg.TrimeshObjectCfg`, authored as an Xform with a USD mesh at `{prim_path}/mesh`. - The mesh is visual-only by default. `collision_props`, `mesh_collision_props` (e.g. triangle mesh for terrain, convex hull for objects), `rigid_props`, `mass_props`, `visual_material`, and `physics_material` are optional and accept both the legacy cfgs and schema fragments, like the other spawners. ```python terrain = sim_utils.MeshFileCfg( mesh="/path/to/terrain.obj", collision_props=[sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True)], mesh_collision_props=[sim_utils.UsdPhysicsMeshCollisionCfg(mesh_approximation_name="none")], ) terrain.func("/World/ground", terrain) ``` **Changed** - `isaaclab.terrains.utils.create_prim_from_mesh` (used by `TerrainImporter.import_mesh`) now spawns through `MeshFileCfg`. I checked that the authored USD is byte-for-byte identical to `develop` for a colored terrain mesh with visual and physics materials. The only difference: its `translation`/`orientation` kwargs now apply to the root prim instead of the `mesh` child (same world pose). **Differences from #5407** - Mesh file paths reuse `MeshConverter` for collision, mass, and rigid body properties, so both legacy cfgs and fragments work. The in-memory path follows the same legacy-or-fragment routing as the mesh-primitive spawners. - Dropped the `.usd` branch (use `UsdFileCfg` for USD files), the duplicate "prim already exists" check (`create_prim` already raises), and the new public `utils.mesh` helpers; mesh-data validation is a private helper in the spawner. ## Tests - `test/sim/test_spawn_from_files.py`: `test_spawn_mesh_from_triangle_data` (colored rigid body with a convex-hull collider) and `test_spawn_mesh_from_obj_file`. - The trimesh terrain path is covered by the existing terrain importer, ray caster, and material fragment tests; `test_terrain_importer.py` passes in full. **Fix tests** (one focused check per fix, extending existing tests where possible) - `test/terrains/test_terrain_generator.py`: new `test_repeated_objects_default_object_type` (the three repeated-object configs share the code path, so one config is covered). - `test/sim/test_schemas.py`: new `test_modify_articulation_root_fix_root_link_uses_given_stage` (in-memory stage that is not the current stage). - `test/sim/test_utils_queries.py`: `test_find_global_fixed_joint_prim` also passes an `Sdf.Path`. - `test/sim/test_mesh_converter.py`: `test_convert_obj` now converts the OBJ from a dotted file name (`duck.v2.obj`) and checks the prim name, instead of adding a separate conversion. All of these fail against `develop` sources and pass with the fixes. ## Type of change - Bug fix (non-breaking change which fixes an issue) - New feature (non-breaking change which adds functionality) ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` ## Checklist - [x] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [x] I have made corresponding changes to the documentation - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added a changelog fragment under `source//changelog.d/` for every touched package (do **not** edit `CHANGELOG.rst` or bump `extension.toml` — CI handles that) - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --------- Co-authored-by: Octi Zhang --- docs/source/api/lab/isaaclab.sim.spawners.rst | 7 + .../core-cleanup-sim-terrain-fixes.minor.rst | 31 +++ source/isaaclab/isaaclab/sim/__init__.pyi | 4 + .../isaaclab/sim/converters/mesh_converter.py | 3 +- .../isaaclab/isaaclab/sim/schemas/schemas.py | 2 +- .../isaaclab/sim/spawners/__init__.pyi | 4 + .../sim/spawners/from_files/__init__.py | 1 + .../sim/spawners/from_files/__init__.pyi | 4 + .../sim/spawners/from_files/from_files.py | 188 ++++++++++++++++++ .../sim/spawners/from_files/from_files_cfg.py | 89 +++++++++ source/isaaclab/isaaclab/sim/utils/queries.py | 1 + .../terrains/trimesh/mesh_terrains.py | 11 +- source/isaaclab/isaaclab/terrains/utils.py | 61 ++---- .../isaaclab/test/sim/test_mesh_converter.py | 13 +- source/isaaclab/test/sim/test_schemas.py | 23 ++- .../test/sim/test_spawn_from_files.py | 48 +++++ .../isaaclab/test/sim/test_utils_queries.py | 3 +- .../test/terrains/test_terrain_generator.py | 21 +- 18 files changed, 451 insertions(+), 63 deletions(-) create mode 100644 source/isaaclab/changelog.d/core-cleanup-sim-terrain-fixes.minor.rst diff --git a/docs/source/api/lab/isaaclab.sim.spawners.rst b/docs/source/api/lab/isaaclab.sim.spawners.rst index 2c7bc5224e1d..80bf0323ab02 100644 --- a/docs/source/api/lab/isaaclab.sim.spawners.rst +++ b/docs/source/api/lab/isaaclab.sim.spawners.rst @@ -255,6 +255,7 @@ From Files UrdfFileCfg UsdFileCfg + MeshFileCfg GroundPlaneCfg .. autofunction:: spawn_from_urdf @@ -269,6 +270,12 @@ From Files :members: :exclude-members: __init__, func +.. autofunction:: spawn_from_mesh + +.. autoclass:: MeshFileCfg + :members: + :exclude-members: __init__, func + .. autofunction:: spawn_ground_plane .. autoclass:: GroundPlaneCfg diff --git a/source/isaaclab/changelog.d/core-cleanup-sim-terrain-fixes.minor.rst b/source/isaaclab/changelog.d/core-cleanup-sim-terrain-fixes.minor.rst new file mode 100644 index 000000000000..116644636ab2 --- /dev/null +++ b/source/isaaclab/changelog.d/core-cleanup-sim-terrain-fixes.minor.rst @@ -0,0 +1,31 @@ +Added +^^^^^ + +* Added :class:`~isaaclab.sim.MeshFileCfg` and :func:`~isaaclab.sim.spawn_from_mesh` to spawn a mesh from a + mesh file (e.g. ``.obj``, ``.stl``, ``.fbx``) or from in-memory triangle data + (:class:`~isaaclab.sim.MeshFileCfg.TriangleMeshCfg` or a :class:`trimesh.Trimesh` through + :class:`~isaaclab.sim.MeshFileCfg.TrimeshObjectCfg`), with optional collision, mesh collision + approximation, rigid body, mass, and material properties. + +Changed +^^^^^^^ + +* Changed :func:`~isaaclab.terrains.utils.create_prim_from_mesh` to spawn the terrain mesh with + :class:`~isaaclab.sim.MeshFileCfg`. The authored USD is unchanged, except that the ``translation`` and + ``orientation`` keyword arguments now apply to the root prim instead of its ``mesh`` child prim; the + resulting world pose is the same. + +Fixed +^^^^^ + +* Fixed the built-in :class:`~isaaclab.terrains.MeshRepeatedBoxesTerrainCfg`, + :class:`~isaaclab.terrains.MeshRepeatedCylindersTerrainCfg`, and + :class:`~isaaclab.terrains.MeshRepeatedPyramidsTerrainCfg` raising ``ValueError`` with their default + ``object_type``. The resolvable ``"module:function"`` default was looked up as ``make_`` + instead of being called. +* Fixed :func:`~isaaclab.sim.schemas.modify_articulation_root_properties` looking up the existing fixed + joint on the current stage instead of the ``stage`` argument when ``fix_root_link`` is set. +* Fixed :func:`~isaaclab.sim.utils.queries.find_global_fixed_joint_prim` raising ``AttributeError`` for an + :class:`pxr.Sdf.Path` argument. +* Fixed :class:`~isaaclab.sim.converters.MeshConverter` raising ``ValueError`` for mesh file names with + more than one dot. diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index 73f4e1f84cd6..ccca5d87bae5 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -92,11 +92,13 @@ __all__ = [ "RigidObjectSpawnerCfg", "DeformableObjectSpawnerCfg", "spawn_from_mjcf", + "spawn_from_mesh", "spawn_from_urdf", "spawn_from_usd", "spawn_from_usd_with_compliant_contact_material", "spawn_ground_plane", "GroundPlaneCfg", + "MeshFileCfg", "MjcfFileCfg", "UrdfFileCfg", "UsdFileCfg", @@ -348,6 +350,7 @@ from .spawners import ( MeshCylinderCfg, MeshRectangleCfg, MeshSphereCfg, + MeshFileCfg, MjcfFileCfg, MultiAssetSpawnerCfg, MultiUsdFileCfg, @@ -379,6 +382,7 @@ from .spawners import ( spawn_deformable_body_material, spawn_from_mdl_file, spawn_from_mjcf, + spawn_from_mesh, spawn_from_urdf, spawn_from_usd, spawn_from_usd_with_compliant_contact_material, diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py index dab9c5b30eaf..5ec51b2a5a55 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py @@ -83,8 +83,7 @@ def _convert_asset(self, cfg: MeshConverterCfg): RuntimeError: If the conversion using the Omniverse asset converter fails. """ # resolve mesh name and format - mesh_file_basename, mesh_file_format = os.path.basename(cfg.asset_path).split(".") - mesh_file_format = mesh_file_format.lower() + mesh_file_basename = os.path.splitext(os.path.basename(cfg.asset_path))[0] # Check if mesh_file_basename is a valid USD identifier if not Tf.IsValidIdentifier(mesh_file_basename): diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index b13fd91ac9ac..5f6cec87e3d6 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -533,7 +533,7 @@ def modify_articulation_root_properties( # we do the fixed joint processing later to not interfere with setting other properties if fix_root_link is not None: # check if a global fixed joint exists under the root prim - existing_fixed_joint_prim = find_global_fixed_joint_prim(prim_path) + existing_fixed_joint_prim = find_global_fixed_joint_prim(prim_path, stage=stage) # if we found a fixed joint, enable/disable it based on the input # otherwise, create a fixed joint between the world and the root link diff --git a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi index ac0a48b31945..339d1ef667ba 100644 --- a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi @@ -8,11 +8,13 @@ __all__ = [ "RigidObjectSpawnerCfg", "DeformableObjectSpawnerCfg", "spawn_from_mjcf", + "spawn_from_mesh", "spawn_from_urdf", "spawn_from_usd", "spawn_from_usd_with_compliant_contact_material", "spawn_ground_plane", "GroundPlaneCfg", + "MeshFileCfg", "MjcfFileCfg", "UrdfFileCfg", "UsdFileCfg", @@ -83,11 +85,13 @@ __all__ = [ from .spawner_cfg import SpawnerCfg, RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg from .from_files import ( spawn_from_mjcf, + spawn_from_mesh, spawn_from_urdf, spawn_from_usd, spawn_from_usd_with_compliant_contact_material, spawn_ground_plane, GroundPlaneCfg, + MeshFileCfg, MjcfFileCfg, UrdfFileCfg, UsdFileCfg, diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py index 5d953905821e..456c8a8230c9 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py @@ -9,6 +9,7 @@ * :class:`UsdFileCfg`: Spawn an asset from a USD file. * :class:`UrdfFileCfg`: Spawn an asset from a URDF file. +* :class:`MeshFileCfg`: Spawn a mesh from a mesh file or from in-memory triangle data. * :class:`GroundPlaneCfg`: Spawn a ground plane using the grid-world USD file. """ diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.pyi index 6003fa170fea..e853882f60ac 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.pyi @@ -5,11 +5,13 @@ __all__ = [ "spawn_from_mjcf", + "spawn_from_mesh", "spawn_from_urdf", "spawn_from_usd", "spawn_from_usd_with_compliant_contact_material", "spawn_ground_plane", "GroundPlaneCfg", + "MeshFileCfg", "MjcfFileCfg", "UrdfFileCfg", "UsdFileCfg", @@ -18,6 +20,7 @@ __all__ = [ from .from_files import ( spawn_from_mjcf, + spawn_from_mesh, spawn_from_urdf, spawn_from_usd, spawn_from_usd_with_compliant_contact_material, @@ -25,6 +28,7 @@ from .from_files import ( ) from .from_files_cfg import ( GroundPlaneCfg, + MeshFileCfg, MjcfFileCfg, UrdfFileCfg, UsdFileCfg, 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 2b67c2e44b4d..04a74addedaf 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -11,6 +11,7 @@ from contextlib import nullcontext from typing import TYPE_CHECKING +import numpy as np from filelock import FileLock from isaaclab.utils.assets import check_file_path, retrieve_file_path @@ -171,6 +172,66 @@ def spawn_from_mjcf( return _spawn_from_usd_file(prim_path, mjcf_loader.usd_path, cfg, translation, orientation) +@clone +def spawn_from_mesh( + prim_path: str, + cfg: from_files_cfg.MeshFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Spawn a mesh from a mesh file or from in-memory triangle data. + + A mesh file path is converted to USD with :class:`~isaaclab.sim.converters.MeshConverter`, which + also applies the collision, mass, and rigid body properties, and the result is referenced at + ``prim_path``. In-memory meshes are authored as an Xform at ``prim_path`` with a USD mesh prim at + ``{prim_path}/mesh``. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single configuration. + + Args: + prim_path: The prim path or pattern to spawn the mesh at. + cfg: The mesh spawner configuration. + translation: Translation of the mesh root [m] with respect to its parent prim. Defaults to None, + in which case the translation is not modified. + orientation: Orientation of the mesh root in (x, y, z, w) with respect to its parent prim. + Defaults to None, in which case the orientation is not modified. + **kwargs: Additional keyword arguments, unused and accepted for compatibility with other spawners. + + Returns: + The prim of the spawned mesh root. + + Raises: + ValueError: If the in-memory mesh data does not have the expected shapes. + """ + from . import from_files_cfg # noqa: PLC0415 + + if isinstance(cfg.mesh, str): + mesh_converter = converters.MeshConverter( + converters.MeshConverterCfg( + asset_path=cfg.mesh, + mass_props=cfg.mass_props, + rigid_props=cfg.rigid_props, + collision_props=cfg.collision_props, + mesh_collision_props=cfg.mesh_collision_props if cfg.collision_props is not None else None, + ) + ) + usd_cfg = from_files_cfg.UsdFileCfg( + usd_path=mesh_converter.usd_path, + scale=cfg.scale, + visual_material_path=cfg.visual_material_path, + visual_material=cfg.visual_material, + physics_material_path=cfg.physics_material_path, + physics_material=cfg.physics_material, + ) + return _spawn_from_usd_file(prim_path, mesh_converter.usd_path, usd_cfg, translation, orientation) + + return _spawn_mesh_data(prim_path, cfg, translation, orientation) + + def spawn_ground_plane( prim_path: str, cfg: from_files_cfg.GroundPlaneCfg, @@ -487,6 +548,133 @@ def _apply_articulation_schema_properties(prim_path: str, cfg: from_files_cfg.Fi schemas.modify_joint_drive_properties(prim_path, cfg.joint_drive_props) +def _triangle_mesh_arrays(mesh_source) -> tuple[np.ndarray, np.ndarray, np.ndarray | None]: + """Return the vertices, triangle faces, and RGBA vertex colors in ``[0, 1]`` of an in-memory mesh source.""" + from . import from_files_cfg # noqa: PLC0415 + + if isinstance(mesh_source, from_files_cfg.MeshFileCfg.TrimeshObjectCfg): + mesh = mesh_source.mesh + vertices, faces, colors = mesh.vertices, mesh.faces, mesh.visual.vertex_colors + elif isinstance(mesh_source, from_files_cfg.MeshFileCfg.TriangleMeshCfg): + vertices, faces, colors = mesh_source.vertices, mesh_source.faces, mesh_source.vertex_colors + else: + raise TypeError( + "Expected a mesh file path, MeshFileCfg.TriangleMeshCfg, or MeshFileCfg.TrimeshObjectCfg." + f" Received: {type(mesh_source).__name__}." + ) + + vertices = np.asarray(vertices, dtype=np.float32) + faces = np.asarray(faces, dtype=np.int64) + if vertices.ndim != 2 or vertices.shape[1] != 3: + raise ValueError(f"Expected mesh vertices with shape (num_vertices, 3). Received: {vertices.shape}.") + if faces.ndim != 2 or faces.shape[1] != 3: + raise ValueError(f"Expected triangle faces with shape (num_faces, 3). Received: {faces.shape}.") + if colors is None or len(colors) == 0: + return vertices, faces, None + + colors = np.asarray(colors, dtype=np.float32) + if colors.shape not in ((len(vertices), 3), (len(vertices), 4)): + raise ValueError( + f"Expected one RGB or RGBA color per vertex, shape ({len(vertices)}, 3 or 4). Received: {colors.shape}." + ) + if colors.max() > 1.0: + colors = colors / 255.0 + if colors.shape[1] == 3: + colors = np.hstack([colors, np.ones((len(colors), 1), dtype=np.float32)]) + return vertices, faces, colors + + +def _spawn_mesh_data( + prim_path: str, + cfg: from_files_cfg.MeshFileCfg, + translation: tuple[float, float, float] | None, + orientation: tuple[float, float, float, float] | None, +) -> Usd.Prim: + """Author an in-memory mesh as an Xform at ``prim_path`` with a mesh prim at ``{prim_path}/mesh``.""" + from pxr import Sdf, UsdGeom # noqa: PLC0415 + + stage = get_current_stage() + vertices, faces, colors = _triangle_mesh_arrays(cfg.mesh) + + root_prim = create_prim( + prim_path, "Xform", translation=translation, orientation=orientation, scale=cfg.scale, stage=stage + ) + mesh_prim_path = f"{prim_path}/mesh" + mesh_prim = create_prim( + mesh_prim_path, + "Mesh", + attributes={ + "points": vertices, + "faceVertexIndices": faces.flatten(), + "faceVertexCounts": np.full(len(faces), 3), + "subdivisionScheme": "bilinear", + }, + stage=stage, + ) + if colors is not None: + UsdGeom.PrimvarsAPI(mesh_prim).CreatePrimvar( + "displayColor", Sdf.ValueTypeNames.Color3fArray, UsdGeom.Tokens.vertex + ).Set(colors[:, :3]) + UsdGeom.PrimvarsAPI(mesh_prim).CreatePrimvar( + "displayOpacity", Sdf.ValueTypeNames.FloatArray, UsdGeom.Tokens.vertex + ).Set(colors[:, 3]) + + # collision properties anchor at the mesh prim, like the mesh converter + if cfg.collision_props is not None: + collision_props_mapping = fragment_mapping(cfg.collision_props) + if collision_props_mapping is not None: + for pattern, fragments in collision_props_mapping.items(): + schemas.apply_collision_properties( + props_expr(mesh_prim_path, pattern), fragments, create_if_missing=True, stage=stage + ) + else: + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props, stage=stage) + if cfg.mesh_collision_props is not None: + if bare_fragments(cfg.mesh_collision_props): + fragments = cfg.mesh_collision_props + if not isinstance(fragments, (list, tuple)): + fragments = [fragments] + schemas.apply_mesh_collision_properties(mesh_prim_path, fragments, stage=stage) + else: + schemas.define_mesh_collision_properties(mesh_prim_path, cfg.mesh_collision_props, stage=stage) + + if cfg.visual_material is not None: + material_path = _resolve_material_path(prim_path, cfg.visual_material_path) + cfg.visual_material.func(material_path, cfg.visual_material) + bind_visual_material(mesh_prim_path, material_path, stage=stage) + if cfg.physics_material is not None: + material_path = _resolve_material_path(prim_path, cfg.physics_material_path) + spawn_physics_material(material_path, cfg.physics_material, stage=stage) + bind_physics_material(mesh_prim_path, material_path, stage=stage) + + # mass and rigid body properties anchor at the root prim + if cfg.rigid_props is not None: + if cfg.mass_props is not None: + mass_props_mapping = fragment_mapping(cfg.mass_props) + if mass_props_mapping is not None: + for pattern, fragments in mass_props_mapping.items(): + schemas.apply_mass_properties( + props_expr(prim_path, pattern), fragments, create_if_missing=True, stage=stage + ) + else: + schemas.define_mass_properties(prim_path, cfg.mass_props, stage=stage) + rigid_props_mapping = fragment_mapping(cfg.rigid_props) + if rigid_props_mapping is not None: + for pattern, fragments in rigid_props_mapping.items(): + schemas.apply_rigid_body_properties( + props_expr(prim_path, pattern), fragments, create_if_missing=True, stage=stage + ) + else: + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props, stage=stage) + + return root_prim + + +def _resolve_material_path(prim_path: str, material_path: str) -> str: + """Resolve a material path relative to ``prim_path`` unless it is absolute.""" + return material_path if material_path.startswith("/") else f"{prim_path}/{material_path}" + + def _spawn_from_usd_file( prim_path: str, usd_path: str, diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py index 0946a8afc23c..1981a7d81495 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py @@ -7,6 +7,7 @@ from collections.abc import Callable from dataclasses import MISSING +from typing import TYPE_CHECKING from isaaclab.utils import configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR @@ -15,6 +16,10 @@ from .. import materials from ..spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg, SpawnerCfg +if TYPE_CHECKING: + import numpy as np + import trimesh + _DEFAULT_GROUND_PLANE_USD = ( f"{ISAACLAB_NUCLEUS_DIR}/Environments/Grid/default_ground_plane_checker_v1/default_ground_plane.usda" ) @@ -396,3 +401,87 @@ class GroundPlaneCfg(SpawnerCfg): :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialFragment`, or a list of such single-namespace fragments. """ + + +@configclass +class MeshFileCfg(RigidObjectSpawnerCfg): + """Spawn a mesh from a mesh file or from in-memory triangle data. + + A mesh file path (e.g. ``.obj``, ``.stl``, or ``.fbx``) is converted to USD with + :class:`~isaaclab.sim.converters.MeshConverter` and referenced. In-memory meshes are authored + directly as a USD mesh prim at ``{prim_path}/mesh``. + + The mesh is visual-only by default. Set :attr:`collision_props` to enable collision and + :attr:`mesh_collision_props` to choose the collision approximation, e.g. triangle mesh for + terrain or convex hull for dynamic objects. + """ + + @configclass + class TriangleMeshCfg: + """Triangle mesh data to author directly as a USD mesh prim.""" + + vertices: np.ndarray | list[tuple[float, float, float]] = MISSING + """Mesh vertices [m], shape ``(num_vertices, 3)``.""" + + faces: np.ndarray | list[tuple[int, int, int]] = MISSING + """Triangle vertex indices, shape ``(num_faces, 3)``.""" + + vertex_colors: np.ndarray | list[tuple[float, ...]] | None = None + """RGB or RGBA vertex colors in ``[0, 1]`` or ``[0, 255]``, shape ``(num_vertices, 3 or 4)``. + + Defaults to None, in which case no vertex colors are authored. + """ + + @configclass + class TrimeshObjectCfg: + """In-memory :class:`trimesh.Trimesh` to author directly as a USD mesh prim. + + Its vertex colors, if any, are authored as display colors. + """ + + mesh: trimesh.Trimesh = MISSING + """The triangle mesh.""" + + func: Callable | str = "{DIR}.from_files:spawn_from_mesh" + + mesh: str | TriangleMeshCfg | TrimeshObjectCfg = MISSING + """Mesh source to spawn: a mesh file path, :class:`TriangleMeshCfg`, or :class:`TrimeshObjectCfg`.""" + + scale: tuple[float, float, float] | None = None + """Scale of the mesh root prim. Defaults to None, in which case the scale is not modified.""" + + mesh_collision_props: ( + schemas.MeshCollisionBaseCfg | schemas.MeshCollisionFragment | list[schemas.MeshCollisionFragment] | None + ) = None + """Mesh collision approximation to apply to the mesh prim. Defaults to None. + + Only used when :attr:`collision_props` is set. Accepts the same values as + :attr:`~isaaclab.sim.converters.MeshConverterCfg.mesh_collision_props`. + """ + + visual_material_path: str = "visualMaterial" + """Path to the visual material to use for the mesh. Defaults to "visualMaterial". + + If the path is relative, then it will be relative to the prim's path. + """ + + visual_material: materials.VisualMaterialCfg | None = None + """Visual material properties. Defaults to None, in which case no visual material is added.""" + + physics_material_path: str = "physicsMaterial" + """Path to the physics material to use for the mesh. Defaults to "physicsMaterial". + + If the path is relative, then it will be relative to the prim's path. + """ + + physics_material: ( + materials.PhysicsMaterialCfg + | materials.RigidBodyMaterialFragment + | list[materials.RigidBodyMaterialFragment] + | None + ) = None + """Physics material properties. Defaults to None, in which case no physics material is added. + + Accepts a legacy material cfg, a single + :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialFragment`, or a list of such fragments. + """ diff --git a/source/isaaclab/isaaclab/sim/utils/queries.py b/source/isaaclab/isaaclab/sim/utils/queries.py index 079fa509ccf1..65917af23a43 100644 --- a/source/isaaclab/isaaclab/sim/utils/queries.py +++ b/source/isaaclab/isaaclab/sim/utils/queries.py @@ -519,6 +519,7 @@ def find_global_fixed_joint_prim( if stage is None: stage = get_current_stage() + prim_path = str(prim_path) # check prim path is global if not prim_path.startswith("/"): raise ValueError(f"Prim path '{prim_path}' is not global. It must start with '/'.") diff --git a/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py b/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py index 1398708c18ff..12d8d3f16475 100644 --- a/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py +++ b/source/isaaclab/isaaclab/terrains/trimesh/mesh_terrains.py @@ -751,13 +751,16 @@ def repeated_objects_terrain( MeshRepeatedPyramidsTerrainCfg, ) - # if object type is a string, get the function: make_{object_type} - if isinstance(cfg.object_type, str): + # callables are checked first since resolvable strings such as "{DIR}.utils:make_box" are callable str + # subclasses; any other string names a function in this module: make_{object_type} + if callable(cfg.object_type): + object_func = cfg.object_type + elif isinstance(cfg.object_type, str): object_func = globals().get(f"make_{cfg.object_type}") else: - object_func = cfg.object_type + object_func = None if not callable(object_func): - raise ValueError(f"The attribute 'object_type' must be a string or a callable. Received: {object_func}") + raise ValueError(f"The attribute 'object_type' must be a string or a callable. Received: {cfg.object_type}") # Resolve the terrain configuration # -- pass parameters to make calling simpler diff --git a/source/isaaclab/isaaclab/terrains/utils.py b/source/isaaclab/isaaclab/terrains/utils.py index 9b7038c96cfd..f0d24314f3d8 100644 --- a/source/isaaclab/isaaclab/terrains/utils.py +++ b/source/isaaclab/isaaclab/terrains/utils.py @@ -10,10 +10,7 @@ import trimesh import warp as wp -from pxr import UsdGeom - from .. import sim as sim_utils -from ..sim.spawners.materials import spawn_physics_material from ..utils.warp import raycast_mesh @@ -66,65 +63,31 @@ def create_prim_from_mesh(prim_path: str, mesh: trimesh.Trimesh, **kwargs): following steps: - Create a USD Xform prim at the path :obj:`prim_path`. - - Create a USD prim with a mesh defined from the input vertices and triangles at the path :obj:`{prim_path}/mesh`. + - Create a USD prim with a mesh defined from the input vertices and triangles at the path :obj:`{prim_path}/mesh`, + with a collider and the mesh vertex colors. - Assign a physics material to the mesh at the path :obj:`{prim_path}/physicsMaterial`. - Assign a visual material to the mesh at the path :obj:`{prim_path}/visualMaterial`. + The prim is spawned with :class:`~isaaclab.sim.MeshFileCfg`. + Args: prim_path: The path to the primitive to be created. mesh: The mesh to be used for the primitive. Keyword Args: - translation: The translation of the terrain. Defaults to None. - orientation: The orientation of the terrain. Defaults to None. + translation: The translation of the terrain root prim [m]. Defaults to None. + orientation: The orientation (x, y, z, w) of the terrain root prim. Defaults to None. visual_material: The visual material to apply. Defaults to None. physics_material: The physics material to apply. Defaults to None. Accepts a legacy rigid material cfg, a single rigid-material fragment, or a list of fragments. """ - # create parent prim - sim_utils.create_prim(prim_path, "Xform") - # create mesh prim - prim = sim_utils.create_prim( - f"{prim_path}/mesh", - "Mesh", - translation=kwargs.get("translation"), - orientation=kwargs.get("orientation"), - attributes={ - "points": mesh.vertices, - "faceVertexIndices": mesh.faces.flatten(), - "faceVertexCounts": np.asarray([3] * len(mesh.faces)), - "subdivisionScheme": "bilinear", - }, + mesh_cfg = sim_utils.MeshFileCfg( + mesh=sim_utils.MeshFileCfg.TrimeshObjectCfg(mesh=mesh), + collision_props=[sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True)], + visual_material=kwargs.get("visual_material"), + physics_material=kwargs.get("physics_material"), ) - # apply collider properties - collider_fragments = [sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True)] - sim_utils.apply_collision_properties(str(prim.GetPrimPath()), collider_fragments, create_if_missing=True) - # add rgba color to the mesh primvars - if mesh.visual.vertex_colors is not None: - # obtain color from the mesh - rgba_colors = np.asarray(mesh.visual.vertex_colors).astype(np.float32) / 255.0 - # displayColor is a primvar attribute that is used to color the mesh - color_prim_attr = prim.GetAttribute("primvars:displayColor") - color_prim_var = UsdGeom.Primvar(color_prim_attr) - color_prim_var.SetInterpolation(UsdGeom.Tokens.vertex) - color_prim_attr.Set(rgba_colors[:, :3]) - # displayOpacity is a primvar attribute that is used to set the opacity of the mesh - display_prim_attr = prim.GetAttribute("primvars:displayOpacity") - display_prim_var = UsdGeom.Primvar(display_prim_attr) - display_prim_var.SetInterpolation(UsdGeom.Tokens.vertex) - display_prim_var.Set(rgba_colors[:, 3]) - - # create visual material - if kwargs.get("visual_material") is not None: - visual_material_cfg: sim_utils.VisualMaterialCfg = kwargs.get("visual_material") - # spawn the material - visual_material_cfg.func(f"{prim_path}/visualMaterial", visual_material_cfg) - sim_utils.bind_visual_material(prim.GetPrimPath(), f"{prim_path}/visualMaterial") - # create physics material - physics_material = kwargs.get("physics_material") - if physics_material is not None: - spawn_physics_material(f"{prim_path}/physicsMaterial", physics_material) - sim_utils.bind_physics_material(prim.GetPrimPath(), f"{prim_path}/physicsMaterial") + mesh_cfg.func(prim_path, mesh_cfg, translation=kwargs.get("translation"), orientation=kwargs.get("orientation")) def find_flat_patches( diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index 892a3911a943..6a6a6e83963b 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -15,6 +15,7 @@ import math import os import random +import shutil import tempfile import pytest @@ -26,7 +27,7 @@ PhysxTriangleMeshSimplificationCfg, ) -from pxr import UsdGeom, UsdPhysics +from pxr import Usd, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -204,10 +205,13 @@ def test_config_change(assets): assert time_usd_file_created != new_time_usd_file_created -def test_convert_obj(assets): - """Convert an OBJ file""" +def test_convert_obj(assets, tmp_path): + """Convert an OBJ file whose name has extra dots; the prim name is made a valid identifier.""" + for key in ("mtl", "png"): + shutil.copy(assets[key], tmp_path) + asset_path = shutil.copy(assets["obj"], tmp_path / "duck.v2.obj") mesh_config = MeshConverterCfg( - asset_path=assets["obj"], + asset_path=str(asset_path), scale=(random.uniform(0.1, 2.0), random.uniform(0.1, 2.0), random.uniform(0.1, 2.0)), translation=(random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0), random.uniform(-10.0, 10.0)), rotation=random_quaternion(), @@ -216,6 +220,7 @@ def test_convert_obj(assets): # check that mesh conversion is successful check_mesh_conversion(mesh_converter) + assert Usd.Stage.Open(mesh_converter.usd_path).GetDefaultPrim().GetName() == "duck_v2" def test_convert_stl(assets): diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 66568bec437f..9c980715d344 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -37,7 +37,7 @@ RigidBodyMaterialCfg, ) -from pxr import UsdPhysics +from pxr import Usd, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils import isaaclab.sim.schemas as schemas @@ -505,6 +505,27 @@ def test_articulation_root_base_no_physx_schema_when_only_fix_root_link_set(setu ) +@pytest.mark.isaacsim_ci +def test_modify_articulation_root_fix_root_link_uses_given_stage(setup_simulation): + """``fix_root_link`` looks up the existing fixed joint on the stage that is passed in, not the current stage.""" + stage = Usd.Stage.CreateInMemory() + UsdGeom.Xform.Define(stage, "/World") + root = UsdGeom.Xform.Define(stage, "/World/Robot").GetPrim() + UsdPhysics.RigidBodyAPI.Apply(root) + UsdPhysics.ArticulationRootAPI.Apply(root) + joint = UsdPhysics.FixedJoint.Define(stage, "/World/Robot/FixedJoint") + joint.CreateBody1Rel().SetTargets(["/World/Robot"]) + joint.CreateJointEnabledAttr(True) + assert not sim_utils.get_current_stage().GetPrimAtPath("/World/Robot").IsValid() + + with pytest.warns(DeprecationWarning, match="modify_articulation_root_properties"): + schemas.modify_articulation_root_properties( + "/World/Robot", schemas.ArticulationRootBaseCfg(fix_root_link=False), stage=stage + ) + + assert joint.GetJointEnabledAttr().Get() is False + + @pytest.mark.isaacsim_ci def test_physx_articulation_root_writes_self_collisions(setup_simulation): """Setting ``enabled_self_collisions`` on ``PhysxArticulationRootPropertiesCfg`` must author diff --git a/source/isaaclab/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index 9471f490b111..7de8e88fe4fe 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -153,6 +153,54 @@ def test_spawn_ground_plane(sim): assert tuple(shader.GetInput("diffuse_tint").Get()) == pytest.approx((0.1, 0.1, 0.1)) +# tetrahedron with one color per vertex +_TET_VERTICES = [(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)] +_TET_FACES = [(0, 2, 1), (0, 1, 3), (0, 3, 2), (1, 2, 3)] +_CONVEX_HULL_COLLIDER = dict( + collision_props=[sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True)], + mesh_collision_props=[sim_utils.UsdPhysicsMeshCollisionCfg(mesh_approximation_name="convexHull")], +) + + +@pytest.mark.isaacsim_ci +def test_spawn_mesh_from_triangle_data(sim): + """Test spawning in-memory triangle data as a colored rigid body with a convex-hull collider.""" + cfg = sim_utils.MeshFileCfg( + mesh=sim_utils.MeshFileCfg.TriangleMeshCfg( + vertices=_TET_VERTICES, faces=_TET_FACES, vertex_colors=[(255, 0, 0)] * len(_TET_VERTICES) + ), + rigid_props=[sim_utils.UsdPhysicsRigidBodyCfg(rigid_body_enabled=True)], + mass_props=[sim_utils.MassCfg(mass=2.0)], + **_CONVEX_HULL_COLLIDER, + ) + prim = cfg.func("/World/Object", cfg) + + assert prim.HasAPI(UsdPhysics.RigidBodyAPI) + assert UsdPhysics.MassAPI(prim).GetMassAttr().Get() == pytest.approx(2.0) + mesh_prim = sim.stage.GetPrimAtPath("/World/Object/mesh") + assert mesh_prim.HasAPI(UsdPhysics.CollisionAPI) + assert UsdPhysics.MeshCollisionAPI(mesh_prim).GetApproximationAttr().Get() == "convexHull" + assert list(UsdGeom.Mesh(mesh_prim).GetDisplayColorAttr().Get()[0]) == [1.0, 0.0, 0.0] + + +@pytest.mark.isaacsim_ci +def test_spawn_mesh_from_obj_file(sim, tmp_path): + """Test spawning an OBJ mesh file with a convex-hull collider.""" + obj_path = tmp_path / "tetrahedron.obj" + obj_path.write_text( + "".join(f"v {x} {y} {z}\n" for x, y, z in _TET_VERTICES) + + "".join(f"f {a + 1} {b + 1} {c + 1}\n" for a, b, c in _TET_FACES) + ) + cfg = sim_utils.MeshFileCfg(mesh=str(obj_path), **_CONVEX_HULL_COLLIDER) + prim = cfg.func("/World/FileMesh", cfg) + + # the converted geometry is instanceable, so its meshes are instance proxies + meshes = [p for p in Usd.PrimRange(prim, Usd.TraverseInstanceProxies()) if p.IsA(UsdGeom.Mesh)] + assert meshes + assert all(p.HasAPI(UsdPhysics.CollisionAPI) for p in meshes) + assert all(UsdPhysics.MeshCollisionAPI(p).GetApproximationAttr().Get() == "convexHull" for p in meshes) + + @pytest.mark.isaacsim_ci def test_spawn_usd_with_compliant_contact_material(sim): """Test loading prim from USD file with physics material applied to specific prim.""" diff --git a/source/isaaclab/test/sim/test_utils_queries.py b/source/isaaclab/test/sim/test_utils_queries.py index 531be667f73c..2cb3b496547b 100644 --- a/source/isaaclab/test/sim/test_utils_queries.py +++ b/source/isaaclab/test/sim/test_utils_queries.py @@ -19,7 +19,7 @@ import pytest -from pxr import UsdPhysics +from pxr import Sdf, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.sim.utils import queries @@ -275,6 +275,7 @@ def test_find_global_fixed_joint_prim(): assert sim_utils.find_global_fixed_joint_prim("/World/ANYmal") is None assert sim_utils.find_global_fixed_joint_prim("/World/Franka") is not None assert sim_utils.find_global_fixed_joint_prim("/World/Franka_Isaac") is not None + assert sim_utils.find_global_fixed_joint_prim(Sdf.Path("/World/Franka")) is not None # make fixed joint disabled manually joint_prim = sim_utils.find_global_fixed_joint_prim("/World/Franka") diff --git a/source/isaaclab/test/terrains/test_terrain_generator.py b/source/isaaclab/test/terrains/test_terrain_generator.py index 789e53714e1f..dac95fb06960 100644 --- a/source/isaaclab/test/terrains/test_terrain_generator.py +++ b/source/isaaclab/test/terrains/test_terrain_generator.py @@ -10,7 +10,13 @@ import pytest import torch -from isaaclab.terrains import FlatPatchSamplingCfg, MeshStarTerrainCfg, TerrainGenerator, TerrainGeneratorCfg +from isaaclab.terrains import ( + FlatPatchSamplingCfg, + MeshRepeatedBoxesTerrainCfg, + MeshStarTerrainCfg, + TerrainGenerator, + TerrainGeneratorCfg, +) from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG from isaaclab.utils.seed import configure_seed @@ -85,6 +91,19 @@ def test_generation_star_terrain(): assert terrain_generator.terrain_origins.shape == (cfg.num_rows, cfg.num_cols, 3) +def test_repeated_objects_default_object_type(): + """The default resolvable ``object_type`` of the repeated-object configs is called, not looked up by name.""" + object_cfg = MeshRepeatedBoxesTerrainCfg.ObjectCfg(num_objects=3, height=0.2, size=(0.3, 0.3)) + cfg = MeshRepeatedBoxesTerrainCfg( + size=(4.0, 4.0), platform_width=1.0, object_params_start=object_cfg, object_params_end=object_cfg + ) + np.random.seed(0) + meshes, origin = cfg.function(0.5, cfg) + # three objects, ground plane and platform + assert len(meshes) == 5 + assert origin.shape == (3,) + + @pytest.mark.parametrize("use_global_seed", [True, False]) @pytest.mark.parametrize("seed", [20, 40, 80]) def test_generation_reproducibility(use_global_seed, seed): From 726f045a7dd2052311bf64099b096391468f0292 Mon Sep 17 00:00:00 2001 From: Antoine RICHARD Date: Thu, 24 Sep 2026 04:39:55 +0200 Subject: [PATCH 05/11] Move external wrench frame selection into WrenchComposer (#7536) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description `WrenchComposer.compose_to_body_frame()` was called unconditionally by every backend writer on every physics step. It reads `body_com_pos_w` and `body_link_quat_w`, which on PhysX chains to `_ensure_fk_fresh()` → `update_articulations_kinematic()` (a scene-global articulation FK pass) plus `root_view.get_link_transforms()`, then launches the composition kernel. That work is unnecessary whenever the buffered wrench is already in a frame the consumer accepts. This PR moves the frame decision into the composer, which hands each writer the cheapest valid representation: - **all-local content** → the local buffers, untouched. An all-local wrench composes to exactly itself, because the pose data is multiplied by zero. - **all-global-at-CoM content** → the global buffers, untouched, submitted in the world frame by backends that accept one. - **anything else** → composed as before. Writers declare what they can consume once at construction (`supports_world_at_com`) and call `get_forces_and_torques()` per step. All frame logic stays inside the composer; no writer touches a raw buffer or knows a frame rule. ## New public API ```python class WrenchComposer: def __init__(self, asset, *, supports_world_at_com: bool = False) -> None: ... def get_forces_and_torques(self) -> tuple[wp.array, wp.array, bool]: ... forces, torques, is_global = composer.get_forces_and_torques() ``` Additive only. `compose_to_body_frame()`, `out_force_b`, and `out_torque_b` remain public and behaviorally unchanged, so no deprecation cycle is needed. `supports_world_at_com` defaults to `False`, which is exactly the previous behavior. The composer tracks local contributions, global contributions, and positioned global forces with plain booleans. These flags stay conservative across partial resets and clear on a full `reset()`; checking whether a partial reset removed every contribution would require scanning the buffers. The returned `is_global` boolean follows the existing asset API and is passed directly to the backend. No frame enum, content bitmask, or classification helper is needed. ## Equivalence All three branches are exact, from `compose_wrench_to_body_frame`: - **local only**: the three global buffers are zero, so both `quat_rotate_inv` terms vanish and the output is identically `(local_force_b, local_torque_b)`. Note a *positioned* local force also qualifies — the kernel folds `cross(P_b, F_b)` straight into `local_torque_b`. - **global-at-CoM only**: `global_force_w` and the local buffers are zero, so `corrected_torque_w == global_torque_w`. Submitting the world buffers is the same wrench, provided the consumer applies force at the CoM — verified against the vendored `omni.physics.tensors` API, where `position_data=None` means "at the link transform" for **both** `is_global` values, so the flag only reinterprets the vectors' frame and never moves the application point. - **anything else**: unchanged. ## Backend coverage | backend | `supports_world_at_com` | change | |---|---|---| | PhysX | `True` | writer passes the returned `is_global` directly | | OvPhysX | `True` | writer passes `is_global` to the packing kernel to skip the inline rotate | | Newton | `False` (default) | writer swaps to `get_forces_and_torques()`; no constructor change | OvPhysX is the largest win. Its wrench binding wants a world-frame wrench, so a global wrench previously round-tripped: the composer rotated world→body, then the packing kernel rotated body→world. Measured on this branch with a body rotated 90° about +z, a local force `(1,0,0)` emits `(0,1,0)` while a global force `(1,0,0)` emits `(1,0,0)` — an exact round trip, so skipping both rotations is output-preserving. The packed `[6:9]` link position is still written unconditionally on both paths. Newton keeps the body frame because it binds a body-frame array to the solver. Worth noting for a follow-up: its writer kernel already performs the same body→world rotation OvPhysX's does, so Newton could plausibly take the same flag and win the global-at-CoM path too. That is deliberately out of scope here. ## Validation The existing composer rotation, positioned-force, merge, reset, and overwrite tests now exercise `get_forces_and_torques()` directly. Existing frame/position cases cover the four add/set index/mask APIs. The existing articulation body-ordering test checks local/world forwarding, composition avoidance, and packed link positions. One shared rigid-object writer check replaces the separate backend/equivalence suites and their duplicate rotation kernel. Local results: - `test_wrench_composer.py`: **375 passed** on CPU/CUDA. - Articulation ordering and rigid-object wrench writer checks: **24 passed** across PhysX, Newton, and OvPhysX. - `uv run isaaclab -f`: all pre-commit checks passed. - Structural audit: no new test classes or kernels; no production changes in the test consolidation. The composer architecture regression still rejects frame/content enums, the classification layer, and the old submission alias. Regression sensitivity was checked by temporarily restoring unconditional composition and stale-pose caching: the updated existing tests fail on unnecessary pose reads and on a second read after the body rotates, respectively. The previously recorded paired Python dispatch microbenchmark changed local buffer selection from about 99 ns to 46 ns per call, and global-at-CoM selection from 127 ns to 60 ns. This measures buffer selection only, not end-to-end simulation; production code is unchanged by the test consolidation. ## Performance The end-to-end measurements below were recorded for the original optimization; they were not rerun for the boolean/API simplification. Real PhysX, RTX 5090, ANYMAL-C (17 bodies) × 4096 envs = 69,632 links. Same-process paired A/B with the arm order alternated per iteration, 400–500 pairs, 95% CI on paired differences. Baseline is `compose_to_body_frame()` — what the writers previously called unconditionally. | scope | content | baseline | patched | paired delta | 95% CI | |---|---|---:|---:|---:|---| | composition path only | global-at-CoM | 0.2194 ms | 0.0191 ms | **−0.2003 ms** (−91.3%) | [0.1915, 0.2091] | | composition path only | local | 0.1926 ms | 0.0159 ms | **−0.1767 ms** (−91.8%) | [0.1712, 0.1822] | | full `write_data_to_sim` | global-at-CoM | 1.0799 ms | 0.9897 ms | **−0.0902 ms** (−8.4%) | [0.0797, 0.1007] | | full `write_data_to_sim` | local | 1.0072 ms | 0.9305 ms | **−0.0767 ms** (−7.6%) | [0.0719, 0.0815] | Read the end-to-end row as the honest headline: **~0.08 ms per asset per physics step**. Roughly half the isolated saving does not reach the caller, because other work in `write_data_to_sim` touches body poses anyway and pays part of that cost regardless. A cuboid rigid-object scene at the same env count saved only ~6.5 µs, as expected — there is no articulation FK pass to skip. Scope caveat: no in-tree task sets `is_global=True` today (both shipped `apply_external_force_torque` events use the local default), so the local path is what delivers value now, and it is the one that reaches every backend. ## Supersedes This replaces two open PRs, both of which found one half of this problem: - **#7431** (@NeoZng) identified the global-at-CoM case and the cost of the pose read. Its mechanism is correct and its writer-level measurement is corroborated here. It is superseded on placement rather than substance: it put the frame decision in three PhysX writers behind a public eligibility flag, leaving Newton and OvPhysX unserved. The credit for the world-at-CoM half of this mechanism is theirs. - **#7362** cached composition for unchanged local wrenches. Superseded because not composing at all is strictly stronger — the cache does nothing when the wrench is rewritten every step, which is the common case for a per-step randomized push. ## Type of change - New feature (non-breaking change which adds functionality) ## Checklist - [x] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [x] I have run the `pre-commit` checks with `./isaaclab.sh --format` - [x] I have made corresponding changes to the documentation - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added a changelog fragment under `source//changelog.d/` for every touched package - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there 🤖 Generated with [Claude Code](https://claude.com/claude-code) --------- Co-authored-by: Octi Zhang --- AGENTS.md | 5 + .../changelog.d/wrench-fast-paths.minor.rst | 9 + .../isaaclab/utils/wrench_composer.py | 67 ++++- .../assets/_articulation_iface_test_utils.py | 8 +- ...igid_object_collection_iface_test_utils.py | 8 +- .../assets/_rigid_object_iface_test_utils.py | 12 +- .../test_articulation_ordering_iface.py | 65 +++-- .../test/assets/test_rigid_object_iface.py | 44 ++++ .../test/utils/test_wrench_composer.py | 239 +++++++++++------- .../changelog.d/wrench-fast-paths.rst | 6 + .../assets/articulation/articulation.py | 10 +- .../assets/rigid_object/rigid_object.py | 6 +- .../rigid_object_collection.py | 6 +- .../changelog.d/pr7536-test-reset.skip | 1 + .../changelog.d/wrench-fast-paths.rst | 7 + .../assets/articulation/articulation.py | 20 +- .../isaaclab_ov/isaaclab_ov/assets/kernels.py | 48 ++-- .../assets/rigid_object/rigid_object.py | 13 +- .../rigid_object_collection.py | 13 +- .../test/assets/test_articulation.py | 8 +- .../changelog.d/wrench-fast-paths.rst | 7 + .../assets/articulation/articulation.py | 15 +- .../assets/rigid_object/rigid_object.py | 49 +--- .../rigid_object_collection.py | 14 +- .../benchmark/assets/runtime.py | 4 +- 25 files changed, 441 insertions(+), 243 deletions(-) create mode 100644 source/isaaclab/changelog.d/wrench-fast-paths.minor.rst create mode 100644 source/isaaclab_newton/changelog.d/wrench-fast-paths.rst create mode 100644 source/isaaclab_ov/changelog.d/pr7536-test-reset.skip create mode 100644 source/isaaclab_ov/changelog.d/wrench-fast-paths.rst create mode 100644 source/isaaclab_physx/changelog.d/wrench-fast-paths.rst diff --git a/AGENTS.md b/AGENTS.md index be46a9f6edf1..3400069a1f88 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -10,6 +10,11 @@ - Use modern Python type hints, including `X | None` instead of `Optional[X]`. - Use `snake_case` for methods, functions, and CLI arguments. - Keep related public symbols discoverable through consistent prefixes. +- For external wrenches, follow the asset API's `is_global` boolean and `_b`/`_w` buffer naming. Keep + frame conversion decisions in `WrenchComposer` and track pending contributions with plain booleans; + do not introduce frame enums, content bitmasks, or a classification layer. +- Name wrench reads `get_forces_and_torques`, matching the existing add/set methods; avoid a separate + "submission" API or compatibility alias for the unreleased `resolve_submission` method. - Use concrete types for public interfaces where practical. - Use Google-style docstrings for public APIs. - Document SI units for public physical quantities in docstrings using inline `[unit]` notation (e.g. `Particle positions [m], shape [N, 3]`); use `[m or rad, depending on joint type]` where applicable, and skip non-physical fields (indices, counts, flags). diff --git a/source/isaaclab/changelog.d/wrench-fast-paths.minor.rst b/source/isaaclab/changelog.d/wrench-fast-paths.minor.rst new file mode 100644 index 000000000000..7cfcbb3a3b46 --- /dev/null +++ b/source/isaaclab/changelog.d/wrench-fast-paths.minor.rst @@ -0,0 +1,9 @@ +Added +^^^^^ + +* Added :meth:`~isaaclab.utils.wrench_composer.WrenchComposer.get_forces_and_torques`, which returns the + cheapest representation of the buffered external wrench a consumer can accept, along with an + ``is_global`` boolean matching the asset API. Consumers that can apply a world-frame wrench at the + center of mass opt in with the new ``supports_world_at_com`` + constructor argument. Wrenches that are already local-frame, or already global-frame at the center of + mass, are now submitted without composing them through the body poses. diff --git a/source/isaaclab/isaaclab/utils/wrench_composer.py b/source/isaaclab/isaaclab/utils/wrench_composer.py index 83c884c8f3b8..1ccfd35e90b5 100644 --- a/source/isaaclab/isaaclab/utils/wrench_composer.py +++ b/source/isaaclab/isaaclab/utils/wrench_composer.py @@ -30,7 +30,12 @@ class WrenchComposer: - def __init__(self, asset: BaseArticulation | BaseRigidObject | BaseRigidObjectCollection) -> None: + def __init__( + self, + asset: BaseArticulation | BaseRigidObject | BaseRigidObjectCollection, + *, + supports_world_at_com: bool = False, + ) -> None: """Wrench composer with dual-buffer architecture. This class composes forces and torques applied to rigid bodies. Forces and torques can be @@ -55,6 +60,9 @@ def __init__(self, asset: BaseArticulation | BaseRigidObject | BaseRigidObjectCo Args: asset: Asset to use. + supports_world_at_com: Whether the consumer can apply a world-frame wrench at the body's + center of mass. When False, :meth:`get_forces_and_torques` always returns a body-frame + wrench. Defaults to False. """ self.num_envs = asset.num_instances # Avoid isinstance to prevent circular import issues; check by attribute presence instead. @@ -66,6 +74,11 @@ def __init__(self, asset: BaseArticulation | BaseRigidObject | BaseRigidObjectCo self._asset = asset self._active = False self._dirty = False + self._supports_world_at_com = supports_world_at_com + # Conservative until a full reset; partial resets do not scan the remaining buffers. + self._has_local_wrench = False + self._has_global_wrench = False + self._has_global_positions = False if hasattr(self._asset.data, "body_com_pos_w"): self._get_com_pos_fn = lambda a=self._asset: a.data.body_com_pos_w.warp else: @@ -265,6 +278,11 @@ def add_forces_and_torques_index( self._active = True self._dirty = True + if is_global: + self._has_global_wrench = True + self._has_global_positions = self._has_global_positions or (forces is not None and positions is not None) + else: + self._has_local_wrench = True wp.launch( add_forces_to_dual_buffers_index_kernel(env_ids, body_ids), @@ -328,6 +346,11 @@ def set_forces_and_torques_index( self._active = True self._dirty = True + if is_global: + self._has_global_wrench = True + self._has_global_positions = self._has_global_positions or (forces is not None and positions is not None) + else: + self._has_local_wrench = True wp.launch( set_forces_to_dual_buffers_index_kernel(env_ids, body_ids), @@ -388,6 +411,11 @@ def add_forces_and_torques_mask( self._active = True self._dirty = True + if is_global: + self._has_global_wrench = True + self._has_global_positions = self._has_global_positions or (forces is not None and positions is not None) + else: + self._has_local_wrench = True wp.launch( add_forces_to_dual_buffers_mask, @@ -453,6 +481,11 @@ def set_forces_and_torques_mask( self._active = True self._dirty = True + if is_global: + self._has_global_wrench = True + self._has_global_positions = self._has_global_positions or (forces is not None and positions is not None) + else: + self._has_local_wrench = True wp.launch( set_forces_to_dual_buffers_mask, @@ -493,6 +526,9 @@ def add_raw_buffers_from(self, other: WrenchComposer): self._active = True self._dirty = True + self._has_local_wrench = self._has_local_wrench or other._has_local_wrench + self._has_global_wrench = self._has_global_wrench or other._has_global_wrench + self._has_global_positions = self._has_global_positions or other._has_global_positions wp.launch( add_raw_wrench_buffers, @@ -542,6 +578,32 @@ def compose_to_body_frame(self): ) self._dirty = False + def get_forces_and_torques(self) -> tuple[wp.array, wp.array, bool]: + """Get the buffered forces and torques in a frame the consumer accepts. + + Composition into the body frame reads the body poses, which on some backends forces a + kinematics update. That work is unnecessary when the buffered wrench is already in a frame + the consumer accepts: an all-local wrench is its own body-frame composition, and an + all-global-at-CoM wrench can be used directly by a consumer that accepts a world frame. + + The eligibility state is conservative after a partial :meth:`reset`: it may keep composing + even when the selected reset removed every contribution that made composition necessary. A + full :meth:`reset` restores it. This never changes the resulting wrench, only the cost. + + Returns: + Force [N], torque [N·m], and ``is_global``: True for world-frame vectors, False for + body-frame vectors. Both representations apply the force at the body's CoM. Shapes are + ``(num_envs, num_bodies)`` with dtype ``wp.vec3f``. The arrays are owned by the composer + and stay valid until the next mutating call. + """ + if not self._has_global_wrench: + return self._local_force_b, self._local_torque_b, False + if self._supports_world_at_com and not self._has_local_wrench and not self._has_global_positions: + return self._global_force_at_com_w, self._global_torque_w, True + # The fallback depends on the live body pose, even when the input buffers are unchanged. + self.compose_to_body_frame() + return self._out_force_b, self._out_torque_b, False + def reset( self, env_ids: wp.array | torch.Tensor | Sequence[int] | slice | None = None, @@ -571,6 +633,9 @@ def reset( self._out_torque_b.zero_() self._active = False self._dirty = False + self._has_local_wrench = False + self._has_global_wrench = False + self._has_global_positions = False elif env_mask is not None: wp.launch( reset_wrench_composer_mask, diff --git a/source/isaaclab/test/assets/_articulation_iface_test_utils.py b/source/isaaclab/test/assets/_articulation_iface_test_utils.py index 61ac035bd2bc..a3896243ce9c 100644 --- a/source/isaaclab/test/assets/_articulation_iface_test_utils.py +++ b/source/isaaclab/test/assets/_articulation_iface_test_utils.py @@ -125,8 +125,8 @@ def create_physx_articulation( data.spatial_tendon_names = spatial_tendon_names # Create wrench composers (pass articulation which has num_instances, num_bodies, device properties) - mock_inst_wrench = WrenchComposer(articulation) - mock_perm_wrench = WrenchComposer(articulation) + mock_inst_wrench = WrenchComposer(articulation, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(articulation, supports_world_at_com=True) object.__setattr__(articulation, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(articulation, "_permanent_wrench_composer", mock_perm_wrench) @@ -281,8 +281,8 @@ def create_ovphysx_articulation( articulation._create_buffers() # Wrench composers - mock_inst_wrench = WrenchComposer(articulation) - mock_perm_wrench = WrenchComposer(articulation) + mock_inst_wrench = WrenchComposer(articulation, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(articulation, supports_world_at_com=True) object.__setattr__(articulation, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(articulation, "_permanent_wrench_composer", mock_perm_wrench) # Prevent __del__ / _clear_callbacks from raising diff --git a/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py b/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py index 602e4949729f..92bda8ac518c 100644 --- a/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py +++ b/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py @@ -101,8 +101,8 @@ def create_physx_rigid_object_collection( data.body_names = [f"object_{i}" for i in range(num_bodies)] # Create wrench composers - mock_inst_wrench = WrenchComposer(collection) - mock_perm_wrench = WrenchComposer(collection) + mock_inst_wrench = WrenchComposer(collection, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(collection, supports_world_at_com=True) object.__setattr__(collection, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(collection, "_permanent_wrench_composer", mock_perm_wrench) @@ -260,8 +260,8 @@ def create_ovphysx_rigid_object_collection( collection._create_buffers() # Use production wrench composers for interface coverage. - mock_inst_wrench = WrenchComposer(collection) - mock_perm_wrench = WrenchComposer(collection) + mock_inst_wrench = WrenchComposer(collection, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(collection, supports_world_at_com=True) object.__setattr__(collection, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(collection, "_permanent_wrench_composer", mock_perm_wrench) diff --git a/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py b/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py index 94b198da64b7..3c6d11de4f28 100644 --- a/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py +++ b/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py @@ -86,8 +86,8 @@ def create_physx_rigid_object( data.body_names = body_names # Create wrench composers - mock_inst_wrench = WrenchComposer(rigid_object) - mock_perm_wrench = WrenchComposer(rigid_object) + mock_inst_wrench = WrenchComposer(rigid_object, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(rigid_object, supports_world_at_com=True) object.__setattr__(rigid_object, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(rigid_object, "_permanent_wrench_composer", mock_perm_wrench) @@ -104,10 +104,6 @@ def create_physx_rigid_object( # Cached .view(wp.float32) wrappers object.__setattr__(rigid_object, "_root_link_pose_w_f32", None) object.__setattr__(rigid_object, "_root_com_vel_w_f32", None) - object.__setattr__(rigid_object, "_inst_wrench_force_f32", None) - object.__setattr__(rigid_object, "_inst_wrench_torque_f32", None) - object.__setattr__(rigid_object, "_perm_wrench_force_f32", None) - object.__setattr__(rigid_object, "_perm_wrench_torque_f32", None) # Pre-allocated pinned CPU buffers for PhysX TensorAPI writes N, B = num_instances, 1 # rigid object has 1 body @@ -247,8 +243,8 @@ def create_ovphysx_rigid_object( obj._create_buffers() # Use production wrench composers for interface coverage. - mock_inst_wrench = WrenchComposer(obj) - mock_perm_wrench = WrenchComposer(obj) + mock_inst_wrench = WrenchComposer(obj, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(obj, supports_world_at_com=True) object.__setattr__(obj, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(obj, "_permanent_wrench_composer", mock_perm_wrench) diff --git a/source/isaaclab/test/assets/test_articulation_ordering_iface.py b/source/isaaclab/test/assets/test_articulation_ordering_iface.py index 85aef24c6d76..0e27ed25d386 100644 --- a/source/isaaclab/test/assets/test_articulation_ordering_iface.py +++ b/source/isaaclab/test/assets/test_articulation_ordering_iface.py @@ -7,7 +7,7 @@ """Mocked cross-backend articulation ordering interface tests.""" -from unittest.mock import MagicMock +from unittest.mock import MagicMock, patch import numpy as np import pytest @@ -17,7 +17,6 @@ from _pytest.mark.structures import ParameterSet from isaaclab.utils.buffers import TimestampedBufferWarp -from isaaclab.utils.wrench_composer import WrenchComposer def _make_body_ordering_backend_data(num_instances: int, num_bodies: int) -> tuple[np.ndarray, ...]: @@ -679,24 +678,21 @@ def _set_body_ordering_backend_data( raise AssertionError(f"Unsupported backend for body-ordering test: {backend}") -def _set_identity_body_poses(backend: str, art, raw_backend) -> None: - """Give wrench transforms deterministic identity rotations.""" +def _set_rotated_body_poses(backend: str, art, raw_backend) -> None: + """Use a known 90-degree Z rotation and nonzero positions for wrench packing.""" + poses = np.zeros((art.num_instances, art.num_bodies, 7), dtype=np.float32) + poses[..., 0] = np.arange(art.num_bodies) + 10.0 + poses[..., 5:7] = 2.0**-0.5 if backend == "newton": - poses = np.zeros((art.num_instances, art.num_bodies, 7), dtype=np.float32) - poses[..., 6] = 1.0 poses_wp = wp.array(poses[:, None], dtype=wp.transformf, device=art.device) raw_backend.set_mock_link_transforms(poses_wp) art.data._sim_bind_body_link_pose_w.assign(poses_wp[:, 0]) art.data._refresh_user_order_body_state() - return - if backend != "ovphysx": - return - from isaaclab_ov import tensor_types as TT + elif backend == "ovphysx": + from isaaclab_ov import tensor_types as TT - poses = np.zeros((art.num_instances, art.num_bodies, 7), dtype=np.float32) - poses[..., 6] = 1.0 - raw_backend.bindings[TT.LINK_POSE]._data = poses - art.data._reset_pose() + raw_backend.bindings[TT.LINK_POSE]._data = poses + art.data._reset_pose() def _read_backend_wrench(backend: str, art, raw_backend, captured: dict) -> tuple[np.ndarray, np.ndarray]: @@ -2011,13 +2007,13 @@ def test_external_wrenches_are_written_in_backend_body_order(self, backend, orde is_fixed_base=is_fixed_base, body_ordering=body_ordering, ) - _set_identity_body_poses(backend, art, raw_backend) - object.__setattr__(art, "_instantaneous_wrench_composer", WrenchComposer(art)) - object.__setattr__(art, "_permanent_wrench_composer", WrenchComposer(art)) + _set_rotated_body_poses(backend, art, raw_backend) captured = {} if backend == "physx": def capture_wrench(*, force_data, torque_data, position_data, indices, is_global): + captured["is_global"] = is_global + assert position_data is None captured["force"] = force_data.numpy().reshape(num_instances, num_bodies, 3).copy() captured["torque"] = torque_data.numpy().reshape(num_instances, num_bodies, 3).copy() @@ -2025,17 +2021,32 @@ def capture_wrench(*, force_data, torque_data, position_data, indices, is_global forces = np.arange(num_instances * num_bodies * 3, dtype=np.float32).reshape(num_instances, num_bodies, 3) torques = forces + 100.0 - art.instantaneous_wrench_composer.set_forces_and_torques_index( - forces=wp.array(forces, dtype=wp.vec3f, device=device), - torques=wp.array(torques, dtype=wp.vec3f, device=device), - ) - - art.write_data_to_sim() - backend_to_user = _expected_backend_to_user(body_ordering, backend_body_names) - backend_force, backend_torque = _read_backend_wrench(backend, art, raw_backend, captured) - np.testing.assert_allclose(backend_force, forces[:, backend_to_user]) - np.testing.assert_allclose(backend_torque, torques[:, backend_to_user]) + composer = art.instantaneous_wrench_composer + for is_global in (False, True): + composer.set_forces_and_torques_index( + forces=wp.array(forces, dtype=wp.vec3f, device=device), + torques=wp.array(torques, dtype=wp.vec3f, device=device), + is_global=is_global, + ) + with patch.object(composer, "compose_to_body_frame", wraps=composer.compose_to_body_frame) as compose: + art.write_data_to_sim() + assert compose.call_count == int(is_global and backend == "newton") + + expected_force, expected_torque = forces, torques + if backend == "physx": + assert captured["is_global"] is is_global + elif not is_global: + expected_force = np.stack((-forces[..., 1], forces[..., 0], forces[..., 2]), axis=-1) + expected_torque = np.stack((-torques[..., 1], torques[..., 0], torques[..., 2]), axis=-1) + backend_force, backend_torque = _read_backend_wrench(backend, art, raw_backend, captured) + np.testing.assert_allclose(backend_force, expected_force[:, backend_to_user], atol=1e-4) + np.testing.assert_allclose(backend_torque, expected_torque[:, backend_to_user], atol=1e-4) + if backend == "ovphysx": + from isaaclab_ov import tensor_types as TT + + packed_positions = raw_backend.bindings[TT.LINK_WRENCH]._data[..., 6:9] + np.testing.assert_array_equal(packed_positions, raw_backend.bindings[TT.LINK_POSE]._data[..., :3]) @_requires_ovphysx def test_ovphysx_configured_defaults_use_public_joint_names(self): diff --git a/source/isaaclab/test/assets/test_rigid_object_iface.py b/source/isaaclab/test/assets/test_rigid_object_iface.py index e8a6147b7b4f..f3dba59bd843 100644 --- a/source/isaaclab/test/assets/test_rigid_object_iface.py +++ b/source/isaaclab/test/assets/test_rigid_object_iface.py @@ -13,12 +13,16 @@ The setup is a bit convoluted so that we can run these tests without requiring Isaac Sim or GPU simulation. """ +from unittest.mock import MagicMock, patch + import numpy as np import pytest import torch import warp as wp from _rigid_object_iface_test_utils import BACKENDS, get_rigid_object +from isaaclab.utils.math import quat_apply + pytestmark = pytest.mark.integration @@ -954,6 +958,46 @@ def test_write_root_velocity_to_sim_mask(self, backend, num_instances, device, r class TestRigidObjectWritersBody: """Test body property writers/setters with all input combinations.""" + @_production_backends + @_default_devices + def test_external_wrench_frames(self, backend, device): + """Forward local and world wrenches through the real writer in each backend's frame.""" + obj, raw_backend = get_rigid_object(backend, num_instances=2, device=device) + composer = obj.permanent_wrench_composer + forces = torch.arange(1.0, 7.0, device=device).reshape(2, 1, 3) + torques = forces + 10.0 + if backend == "physx": + raw_backend.apply_forces_and_torques_at_position = MagicMock() + + for is_global in (False, True): + composer.reset() + composer.set_forces_and_torques_index(forces=forces, torques=torques, is_global=is_global) + with patch.object(composer, "compose_to_body_frame", wraps=composer.compose_to_body_frame) as compose: + obj.write_data_to_sim() + assert compose.call_count == int(is_global and backend == "newton") + + expected_force, expected_torque = forces, torques + if backend == "physx": + call = raw_backend.apply_forces_and_torques_at_position.call_args.kwargs + assert call["is_global"] is is_global + assert call["position_data"] is None + actual_force = call["force_data"].numpy().reshape(2, 1, 3) + actual_torque = call["torque_data"].numpy().reshape(2, 1, 3) + else: + if not is_global: + quat = obj.data.body_link_quat_w.torch + expected_force, expected_torque = quat_apply(quat, forces), quat_apply(quat, torques) + if backend == "newton": + packed = obj.data._sim_bind_body_external_wrench.numpy() + else: + from isaaclab_ov import tensor_types as TT + + packed = raw_backend.bindings[TT.RIGID_BODY_WRENCH]._data.reshape(2, 1, 9) + np.testing.assert_allclose(packed[..., 6:9], obj.data.body_link_pos_w.warp.numpy()) + actual_force, actual_torque = packed[..., :3], packed[..., 3:6] + np.testing.assert_allclose(actual_force, expected_force.cpu().numpy(), atol=1e-5, rtol=1e-5) + np.testing.assert_allclose(actual_torque, expected_torque.cpu().numpy(), atol=1e-5, rtol=1e-5) + @_backends @_default_dims @_default_devices diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py index b0a346eaa6c0..17b64287baff 100644 --- a/source/isaaclab/test/utils/test_wrench_composer.py +++ b/source/isaaclab/test/utils/test_wrench_composer.py @@ -3,7 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause +import ast +import inspect from types import SimpleNamespace +from typing import get_type_hints +from unittest.mock import patch import numpy as np import pytest @@ -17,6 +21,28 @@ pytestmark = pytest.mark.unit +def test_wrench_composer_uses_asset_frame_conventions(): + """Keep frame selection as an is_global boolean, with no enum or content-classification layer.""" + tree = ast.parse(inspect.getsource(WrenchComposer)) + assert [node.name for node in ast.walk(tree) if isinstance(node, ast.ClassDef)] == ["WrenchComposer"] + assert not any( + isinstance(node, ast.Attribute) and node.attr in {"_content", "_classify"} for node in ast.walk(tree) + ) + module_tree = ast.parse(inspect.getsource(inspect.getmodule(WrenchComposer))) + assert not any(isinstance(node, ast.ImportFrom) and node.module == "enum" for node in ast.walk(module_tree)) + assert not hasattr(WrenchComposer, "resolve_submission") + assert get_type_hints(WrenchComposer.get_forces_and_torques)["return"] == tuple[wp.array, wp.array, bool] + + +def _get_wrench_without_pose_reads(composer: WrenchComposer) -> tuple[wp.array, wp.array, bool]: + """Read a fast-path wrench while rejecting the expensive body-pose queries.""" + with ( + patch.object(composer, "_get_com_pos_fn", side_effect=AssertionError("unexpected CoM read")), + patch.object(composer, "_get_link_quat_fn", side_effect=AssertionError("unexpected quaternion read")), + ): + return composer.get_forces_and_torques() + + class _WrenchAssetDataFixture: """Minimal asset data required by :class:`WrenchComposer`.""" @@ -415,27 +441,35 @@ def test_global_forces_with_rotation(device: str, num_envs: int, num_bodies: int """Test that global forces are correctly rotated to the local frame.""" rng = np.random.default_rng(seed=10) - for _ in range(5): + for method in ( + "add_forces_and_torques_index", + "add_forces_and_torques_mask", + "set_forces_and_torques_index", + "set_forces_and_torques_mask", + ): # Create random link quaternions link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) link_quat_torch = torch.from_numpy(link_quat_np) # Create mock asset with custom quaternions mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) + wrench_composer = WrenchComposer(mock_asset, supports_world_at_com=True) # Generate random global forces for all envs and bodies forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) # Apply global forces - wrench_composer.add_forces_and_torques_index(forces=forces_global, is_global=True) + getattr(wrench_composer, method)(forces=forces_global, is_global=True) # Compute expected local forces by rotating global forces by inverse quaternion expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np) # Check raw global buffer has the global forces - global_force_np = wrench_composer.global_force_at_com_w.numpy() + force, torque, is_global = _get_wrench_without_pose_reads(wrench_composer) + assert is_global is True + np.testing.assert_array_equal(torque.numpy(), np.zeros_like(forces_global_np)) + global_force_np = force.numpy() assert np.allclose(global_force_np, forces_global_np, atol=1e-4, rtol=1e-5), ( f"Global force buffer mismatch.\nExpected:\n{forces_global_np}\nGot:\n{global_force_np}" ) @@ -457,27 +491,35 @@ def test_global_torques_with_rotation(device: str, num_envs: int, num_bodies: in """Test that global torques are correctly rotated to the local frame.""" rng = np.random.default_rng(seed=11) - for _ in range(5): + for method in ( + "add_forces_and_torques_index", + "add_forces_and_torques_mask", + "set_forces_and_torques_index", + "set_forces_and_torques_mask", + ): # Create random link quaternions link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) link_quat_torch = torch.from_numpy(link_quat_np) # Create mock asset with custom quaternions mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) + wrench_composer = WrenchComposer(mock_asset, supports_world_at_com=True) # Generate random global torques torques_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) torques_global = wp.from_numpy(torques_global_np, dtype=wp.vec3f, device=device) - # Apply global torques - wrench_composer.add_forces_and_torques_index(torques=torques_global, is_global=True) + # Positions are unused when only torques are supplied. + getattr(wrench_composer, method)(torques=torques_global, positions=torques_global, is_global=True) # Compute expected local torques expected_torques_local = quat_rotate_inv_np(link_quat_np, torques_global_np) # Check raw global buffer has the global torques - global_torque_np = wrench_composer.global_torque_w.numpy() + force, torque, is_global = _get_wrench_without_pose_reads(wrench_composer) + assert is_global is True + np.testing.assert_array_equal(force.numpy(), np.zeros_like(torques_global_np)) + global_torque_np = torque.numpy() assert np.allclose(global_torque_np, torques_global_np, atol=1e-4, rtol=1e-5), ( f"Global torque buffer mismatch.\nExpected:\n{torques_global_np}\nGot:\n{global_torque_np}" ) @@ -499,7 +541,12 @@ def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies """Test global forces at global positions with full coordinate transformation.""" rng = np.random.default_rng(seed=12) - for _ in range(5): + for method in ( + "add_forces_and_torques_index", + "add_forces_and_torques_mask", + "set_forces_and_torques_index", + "set_forces_and_torques_mask", + ): # Create random link poses link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) @@ -508,7 +555,7 @@ def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies # Create mock asset mock_asset = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) + wrench_composer = WrenchComposer(mock_asset, supports_world_at_com=True) # Generate random global forces and positions forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) @@ -517,7 +564,7 @@ def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies positions_global = wp.from_numpy(positions_global_np, dtype=wp.vec3f, device=device) # Apply global forces at global positions - wrench_composer.add_forces_and_torques_index(forces=forces_global, positions=positions_global, is_global=True) + getattr(wrench_composer, method)(forces=forces_global, positions=positions_global, is_global=True) # Compute expected results: # 1. Force in local frame = quat_rotate_inv(link_quat, global_force) @@ -540,17 +587,17 @@ def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies f"Global force buffer mismatch.\nExpected:\n{forces_global_np}\nGot:\n{global_force_np}" ) - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() + force, torque, is_global = wrench_composer.get_forces_and_torques() + assert is_global is False # Verify forces - composed_force_np = wrench_composer.out_force_b.warp.numpy() + composed_force_np = force.numpy() assert np.allclose(composed_force_np, expected_forces_local, atol=1e-3, rtol=1e-4), ( f"Global force at position failed.\nExpected forces:\n{expected_forces_local}\nGot:\n{composed_force_np}" ) # Verify torques - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() + composed_torque_np = torque.numpy() assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-3, rtol=1e-4), ( f"Global force at position failed.\nExpected torques:\n{expected_torques_local}\nGot:\n{composed_torque_np}" ) @@ -622,14 +669,21 @@ def test_90_degree_rotation_global_force(device: str): # Actually, inverse rotation of +90° around Z applied to (1,0,0) gives (0,-1,0) expected_force_local = np.array([[[0.0, -1.0, 0.0]]], dtype=np.float32) - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() + force, _, is_global = wrench_composer.get_forces_and_torques() + assert is_global is False - composed_force_np = wrench_composer.out_force_b.warp.numpy() + composed_force_np = force.numpy() assert np.allclose(composed_force_np, expected_force_local, atol=1e-5), ( f"90-degree rotation test failed.\nExpected:\n{expected_force_local}\nGot:\n{composed_force_np}" ) + # A second read must use the live pose even though the wrench buffers have not changed. + quat_180_z = torch.tensor([[[0.0, 0.0, 1.0, 0.0]]], device=device) + mock_asset.data.body_link_quat_w = ProxyArray(wp.from_torch(quat_180_z, dtype=wp.quatf)) + force, _, is_global = wrench_composer.get_forces_and_torques() + assert is_global is False + np.testing.assert_allclose(force.numpy(), [[[-1.0, 0.0, 0.0]]], atol=1e-6) + @pytest.mark.parametrize("device", test_devices()) def test_composition_mixed_local_and_global(device: str): @@ -642,7 +696,7 @@ def test_composition_mixed_local_and_global(device: str): link_quat_torch = torch.from_numpy(link_quat_np) mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) + wrench_composer = WrenchComposer(mock_asset, supports_world_at_com=True) # Generate random local and global forces forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) @@ -667,10 +721,10 @@ def test_composition_mixed_local_and_global(device: str): global_force_at_com_np = wrench_composer.global_force_at_com_w.numpy() assert np.allclose(global_force_at_com_np, forces_global_np, atol=1e-4, rtol=1e-5) - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() + force, _, is_global = wrench_composer.get_forces_and_torques() + assert is_global is False - composed_force_np = wrench_composer.out_force_b.warp.numpy() + composed_force_np = force.numpy() assert np.allclose(composed_force_np, expected_total, atol=1e-4, rtol=1e-5), ( f"Mixed local/global composition failed.\nExpected:\n{expected_total}\nGot:\n{composed_force_np}" ) @@ -683,7 +737,12 @@ def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: """Test local forces at local positions (offset from link frame).""" rng = np.random.default_rng(seed=15) - for _ in range(5): + for method in ( + "add_forces_and_torques_index", + "add_forces_and_torques_mask", + "set_forces_and_torques_index", + "set_forces_and_torques_mask", + ): # Create random link poses (shouldn't affect local frame calculations) link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) @@ -691,7 +750,7 @@ def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: link_quat_torch = torch.from_numpy(link_quat_np) mock_asset = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) + wrench_composer = WrenchComposer(mock_asset, supports_world_at_com=True) # Generate random local forces and local positions (offsets) forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) @@ -700,7 +759,7 @@ def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: positions_local = wp.from_numpy(positions_local_np, dtype=wp.vec3f, device=device) # Apply local forces at local positions - wrench_composer.add_forces_and_torques_index(forces=forces_local, positions=positions_local, is_global=False) + getattr(wrench_composer, method)(forces=forces_local, positions=positions_local, is_global=False) # Expected: forces stay as-is, torque = cross(position, force) expected_forces = forces_local_np @@ -710,12 +769,12 @@ def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: local_force_np = wrench_composer.local_force_b.numpy() assert np.allclose(local_force_np, expected_forces, atol=1e-4, rtol=1e-5) - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() + force, torque, is_global = _get_wrench_without_pose_reads(wrench_composer) + assert is_global is False # Verify - composed_force_np = wrench_composer.out_force_b.warp.numpy() - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() + composed_force_np = force.numpy() + composed_torque_np = torque.numpy() assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5) assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5) @@ -785,7 +844,7 @@ def test_add_raw_buffers_from(device: str, num_envs: int, num_bodies: int): mock_a = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) mock_b = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - composer_a = WrenchComposer(mock_a) + composer_a = WrenchComposer(mock_a, supports_world_at_com=True) composer_b = WrenchComposer(mock_b) # Populate composer_a with local forces at positions @@ -824,15 +883,16 @@ def test_add_raw_buffers_from(device: str, num_envs: int, num_bodies: int): ) # Compose both and compare - composer_a.compose_to_body_frame() + force, torque, is_global = composer_a.get_forces_and_torques() + assert is_global is False composer_ref.compose_to_body_frame() - assert np.allclose( - composer_a.out_force_b.warp.numpy(), composer_ref.out_force_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "add_raw_buffers_from force mismatch vs direct accumulation" - assert np.allclose( - composer_a.out_torque_b.warp.numpy(), composer_ref.out_torque_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "add_raw_buffers_from torque mismatch vs direct accumulation" + assert np.allclose(force.numpy(), composer_ref.out_force_b.warp.numpy(), atol=1e-4, rtol=1e-5), ( + "add_raw_buffers_from force mismatch vs direct accumulation" + ) + assert np.allclose(torque.numpy(), composer_ref.out_torque_b.warp.numpy(), atol=1e-4, rtol=1e-5), ( + "add_raw_buffers_from torque mismatch vs direct accumulation" + ) @pytest.mark.parametrize("device", test_devices()) @@ -1042,12 +1102,11 @@ def test_set_forces_overwrites_previous_add(device: str): forces=wp.from_numpy(forces_b_np, dtype=wp.vec3f, device=device), ) - composer.compose_to_body_frame() + force, _, is_global = _get_wrench_without_pose_reads(composer) + assert is_global is False # Output should match forces_b only (forces_a should be gone) - assert np.allclose(composer.out_force_b.warp.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), ( - "set_forces did not clear previous add" - ) + assert np.allclose(force.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), "set_forces did not clear previous add" @pytest.mark.parametrize("device", test_devices()) @@ -1122,55 +1181,45 @@ def test_set_forces_clears_targeted_envs_only(device: str): @pytest.mark.parametrize("device", test_devices()) -def test_partial_reset_zeros_only_specified_envs(device: str): - """Test that partial reset zeros only the specified environments and leaves others intact.""" +@pytest.mark.parametrize("source_is_global", [False, True]) +def test_partial_reset_zeros_only_specified_envs(device: str, source_is_global: bool): + """Partial resets preserve the remaining merged wrench and its required composition.""" num_envs, num_bodies = 8, 3 rng = np.random.default_rng(seed=50) - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - # Populate all envs with local forces - forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), + asset = create_mock_asset(num_envs, num_bodies, device) + composer = WrenchComposer(asset, supports_world_at_com=True) + source = WrenchComposer(asset) + forces = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) + source_forces = rng.uniform(-100.0, 100.0, forces.shape).astype(np.float32) + positions = rng.uniform(-5.0, 5.0, forces.shape).astype(np.float32) + composer.add_forces_and_torques_index(forces=wp.array(forces, dtype=wp.vec3f, device=device), is_global=True) + assert _get_wrench_without_pose_reads(composer)[2] is True + source.add_forces_and_torques_index( + forces=wp.array(source_forces, dtype=wp.vec3f, device=device), + positions=wp.array(positions, dtype=wp.vec3f, device=device), + is_global=source_is_global, ) + composer.add_raw_buffers_from(source) - # Also add global forces to populate more buffers - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device), - is_global=True, - ) - - # Partial reset: only envs [1, 3, 5] - reset_env_ids = np.array([1, 3, 5], dtype=np.int32) - kept_env_ids = np.array([0, 2, 4, 6, 7], dtype=np.int32) - composer.reset(env_ids=wp.from_numpy(reset_env_ids, dtype=wp.int32, device=device)) - - # Reset envs should be zeroed across all input buffers - zeros = np.zeros((num_bodies, 3), dtype=np.float32) - local_force = composer.local_force_b.numpy() - global_force_at_com = composer.global_force_at_com_w.numpy() - for eid in reset_env_ids: - assert np.allclose(local_force[eid], zeros, atol=1e-7), f"local_force_b not zeroed for env {eid}" - assert np.allclose(global_force_at_com[eid], zeros, atol=1e-7), ( - f"global_force_at_com_w not zeroed for env {eid}" - ) - - # Kept envs should retain their values - for eid in kept_env_ids: - assert np.allclose(local_force[eid], forces_np[eid], atol=1e-4, rtol=1e-5), ( - f"local_force_b changed for non-reset env {eid}" - ) - assert np.allclose(global_force_at_com[eid], forces_global_np[eid], atol=1e-4, rtol=1e-5), ( - f"global_force_at_com_w changed for non-reset env {eid}" - ) - - # Flags: _active should still be True, _dirty should be True + reset_env_ids = [1, 3, 5] + if source_is_global: + mask = np.zeros(num_envs, dtype=bool) + mask[reset_env_ids] = True + composer.reset(env_mask=wp.array(mask, dtype=wp.bool, device=device)) + else: + composer.reset(env_ids=reset_env_ids) assert composer.active assert composer._dirty + force, torque, is_global = composer.get_forces_and_torques() + assert is_global is False + expected_force = forces + source_forces + expected_torque = np.cross(positions, source_forces) + expected_force[reset_env_ids] = 0.0 + expected_torque[reset_env_ids] = 0.0 + np.testing.assert_allclose(force.numpy(), expected_force, atol=1e-4, rtol=1e-5) + np.testing.assert_allclose(torque.numpy(), expected_torque, atol=1e-4, rtol=1e-5) + @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("env_ids", [None, slice(None)], ids=["none", "full_slice"]) @@ -1179,18 +1228,33 @@ def test_full_reset_clears_active_flag(device: str, env_ids: slice | None): num_envs, num_bodies = 4, 2 mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) + composer = WrenchComposer(mock_asset, supports_world_at_com=True) forces_np = np.ones((num_envs, num_bodies, 3), dtype=np.float32) composer.add_forces_and_torques_index( forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), ) + composer.add_forces_and_torques_index( + forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), + positions=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), + is_global=True, + ) + composer.get_forces_and_torques() assert composer.active composer.reset(env_ids=env_ids) assert not composer.active assert not composer._dirty + force, torque, is_global = _get_wrench_without_pose_reads(composer) + assert is_global is False + np.testing.assert_array_equal(force.numpy(), np.zeros_like(forces_np)) + np.testing.assert_array_equal(torque.numpy(), np.zeros_like(forces_np)) + composer.add_forces_and_torques_index( + forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), is_global=True + ) + assert _get_wrench_without_pose_reads(composer)[2] is True + # ============================================================================ # Deprecated API Backward-Compatibility Tests @@ -1282,10 +1346,11 @@ def test_set_forces_mask_overwrites_previous_add(device: str): forces=wp.from_numpy(forces_b_np, dtype=wp.vec3f, device=device), ) - composer.compose_to_body_frame() + force, _, is_global = _get_wrench_without_pose_reads(composer) + assert is_global is False # Output should match forces_b only (forces_a should be gone) - assert np.allclose(composer.out_force_b.warp.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), ( + assert np.allclose(force.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), ( "set_forces_and_torques_mask did not clear previous add" ) diff --git a/source/isaaclab_newton/changelog.d/wrench-fast-paths.rst b/source/isaaclab_newton/changelog.d/wrench-fast-paths.rst new file mode 100644 index 000000000000..7e9948735597 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/wrench-fast-paths.rst @@ -0,0 +1,6 @@ +Changed +^^^^^^^ + +* Changed the external-wrench writers to submit through + :meth:`~isaaclab.utils.wrench_composer.WrenchComposer.get_forces_and_torques`, so an all-local-frame + wrench no longer reads the body transforms before being written to the solver. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py index 8621d57897d0..5784e07a5d56 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py @@ -397,7 +397,7 @@ def write_data_to_sim(self): composer.add_raw_buffers_from(self._permanent_wrench_composer) else: composer = self._permanent_wrench_composer - composer.compose_to_body_frame() + force_b, torque_b, _ = composer.get_forces_and_torques() # Kept separate from the joint-target gather below: this scatter runs # over bodies while the target gather runs over joints (mismatched # item axes), and it must precede the actuator compute/submit below, @@ -409,8 +409,8 @@ def write_data_to_sim(self): dim=(self.num_instances, self.num_bodies), device=self.device, inputs=[ - composer.out_force_b.warp, - composer.out_torque_b.warp, + force_b, + torque_b, self._data.body_link_pose_w.warp, self._body_user_to_backend_map(), self._data._sim_bind_body_external_wrench, @@ -424,8 +424,8 @@ def write_data_to_sim(self): dim=(self.num_instances, self.num_bodies), device=self.device, inputs=[ - composer.out_force_b, - composer.out_torque_b, + force_b, + torque_b, self._data.body_link_pose_w.warp, self._data._sim_bind_body_external_wrench, self._ALL_ENV_MASK, diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py index 39251a427603..eb4cf3ad5b21 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py @@ -148,14 +148,14 @@ def write_data_to_sim(self) -> None: composer.add_raw_buffers_from(self._permanent_wrench_composer) else: composer = self._permanent_wrench_composer - composer.compose_to_body_frame() + force_b, torque_b, _ = composer.get_forces_and_torques() wp.launch( shared_kernels.update_wrench_array_with_force_and_torque, dim=(self.num_instances, self.num_bodies), device=self.device, inputs=[ - composer.out_force_b, - composer.out_torque_b, + force_b, + torque_b, self._data.body_link_pose_w.warp, self._data._sim_bind_body_external_wrench, self._ALL_ENV_MASK, diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py index b4e4edb4e95a..c52ae05bc56d 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py @@ -194,14 +194,14 @@ def write_data_to_sim(self) -> None: composer.add_raw_buffers_from(self._permanent_wrench_composer) else: composer = self._permanent_wrench_composer - composer.compose_to_body_frame() + force_b, torque_b, _ = composer.get_forces_and_torques() wp.launch( shared_kernels.update_wrench_array_with_force_and_torque, dim=(self.num_instances, self.num_bodies), device=self.device, inputs=[ - composer.out_force_b, - composer.out_torque_b, + force_b, + torque_b, self._data.body_link_pose_w.warp, self._wrench_buffer, self._ALL_ENV_MASK, diff --git a/source/isaaclab_ov/changelog.d/pr7536-test-reset.skip b/source/isaaclab_ov/changelog.d/pr7536-test-reset.skip new file mode 100644 index 000000000000..09ec8ebf8153 --- /dev/null +++ b/source/isaaclab_ov/changelog.d/pr7536-test-reset.skip @@ -0,0 +1 @@ +Test-only fix: preserve environment origins when resetting the external-wrench test articulations. diff --git a/source/isaaclab_ov/changelog.d/wrench-fast-paths.rst b/source/isaaclab_ov/changelog.d/wrench-fast-paths.rst new file mode 100644 index 000000000000..a3e008ddf5dd --- /dev/null +++ b/source/isaaclab_ov/changelog.d/wrench-fast-paths.rst @@ -0,0 +1,7 @@ +Changed +^^^^^^^ + +* Changed the external-wrench writers to submit through + :meth:`~isaaclab.utils.wrench_composer.WrenchComposer.get_forces_and_torques`. A wrench that is already + global-frame at the center of mass is now packed without rotating it into the body frame and back, + and an all-local-frame wrench no longer reads the body transforms before packing. diff --git a/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py b/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py index 98efca433794..2df2dade3aee 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py @@ -244,11 +244,10 @@ def write_data_to_sim(self) -> None: if inst.active: if perm.active: inst.add_raw_buffers_from(perm) - force_b = inst.out_force_b.warp - torque_b = inst.out_torque_b.warp + composer = inst else: - force_b = perm.out_force_b.warp - torque_b = perm.out_torque_b.warp + composer = perm + force_in, torque_in, is_global = composer.get_forces_and_torques() # rotate body-frame wrenches into the world frame expected by ``LINK_WRENCH``. # Read the link poses directly from the backend-order ``LINK_POSE`` buffer: the @@ -259,7 +258,14 @@ def write_data_to_sim(self) -> None: wp.launch( shared_kernels._body_wrench_to_world_ordered, dim=(self._num_instances, self._num_bodies), - inputs=[force_b, torque_b, poses, self._body_user_to_backend_map(), has_body_ordering], + inputs=[ + force_in, + torque_in, + poses, + self._body_user_to_backend_map(), + has_body_ordering, + is_global, + ], outputs=[self._wrench_buf], device=self._device, ) @@ -4064,8 +4070,8 @@ def _create_buffers(self) -> None: self._wrench_buf = wp.zeros((N, B, 9), dtype=wp.float32, device=device) # Wrench composers. - self._instantaneous_wrench_composer = WrenchComposer(self) - self._permanent_wrench_composer = WrenchComposer(self) + self._instantaneous_wrench_composer = WrenchComposer(self, supports_world_at_com=True) + self._permanent_wrench_composer = WrenchComposer(self, supports_world_at_com=True) # Pinned-host CPU staging for env ids/masks (PR #5329 pattern). self._cpu_env_ids_all = wp.zeros(N, dtype=wp.int32, device="cpu", pinned=True) diff --git a/source/isaaclab_ov/isaaclab_ov/assets/kernels.py b/source/isaaclab_ov/isaaclab_ov/assets/kernels.py index fc557f68f3c0..14ff26a0fcbe 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/kernels.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/kernels.py @@ -1164,12 +1164,13 @@ def derive_body_acceleration_from_body_com_velocities( @wp.kernel def _body_wrench_to_world( - force_b: wp.array(dtype=wp.vec3f, ndim=2), - torque_b: wp.array(dtype=wp.vec3f, ndim=2), + force_in: wp.array(dtype=wp.vec3f, ndim=2), + torque_in: wp.array(dtype=wp.vec3f, ndim=2), poses: wp.array(dtype=wp.transformf, ndim=2), + is_global: bool, wrench_out: wp.array(dtype=wp.float32, ndim=3), ): - """Rotate body-frame force/torque to world frame and pack into a flat output array. + """Rotate a force/torque to world frame and pack into a flat output array. Output layout per ``(i, j)`` slice (9 floats total): @@ -1178,15 +1179,21 @@ def _body_wrench_to_world( * ``[6:9]`` -- world-frame link position ``[m]`` Args: - force_b: Body-frame applied forces ``[N]``. Shape is ``(N, L)``. - torque_b: Body-frame applied torques ``[N*m]``. Shape is ``(N, L)``. + force_in: Applied forces ``[N]`` in the frame selected by ``is_global``. Shape is ``(N, L)``. + torque_in: Applied torques ``[N*m]`` in the frame selected by ``is_global``. Shape is ``(N, L)``. poses: Link poses in world frame. Shape is ``(N, L)``. + is_global: Whether the input wrench is already in the world frame, in which case no + rotation is applied. wrench_out: Output packed wrench array. Shape is ``(N, L, 9)``. """ i, j = wp.tid() - q = wp.transform_get_rotation(poses[i, j]) - f_w = wp.quat_rotate(q, force_b[i, j]) - t_w = wp.quat_rotate(q, torque_b[i, j]) + if is_global: + f_w = force_in[i, j] + t_w = torque_in[i, j] + else: + q = wp.transform_get_rotation(poses[i, j]) + f_w = wp.quat_rotate(q, force_in[i, j]) + t_w = wp.quat_rotate(q, torque_in[i, j]) wrench_out[i, j, 0] = f_w[0] wrench_out[i, j, 1] = f_w[1] wrench_out[i, j, 2] = f_w[2] @@ -1201,16 +1208,17 @@ def _body_wrench_to_world( @wp.kernel def _body_wrench_to_world_ordered( - force_b: wp.array(dtype=wp.vec3f, ndim=2), - torque_b: wp.array(dtype=wp.vec3f, ndim=2), + force_in: wp.array(dtype=wp.vec3f, ndim=2), + torque_in: wp.array(dtype=wp.vec3f, ndim=2), poses: wp.array(dtype=wp.transformf, ndim=2), user_to_backend: wp.array(dtype=wp.int32), has_ordering: bool, + is_global: bool, wrench_out: wp.array(dtype=wp.float32, ndim=3), ): """Rotate public-order body wrenches to world frame and write them in backend order. - The wrench (``force_b`` / ``torque_b``) is indexed in public body order while the + The wrench (``force_in`` / ``torque_in``) is indexed in public body order while the link ``poses`` are read directly from the backend-order ``LINK_POSE`` buffer. The public body ``user_body_id`` and the backend body ``backend_body_id`` address the same physical body, so its world-frame orientation and position are identical in @@ -1218,12 +1226,16 @@ def _body_wrench_to_world_ordered( pose shadow (no per-substep reorder launch). Args: - force_b: Body-frame applied forces ``[N]`` in public body order. Shape is ``(N, L)``. - torque_b: Body-frame applied torques ``[N*m]`` in public body order. Shape is ``(N, L)``. + force_in: Applied forces ``[N]`` in public body order, in the frame selected by + ``is_global``. Shape is ``(N, L)``. + torque_in: Applied torques ``[N*m]`` in public body order, in the frame selected by + ``is_global``. Shape is ``(N, L)``. poses: Link poses in world frame in backend body order (identity when ``has_ordering`` is False). Shape is ``(N, L)``. user_to_backend: Map from public body index to backend body index. Shape is ``(L,)``. has_ordering: Whether the public-to-backend body map is nonidentity. + is_global: Whether the input wrench is already in the world frame, in which case no + rotation is applied. wrench_out: Output packed wrench array in backend body order. Shape is ``(N, L, 9)`` with ``[0:3]`` world force ``[N]``, ``[3:6]`` world torque ``[N*m]``, ``[6:9]`` world link position ``[m]``. @@ -1232,9 +1244,13 @@ def _body_wrench_to_world_ordered( backend_body_id = user_body_id if has_ordering: backend_body_id = user_to_backend[user_body_id] - q = wp.transform_get_rotation(poses[i, backend_body_id]) - f_w = wp.quat_rotate(q, force_b[i, user_body_id]) - t_w = wp.quat_rotate(q, torque_b[i, user_body_id]) + if is_global: + f_w = force_in[i, user_body_id] + t_w = torque_in[i, user_body_id] + else: + q = wp.transform_get_rotation(poses[i, backend_body_id]) + f_w = wp.quat_rotate(q, force_in[i, user_body_id]) + t_w = wp.quat_rotate(q, torque_in[i, user_body_id]) wrench_out[i, backend_body_id, 0] = f_w[0] wrench_out[i, backend_body_id, 1] = f_w[1] wrench_out[i, backend_body_id, 2] = f_w[2] diff --git a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py index e38a59b0c35f..262c2f57c214 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py @@ -166,17 +166,16 @@ def write_data_to_sim(self) -> None: if inst.active: if perm.active: inst.add_raw_buffers_from(perm) - force_b = inst.out_force_b.warp - torque_b = inst.out_torque_b.warp + composer = inst else: - force_b = perm.out_force_b.warp - torque_b = perm.out_torque_b.warp + composer = perm + force_in, torque_in, is_global = composer.get_forces_and_torques() poses = self._data.body_link_pose_w.warp # (N, 1) wp.transformf wp.launch( _body_wrench_to_world, dim=(self._num_instances, 1), - inputs=[force_b, torque_b, poses], + inputs=[force_in, torque_in, poses, is_global], outputs=[self._wrench_buf], device=self._device, ) @@ -1038,8 +1037,8 @@ def _create_buffers(self) -> None: device=device, copy=False, ) - self._instantaneous_wrench_composer = WrenchComposer(self) - self._permanent_wrench_composer = WrenchComposer(self) + self._instantaneous_wrench_composer = WrenchComposer(self, supports_world_at_com=True) + self._permanent_wrench_composer = WrenchComposer(self, supports_world_at_com=True) # set information about rigid body into data self._data.body_names = self._body_names diff --git a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py index a3ca1621da95..e57ffedfc181 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py @@ -195,17 +195,16 @@ def write_data_to_sim(self) -> None: if inst.active: if perm.active: inst.add_raw_buffers_from(perm) - force_b = inst.out_force_b.warp - torque_b = inst.out_torque_b.warp + composer = inst else: - force_b = perm.out_force_b.warp - torque_b = perm.out_torque_b.warp + composer = perm + force_in, torque_in, is_global = composer.get_forces_and_torques() poses = self._data.body_link_pose_w.warp # (N, B) wp.transformf wp.launch( _body_wrench_to_world, dim=(self._num_instances, self._num_bodies), - inputs=[force_b, torque_b, poses], + inputs=[force_in, torque_in, poses, is_global], outputs=[self._wrench_buf], device=self._device, ) @@ -1193,8 +1192,8 @@ def _create_buffers(self) -> None: # The fused LINK_WRENCH binding writes from a single (N, B, 9) buffer. self._wrench_buf = wp.zeros((N, B, 9), dtype=wp.float32, device=self._device) - self._instantaneous_wrench_composer = WrenchComposer(self) - self._permanent_wrench_composer = WrenchComposer(self) + self._instantaneous_wrench_composer = WrenchComposer(self, supports_world_at_com=True) + self._permanent_wrench_composer = WrenchComposer(self, supports_world_at_com=True) # set information about rigid body into data self._data.body_names = self._body_names_list diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index 0652fad2f06a..e5bdb0890793 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -2479,7 +2479,7 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d num_articulations: Number of articulations to test """ articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=sim.device) # Play the simulator sim.reset() @@ -2499,8 +2499,10 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d # Now we are ready! for i in range(5): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) + # Preserve environment separation when converting the default root pose to world coordinates. + root_pose = articulation.data.default_root_pose.torch.clone() + root_pose[:, :3] += translations + articulation.write_root_pose_to_sim_index(root_pose=root_pose) articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) # reset dof state joint_pos, joint_vel = ( diff --git a/source/isaaclab_physx/changelog.d/wrench-fast-paths.rst b/source/isaaclab_physx/changelog.d/wrench-fast-paths.rst new file mode 100644 index 000000000000..97d67a3b7f69 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/wrench-fast-paths.rst @@ -0,0 +1,7 @@ +Changed +^^^^^^^ + +* Changed the external-wrench writers to submit through + :meth:`~isaaclab.utils.wrench_composer.WrenchComposer.get_forces_and_torques`, so a wrench that is + already local-frame, or already global-frame at the center of mass, is sent to PhysX without + reading the body transforms. diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py index eac8ca94050f..e1950a8ab516 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py @@ -251,7 +251,7 @@ def write_data_to_sim(self): composer.add_raw_buffers_from(self._permanent_wrench_composer) else: composer = self._permanent_wrench_composer - composer.compose_to_body_frame() + force_user, torque_user, is_global = composer.get_forces_and_torques() if self.data.has_body_ordering: force_backend = self._body_wrench_force_backend torque_backend = self._body_wrench_torque_backend @@ -259,8 +259,8 @@ def write_data_to_sim(self): ordering_kernels.reorder_body_wrench_user_to_backend, dim=(self.num_instances, self.num_bodies), inputs=[ - composer.out_force_b.warp, - composer.out_torque_b.warp, + force_user, + torque_user, self.data.body_ordering.backend_to_user, ], outputs=[force_backend, torque_backend], @@ -269,14 +269,13 @@ def write_data_to_sim(self): force_data = force_backend torque_data = torque_backend else: - force_data = composer.out_force_b.warp - torque_data = composer.out_torque_b.warp + force_data, torque_data = force_user, torque_user self.root_view.apply_forces_and_torques_at_position( force_data=force_data.flatten().view(wp.float32), torque_data=torque_data.flatten().view(wp.float32), position_data=None, indices=self._ALL_INDICES, - is_global=False, + is_global=is_global, ) if self._instantaneous_wrench_composer.active: self._instantaneous_wrench_composer.reset() @@ -4003,8 +4002,8 @@ def _create_buffers(self): self._cpu_env_ids_views: dict[int, wp.array] = {} # external wrench composer - self._instantaneous_wrench_composer = WrenchComposer(self) - self._permanent_wrench_composer = WrenchComposer(self) + self._instantaneous_wrench_composer = WrenchComposer(self, supports_world_at_com=True) + self._permanent_wrench_composer = WrenchComposer(self, supports_world_at_com=True) # asset named data self._joint_pos_target_backend: wp.array | None = None diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py index 23951536c649..c190b516cac4 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py @@ -153,19 +153,15 @@ def write_data_to_sim(self) -> None: if self._instantaneous_wrench_composer.active: composer = self._instantaneous_wrench_composer composer.add_raw_buffers_from(self._permanent_wrench_composer) - get_force_data = self._get_inst_wrench_force_f32 - get_torque_data = self._get_inst_wrench_torque_f32 else: composer = self._permanent_wrench_composer - get_force_data = self._get_perm_wrench_force_f32 - get_torque_data = self._get_perm_wrench_torque_f32 - composer.compose_to_body_frame() + force_user, torque_user, is_global = composer.get_forces_and_torques() self.root_view.apply_forces_and_torques_at_position( - force_data=get_force_data(), - torque_data=get_torque_data(), + force_data=force_user.flatten().view(wp.float32), + torque_data=torque_user.flatten().view(wp.float32), position_data=None, indices=self._ALL_INDICES, - is_global=False, + is_global=is_global, ) self._instantaneous_wrench_composer.reset() @@ -1029,8 +1025,8 @@ def _create_buffers(self): self._ALL_BODY_INDICES = wp.array(np.arange(self.num_bodies, dtype=np.int32), device=self.device) # external wrench composer - self._instantaneous_wrench_composer = WrenchComposer(self) - self._permanent_wrench_composer = WrenchComposer(self) + self._instantaneous_wrench_composer = WrenchComposer(self, supports_world_at_com=True) + self._permanent_wrench_composer = WrenchComposer(self, supports_world_at_com=True) # set information about rigid body into data self._data.body_names = self.body_names @@ -1040,11 +1036,6 @@ def _create_buffers(self): # Reset to None each time _create_buffers runs (during initialization). self._root_link_pose_w_f32: wp.array | None = None self._root_com_vel_w_f32: wp.array | None = None - # Cached wrench views for write_data_to_sim - self._inst_wrench_force_f32: wp.array | None = None - self._inst_wrench_torque_f32: wp.array | None = None - self._perm_wrench_force_f32: wp.array | None = None - self._perm_wrench_torque_f32: wp.array | None = None # Pre-allocated pinned CPU buffers for PhysX TensorAPI writes. # PhysX requires CPU arrays for "model" property updates (masses, coms, inertias). @@ -1156,34 +1147,6 @@ def _get_root_com_vel_w_f32(self) -> wp.array: self._root_com_vel_w_f32 = self.data._root_com_vel_w.data.view(wp.float32) return self._root_com_vel_w_f32 - def _get_inst_wrench_force_f32(self) -> wp.array: - """Get a cached flattened float32 view of instantaneous wrench force. Invalidated in ``_create_buffers``.""" - if self._inst_wrench_force_f32 is None: - self._inst_wrench_force_f32 = self._instantaneous_wrench_composer.out_force_b.warp.flatten().view( - wp.float32 - ) - return self._inst_wrench_force_f32 - - def _get_inst_wrench_torque_f32(self) -> wp.array: - """Get a cached flattened float32 view of instantaneous wrench torque. Invalidated in ``_create_buffers``.""" - if self._inst_wrench_torque_f32 is None: - self._inst_wrench_torque_f32 = self._instantaneous_wrench_composer.out_torque_b.warp.flatten().view( - wp.float32 - ) - return self._inst_wrench_torque_f32 - - def _get_perm_wrench_force_f32(self) -> wp.array: - """Get a cached flattened float32 view of permanent wrench force. Invalidated in ``_create_buffers``.""" - if self._perm_wrench_force_f32 is None: - self._perm_wrench_force_f32 = self._permanent_wrench_composer.out_force_b.warp.flatten().view(wp.float32) - return self._perm_wrench_force_f32 - - def _get_perm_wrench_torque_f32(self) -> wp.array: - """Get a cached flattened float32 view of permanent wrench torque. Invalidated in ``_create_buffers``.""" - if self._perm_wrench_torque_f32 is None: - self._perm_wrench_torque_f32 = self._permanent_wrench_composer.out_torque_b.warp.flatten().view(wp.float32) - return self._perm_wrench_torque_f32 - def _sim_env_ids_view(self, count: int) -> wp.array: """Return a cached prefix of the simulator-index scratch buffer.""" if count not in self._sim_env_ids_views: diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py index 714a2bd17ad7..3f8afeb8aefa 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py @@ -191,17 +191,15 @@ def write_data_to_sim(self) -> None: composer.add_raw_buffers_from(self._permanent_wrench_composer) else: composer = self._permanent_wrench_composer - composer.compose_to_body_frame() + force_user, torque_user, is_global = composer.get_forces_and_torques() self.root_view.apply_forces_and_torques_at_position( - force_data=self.reshape_data_to_view_2d(composer.out_force_b.warp, device=self.device).view(wp.float32), - torque_data=self.reshape_data_to_view_2d(composer.out_torque_b.warp, device=self.device).view( - wp.float32 - ), + force_data=self.reshape_data_to_view_2d(force_user, device=self.device).view(wp.float32), + torque_data=self.reshape_data_to_view_2d(torque_user, device=self.device).view(wp.float32), position_data=None, indices=self._env_body_ids_to_view_ids( self._ALL_ENV_INDICES, self._ALL_BODY_INDICES, device=self.device ), - is_global=False, + is_global=is_global, ) self._instantaneous_wrench_composer.reset() @@ -1394,8 +1392,8 @@ def _create_buffers(self): self._cpu_view_ids_views: dict[int, wp.array] = {} # external wrench composer - self._instantaneous_wrench_composer = WrenchComposer(self) - self._permanent_wrench_composer = WrenchComposer(self) + self._instantaneous_wrench_composer = WrenchComposer(self, supports_world_at_com=True) + self._permanent_wrench_composer = WrenchComposer(self, supports_world_at_com=True) # set information about rigid body into data self._data.body_names = self.body_names diff --git a/source/isaaclab_physx/isaaclab_physx/benchmark/assets/runtime.py b/source/isaaclab_physx/isaaclab_physx/benchmark/assets/runtime.py index afb7e7e8637e..6e7577734cb6 100644 --- a/source/isaaclab_physx/isaaclab_physx/benchmark/assets/runtime.py +++ b/source/isaaclab_physx/isaaclab_physx/benchmark/assets/runtime.py @@ -122,8 +122,8 @@ def create_test_articulation( object.__setattr__(articulation, "_data", data) # Create mock wrench composers (pass articulation which has num_instances, num_bodies, device properties) - mock_inst_wrench = WrenchComposer(articulation) - mock_perm_wrench = WrenchComposer(articulation) + mock_inst_wrench = WrenchComposer(articulation, supports_world_at_com=True) + mock_perm_wrench = WrenchComposer(articulation, supports_world_at_com=True) object.__setattr__(articulation, "_instantaneous_wrench_composer", mock_inst_wrench) object.__setattr__(articulation, "_permanent_wrench_composer", mock_perm_wrench) From 1370bb6f7ce1ab57b8939dd9e9b8bfde6f425000 Mon Sep 17 00:00:00 2001 From: vidurv-nvidia Date: Wed, 23 Sep 2026 19:58:27 -0700 Subject: [PATCH 06/11] Migrate remaining legacy schema cfg call sites to fragments (#7971) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description Migrates the remaining in-repo users of the legacy physics schema cfgs to schema fragments. These classes are deprecated in 3.1 and removed in 3.2 (#7839), so every site here currently emits `DeprecationWarning`s and would break at removal. The earlier migration (#7838) converted `source/` but never covered `scripts/`, and its grep missed the `*BaseCfg` spellings. On top of that, some new legacy usage has landed since. An AST scan (comments and docstrings excluded) finds **144 live legacy constructions or writer calls in 37 files on `develop`. This PR brings that to 0**, outside the deliberate exclusions listed below. **Scope** - **`scripts/` (29 files):** all tutorials, demos (including the MPM demos), `convert_mesh` / `convert_instanceable`, and three benchmarks. - **Task configs:** `core/reach/reach_env_cfg.py`, `contrib/ur10_particle_push`, and one of the five `contrib/nist` sites. - **Tests:** the camera-pose, first-frame-rendering and OpenCV-distortion sensor tests; the rigid/mass/collision parts of the cable and MPM tests; `test_ovphysx_gravity`. ## Mapping rules Every one of these has caused a real regression in an earlier migration: - **A bare fragment when one covers the fields.** This keeps in-place tuning such as `cfg.rigid_props.disable_gravity = True` working downstream. - **Empty legacy cfgs map only to the core `UsdPhysics*` fragment.** A fragment applies its schema even when all of its fields are `None`. `RigidBodyPropertiesCfg()` therefore becomes `UsdPhysicsRigidBodyCfg()` alone. Adding `PhysxRigidBodyCfg()` would add a `PhysxRigidBodyAPI` the legacy class never applied. - **`enabled_self_collisions` keeps its Newton mirror.** The legacy writer also authors `newton:selfCollisionEnabled`, so these sites emit `NewtonArticulationCfg` next to `PhysxArticulationCfg`. - **`disable_gravity` goes to `PhysxRigidBodyCfg`**, including on Newton-flavoured cfgs. - **`define_*` writer calls become `apply_*(..., create_if_missing=True)`**, which preserves creation. ## Verification **USD-attribute parity at every site, with negative controls.** Legacy and fragment forms are authored through the real spawners on sibling prims, and the harness diffs `GetPrimTypeInfo().GetAppliedAPISchemas()`, every authored attribute, and relationships. - `scripts/`: 64 groups. 60 are identical. The other 4 differ only in that legacy authored `physics:approximation="none"` explicitly, while the fragment leaves it unset and it resolves to the same `"none"`. 8 of 8 negative controls are flagged. - `source/`: every site is identical, and every negative control is flagged. **Task configs** are also checked end to end. Each task is loaded through its gym `env_cfg_entry_point` on both `develop` and this branch, across presets. All 22 affected spawned entities author identical USD, including the nist Franka on the real asset. A task-level negative control produces a diff. **Execution:** 13 scripts ran headless for 100–300 steps with no schema deprecations: the tutorials, bin_packing, multi_asset, the sensor demos, and tacsl. `convert_mesh` was run end to end for 6 approximations. All 29 scripts compile. The migrated test files pass. **Not runnable in the dev environment, so covered by parity only:** the Newton-physics script paths, including the MPM demos, which need a newer `newton` than was installed locally. Also the `omni.replicator`, ovrtx, and Haply scripts. CI covers the rest. ## Deliberately left on the legacy API - **Spawner, converter, and schema routing internals.** They *are* the legacy path, which must keep working until 3.2. - **Tests that exercise or compare the legacy path**, such as `test_schemas*`, the parity tests, and the legacy arms of the fragment tests. - **`integration_scene_cfgs.py`.** It is a backend-neutral core module, and its `disable_gravity` has no non-PhysX fragment home. - **Everything deformable.** The deformable deprecation PR, stacked on #6673, handles it. - **4 of the 5 `contrib/nist` sites.** They nest a mesh-collision cfg inside `collision_props`. The deprecation messages point users to a spawner `mesh_collision_props` slot, but that slot exists only on `MeshConverterCfg`, not on `UsdFileCfg` or the shape and mesh spawners. There is no supported fragment path for these sites yet. That gap needs its own fix before 3.2. ## Found along the way, not changed here - `RigidBodyMaterialCfg` is deprecated too, and 8 scripts still use it. - `convert_instanceable.py`'s mesh branch is already broken on `develop`: it passes `collision_approximation=` to `MeshConverterCfg`, which raises `TypeError`. - `docs/source/how-to/write_articulation_cfg.rst` still describes the legacy names. ## Type of change - Bug fix (non-breaking change which fixes an issue) ## Checklist - [x] I have read and understood the contribution guidelines - [x] I have run the `pre-commit` checks with `./isaaclab.sh --format` - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added a changelog fragment under `source//changelog.d/` for every touched package - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --- .../benchmarks/benchmark_newton_raycast.py | 4 +- .../benchmarks/benchmark_view_comparison.py | 18 ++--- .../benchmarks/benchmark_xform_prim_view.py | 6 +- scripts/demos/bin_packing.py | 32 +++++---- scripts/demos/haply_teleoperation.py | 6 +- scripts/demos/mpm/newton_mpm_granular.py | 9 +-- .../demos/mpm/newton_mpm_twoway_coupling.py | 9 +-- scripts/demos/mpm/snowball_smash.py | 10 +-- scripts/demos/mpm/teapot_fill.py | 68 +++++++++++-------- scripts/demos/multi_asset.py | 22 +++--- scripts/demos/newton_viewer_dominoes.py | 14 ++-- scripts/demos/pick_and_place.py | 6 +- scripts/demos/sensors/contact_sensor.py | 6 +- .../demos/sensors/frame_transformer_sensor.py | 6 +- scripts/demos/sensors/multi_mesh_raycaster.py | 11 ++- .../sensors/multi_mesh_raycaster_camera.py | 9 ++- .../sensors/newton_raycast_heightfield.py | 4 +- .../sensors/newton_raycast_moving_geometry.py | 14 ++-- scripts/demos/sensors/ppisp_camera.py | 6 +- scripts/demos/sensors/ppisp_camera_ovrtx.py | 6 +- scripts/demos/sensors/tacsl_sensor.py | 38 +++++------ scripts/tools/convert_instanceable.py | 6 +- scripts/tools/convert_mesh.py | 32 +++++---- scripts/tutorials/01_assets/add_new_robot.py | 13 ++-- .../tutorials/01_assets/run_rigid_object.py | 6 +- .../tutorials/03_envs/create_cube_base_env.py | 5 +- .../tutorials/04_sensors/run_ray_caster.py | 6 +- .../tutorials/04_sensors/run_usd_camera.py | 6 +- scripts/tutorials/05_controllers/run_osc.py | 4 +- ...idurv-migrate-legacy-source-remaining.skip | 1 + .../test_camera_pose_write_across_backends.py | 6 +- .../test_first_frame_textured_rendering.py | 7 +- ...idurv-migrate-legacy-source-remaining.skip | 1 + .../test/assets/test_cable_object.py | 6 +- .../test/assets/test_mpm_object.py | 13 ++-- .../test_camera_opencv_distortion_newton.py | 6 +- ...idurv-migrate-legacy-source-remaining.skip | 1 + .../test/physics/test_ovphysx_gravity.py | 4 +- ...vidurv-migrate-legacy-source-remaining.rst | 12 ++++ .../contrib/nist/factory_assets_cfg.py | 4 +- .../ur10_particle_push_env_cfg.py | 36 +++++----- .../core/reach/reach_env_cfg.py | 2 +- 42 files changed, 266 insertions(+), 215 deletions(-) create mode 100644 source/isaaclab/changelog.d/vidurv-migrate-legacy-source-remaining.skip create mode 100644 source/isaaclab_newton/changelog.d/vidurv-migrate-legacy-source-remaining.skip create mode 100644 source/isaaclab_ov/changelog.d/vidurv-migrate-legacy-source-remaining.skip create mode 100644 source/isaaclab_tasks/changelog.d/vidurv-migrate-legacy-source-remaining.rst diff --git a/scripts/benchmarks/benchmark_newton_raycast.py b/scripts/benchmarks/benchmark_newton_raycast.py index a96cd2365cc8..cd24441d4eae 100644 --- a/scripts/benchmarks/benchmark_newton_raycast.py +++ b/scripts/benchmarks/benchmark_newton_raycast.py @@ -81,8 +81,8 @@ class RaycastBenchSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/SensorBody", spawn=sim_utils.CuboidCfg( size=(0.1, 0.1, 0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), ) diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 868f19115e3f..e40e7af8c33b 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -101,9 +101,9 @@ def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> dict[str, fl object_cfg = sim_utils.ConeCfg( radius=0.15, height=0.5, - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), ) for i in range(args_cli.num_envs): @@ -151,9 +151,9 @@ class _SceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), ) @@ -207,9 +207,9 @@ def benchmark_physx(num_iterations: int) -> dict[str, float]: object_cfg = sim_utils.ConeCfg( radius=0.15, height=0.5, - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), ) for i in range(args_cli.num_envs): diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 13fd08530c46..1f2006ed84a0 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -63,9 +63,9 @@ class _NewtonSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Object", spawn=sim_utils.CuboidCfg( size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), ) diff --git a/scripts/demos/bin_packing.py b/scripts/demos/bin_packing.py index 3f8cf063e2a0..c03dbfc8c5f3 100644 --- a/scripts/demos/bin_packing.py +++ b/scripts/demos/bin_packing.py @@ -55,6 +55,7 @@ import torch import warp as wp +from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -148,10 +149,10 @@ def spawn_grocery( ) -> Usd.Prim: """Spawn a visual YCB model and author the physics schemas it does not ship with. - These models carry no physics schemas, so the ``rigid_props`` and ``collision_props`` of a - spawner configuration would find nothing to modify. This spawner defines each schema - instead: the rigid body on the asset root and a convex-hull collider on the mesh, matching - how the physics-ready YCB models are authored. + These models carry no physics schemas, so the ``collision_props`` of a spawner configuration + would author the collider on the asset root rather than on its meshes. This spawner defines + each schema instead: the rigid body on the asset root and a convex-hull collider on the mesh, + matching how the physics-ready YCB models are authored. Args: prim_path: Prim path to spawn the asset at. @@ -167,12 +168,16 @@ def spawn_grocery( root_path = prim.GetPath().pathString for mesh in sim_utils.get_all_matching_child_prims(root_path, lambda child: child.GetTypeName() == "Mesh"): mesh_path = mesh.GetPath().pathString - schemas.define_collision_properties(mesh_path, schemas.CollisionBaseCfg(collision_enabled=True)) - schemas.define_mesh_collision_properties( - mesh_path, schemas.MeshCollisionBaseCfg(mesh_approximation_name="convexHull") + schemas.apply_collision_properties( + mesh_path, [schemas.UsdPhysicsCollisionCfg(collision_enabled=True)], create_if_missing=True ) - schemas.define_rigid_body_properties(root_path, sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4)) - schemas.define_mass_properties(root_path, schemas.MassPropertiesCfg(mass=GROCERY_MASS)) + schemas.apply_mesh_collision_properties( + mesh_path, [schemas.UsdPhysicsMeshCollisionCfg(mesh_approximation_name="convexHull")] + ) + schemas.apply_rigid_body_properties( + root_path, [PhysxRigidBodyCfg(solver_position_iteration_count=4)], create_if_missing=True + ) + schemas.apply_mass_properties(root_path, [schemas.MassCfg(mass=GROCERY_MASS)], create_if_missing=True) return prim @@ -240,10 +245,11 @@ class BinPackingSceneCfg(InteractiveSceneCfg): spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/KLT_Bin/small_KLT.usd", scale=(2.0, 2.0, 2.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg( - solver_position_iteration_count=4, solver_velocity_iteration_count=0, kinematic_enabled=True - ), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + rigid_props=[ + sim_utils.UsdPhysicsRigidBodyCfg(kinematic_enabled=True), + PhysxRigidBodyCfg(solver_position_iteration_count=4, solver_velocity_iteration_count=0), + ], + mass_props=sim_utils.MassCfg(mass=1.0), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.15)), ) diff --git a/scripts/demos/haply_teleoperation.py b/scripts/demos/haply_teleoperation.py index b9c6bdc668bf..eb88f728333b 100644 --- a/scripts/demos/haply_teleoperation.py +++ b/scripts/demos/haply_teleoperation.py @@ -175,9 +175,9 @@ class FrankaHaplySceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( size=(0.06, 0.06, 0.06), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=0.5), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=0.5), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.5, dynamic_friction=0.5), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.8, 0.2), metallic=0.2), ), diff --git a/scripts/demos/mpm/newton_mpm_granular.py b/scripts/demos/mpm/newton_mpm_granular.py index a7c009b5e516..844f49f24444 100644 --- a/scripts/demos/mpm/newton_mpm_granular.py +++ b/scripts/demos/mpm/newton_mpm_granular.py @@ -115,6 +115,7 @@ def create_sim_cfg(): def create_scene_cfg(): """Create an Isaac Lab scene config using declarative assets.""" from isaaclab_newton.assets import MPMObjectCfg + from isaaclab_newton.sim.schemas import NewtonCollisionCfg from isaaclab_newton.sim.spawners.mpm import MPMGridCfg import isaaclab.sim as sim_utils @@ -127,10 +128,10 @@ def collider_cfg(prim_path: str, center, half_extents, orientation, friction: fl prim_path=prim_path, spawn=sim_utils.CuboidCfg( size=(2.0 * half_extents[0], 2.0 * half_extents[1], 2.0 * half_extents[2]), - collision_props=sim_utils.NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=COLLIDER_MARGIN, - ), + collision_props=[ + sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=COLLIDER_MARGIN), + ], physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=friction, dynamic_friction=friction, diff --git a/scripts/demos/mpm/newton_mpm_twoway_coupling.py b/scripts/demos/mpm/newton_mpm_twoway_coupling.py index af648f19828f..d841b424de3f 100644 --- a/scripts/demos/mpm/newton_mpm_twoway_coupling.py +++ b/scripts/demos/mpm/newton_mpm_twoway_coupling.py @@ -161,6 +161,7 @@ def create_sim_cfg(): def create_scene_cfg(): """Create the declarative rigid-sphere and granular-bath scene.""" from isaaclab_newton.assets.mpm_object import MPMObjectCfg + from isaaclab_newton.sim.schemas import NewtonCollisionCfg from isaaclab_newton.sim.spawners.mpm import MPMGridCfg, MPMParticleMaterialCfg from isaaclab.assets import AssetBaseCfg, RigidObjectCfg, RigidObjectCollectionCfg @@ -183,7 +184,7 @@ def bath_collider( else sim_utils.spawn_cuboid ), size=size, - collision_props=sim_utils.NewtonCollisionPropertiesCfg(contact_margin=COLLIDER_MARGIN), + collision_props=NewtonCollisionCfg(contact_margin=COLLIDER_MARGIN), physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=0.6, dynamic_friction=0.6, @@ -203,9 +204,9 @@ def bath_collider( color=SPHERE_COLORS[index], ), radius=SPHERE_RADIUS, - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=SPHERE_MASS), - collision_props=sim_utils.NewtonCollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=SPHERE_MASS), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=0.5, dynamic_friction=0.5, diff --git a/scripts/demos/mpm/snowball_smash.py b/scripts/demos/mpm/snowball_smash.py index 0be3833d4aed..e908ab22ba8f 100644 --- a/scripts/demos/mpm/snowball_smash.py +++ b/scripts/demos/mpm/snowball_smash.py @@ -231,9 +231,9 @@ def crate_cfg(index: int) -> RigidObjectCfg: prim_path=f"{{ENV_REGEX_NS}}/Crate_{index}", spawn=sim_utils.CuboidCfg( size=(CRATE_SIZE, CRATE_SIZE, CRATE_SIZE), - rigid_props=sim_utils.NewtonRigidBodyPropertiesCfg(rigid_body_enabled=True), - mass_props=sim_utils.MassPropertiesCfg(mass=args_cli.crate_mass), - collision_props=sim_utils.NewtonCollisionPropertiesCfg(collision_enabled=True), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(rigid_body_enabled=True), + mass_props=sim_utils.MassCfg(mass=args_cli.crate_mass), + collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=CRATE_FRICTION, dynamic_friction=CRATE_FRICTION, @@ -289,11 +289,11 @@ class SnowballSmashSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/MPMGround", spawn=sim_utils.CuboidCfg( size=(12.0, 12.0, 0.10), - rigid_props=sim_utils.NewtonRigidBodyPropertiesCfg( + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg( rigid_body_enabled=True, kinematic_enabled=True, ), - collision_props=sim_utils.NewtonCollisionPropertiesCfg(collision_enabled=True), + collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=CRATE_FRICTION, dynamic_friction=CRATE_FRICTION, diff --git a/scripts/demos/mpm/teapot_fill.py b/scripts/demos/mpm/teapot_fill.py index a3055a6b6a8f..4ac192676dda 100644 --- a/scripts/demos/mpm/teapot_fill.py +++ b/scripts/demos/mpm/teapot_fill.py @@ -433,12 +433,20 @@ def spawn_demo_mesh( if not cfg.visible: UsdGeom.Imageable(stage.GetPrimAtPath(mesh_prim_path)).MakeInvisible() + def as_fragments(value) -> list: + # the schema slots accept a bare fragment or a list of fragments + return list(value) if isinstance(value, (list, tuple)) else [value] + if cfg.collision_props is not None: - schemas.define_collision_properties(mesh_prim_path, cfg.collision_props, stage=stage) + schemas.apply_collision_properties( + mesh_prim_path, as_fragments(cfg.collision_props), create_if_missing=True, stage=stage + ) if cfg.mesh_collision_props is not None: - schemas.define_mesh_collision_properties(mesh_prim_path, cfg.mesh_collision_props, stage=stage) + schemas.apply_mesh_collision_properties(mesh_prim_path, as_fragments(cfg.mesh_collision_props), stage=stage) if cfg.rigid_props is not None: - schemas.define_rigid_body_properties(prim_path, cfg.rigid_props, stage=stage) + schemas.apply_rigid_body_properties( + prim_path, as_fragments(cfg.rigid_props), create_if_missing=True, stage=stage + ) if cfg.visual_material is not None: material_path = cfg.visual_material_path @@ -585,7 +593,9 @@ def create_sim_cfg(): def create_scene_cfg(container_usd: str, island_usd: str | None, bowl_usd: str | None): """Create the teapot-fill scene using declarative Isaac Lab assets.""" from isaaclab_newton.assets import MPMObjectCfg + from isaaclab_newton.sim.schemas import NewtonCollisionCfg from isaaclab_newton.sim.spawners.mpm import MPMParticleMaterialCfg, MPMPointsCfg + from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg, RigidObjectCfg @@ -605,7 +615,12 @@ class DemoMeshCfg(sim_utils.MeshCfg): func: Callable | str = clone(spawn_demo_mesh) vertices: list[list[float]] = MISSING faces: list[list[int]] = MISSING - mesh_collision_props: sim_utils.NewtonMeshCollisionPropertiesCfg | None = None + mesh_collision_props: sim_utils.UsdPhysicsMeshCollisionCfg | None = None + + # The visual assets only disable rigid bodies and colliders they already carry; the explicit + # target mappings keep an asset without physics from gaining a body on its spawn prim. + disable_asset_rigid_bodies = {"(/.*)?": [sim_utils.UsdPhysicsRigidBodyCfg(rigid_body_enabled=False)]} + disable_asset_colliders = {"(/.*)?": [sim_utils.UsdPhysicsCollisionCfg(collision_enabled=False)]} island_cfg = None if island_usd is not None: @@ -615,8 +630,8 @@ class DemoMeshCfg(sim_utils.MeshCfg): usd_path=island_usd, variants={"Physics": "none"}, make_uninstanceable=True, - rigid_props=sim_utils.NewtonRigidBodyPropertiesCfg(rigid_body_enabled=False), - collision_props=sim_utils.NewtonCollisionPropertiesCfg(collision_enabled=False), + rigid_props=disable_asset_rigid_bodies, + collision_props=disable_asset_colliders, ), init_state=AssetBaseCfg.InitialStateCfg(rot=TABLE_ORIENTATION), ) @@ -630,8 +645,8 @@ class DemoMeshCfg(sim_utils.MeshCfg): scale=(BOWL_SCALE,) * 3, variants={"Physics": "none"}, make_uninstanceable=True, - rigid_props=sim_utils.NewtonRigidBodyPropertiesCfg(rigid_body_enabled=False), - collision_props=sim_utils.NewtonCollisionPropertiesCfg(collision_enabled=False), + rigid_props=disable_asset_rigid_bodies, + collision_props=disable_asset_colliders, ), init_state=AssetBaseCfg.InitialStateCfg(pos=BOWL_BASE_POS), ) @@ -646,10 +661,10 @@ class TeapotFillSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/TabletopCollider", spawn=sim_utils.CuboidCfg( size=(2.0 * TABLE_HALF_EXTENTS[0], 2.0 * TABLE_HALF_EXTENTS[1], 2.0 * TABLE_HALF_EXTENTS[2]), - collision_props=sim_utils.NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=COLLIDER_MARGIN, - ), + collision_props=[ + sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=COLLIDER_MARGIN), + ], physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=TABLE_FRICTION, dynamic_friction=TABLE_FRICTION, @@ -674,11 +689,11 @@ class TeapotFillSceneCfg(InteractiveSceneCfg): spawn=DemoMeshCfg( vertices=(BOWL_SCALE * bowl_vertices).tolist(), faces=bowl_faces.tolist(), - collision_props=sim_utils.NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=COLLIDER_MARGIN, - ), - mesh_collision_props=sim_utils.NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="none"), + collision_props=[ + sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=COLLIDER_MARGIN), + ], + mesh_collision_props=sim_utils.UsdPhysicsMeshCollisionCfg(mesh_approximation_name="none"), physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=BOWL_FRICTION, dynamic_friction=BOWL_FRICTION, @@ -701,16 +716,15 @@ class TeapotFillSceneCfg(InteractiveSceneCfg): spawn=DemoMeshCfg( vertices=container_vertices.tolist(), faces=container_faces.tolist(), - rigid_props=sim_utils.NewtonRigidBodyPropertiesCfg( - rigid_body_enabled=True, - kinematic_enabled=True, - disable_gravity=True, - ), - collision_props=sim_utils.NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=COLLIDER_MARGIN, - ), - mesh_collision_props=sim_utils.NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="none"), + rigid_props=[ + sim_utils.UsdPhysicsRigidBodyCfg(rigid_body_enabled=True, kinematic_enabled=True), + PhysxRigidBodyCfg(disable_gravity=True), + ], + collision_props=[ + sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=COLLIDER_MARGIN), + ], + mesh_collision_props=sim_utils.UsdPhysicsMeshCollisionCfg(mesh_approximation_name="none"), physics_material=sim_utils.NewtonMaterialPropertiesCfg( static_friction=CONTAINER_FRICTION, dynamic_friction=CONTAINER_FRICTION, diff --git a/scripts/demos/multi_asset.py b/scripts/demos/multi_asset.py index 98e01ce578cd..d7ee5ab2a6c5 100644 --- a/scripts/demos/multi_asset.py +++ b/scripts/demos/multi_asset.py @@ -45,6 +45,9 @@ # parse the arguments args_cli = parser.parse_args() +from isaaclab_newton.sim.schemas import NewtonArticulationCfg +from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg + import isaaclab.sim as sim_utils ## @@ -71,11 +74,9 @@ GOLD_MATERIAL = {"visual_material": sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.75, 0.0), metallic=0.2)} PURPLE_MATERIAL = {"visual_material": sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 1.0), metallic=0.2)} OBJECT_PHYSICS = { - "rigid_props": sim_utils.RigidBodyPropertiesCfg( - solver_position_iteration_count=4, solver_velocity_iteration_count=0 - ), - "mass_props": sim_utils.MassPropertiesCfg(mass=1.0), - "collision_props": sim_utils.CollisionPropertiesCfg(), + "rigid_props": PhysxRigidBodyCfg(solver_position_iteration_count=4, solver_velocity_iteration_count=0), + "mass_props": sim_utils.MassCfg(mass=1.0), + "collision_props": sim_utils.UsdPhysicsCollisionCfg(), } ## @@ -146,7 +147,7 @@ class MultiObjectSceneCfg(InteractiveSceneCfg): f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd", ], random_choice=False, - rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_props=PhysxRigidBodyCfg( disable_gravity=False, retain_accelerations=False, linear_damping=0.0, @@ -155,9 +156,12 @@ class MultiObjectSceneCfg(InteractiveSceneCfg): max_angular_velocity=1000.0, max_depenetration_velocity=1.0, ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 - ), + articulation_props=[ + PhysxArticulationCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + NewtonArticulationCfg(self_collision_enabled=True), + ], activate_contact_sensors=True, ), init_state=ArticulationCfg.InitialStateCfg( diff --git a/scripts/demos/newton_viewer_dominoes.py b/scripts/demos/newton_viewer_dominoes.py index d0c4275dc256..92adf888df66 100644 --- a/scripts/demos/newton_viewer_dominoes.py +++ b/scripts/demos/newton_viewer_dominoes.py @@ -56,9 +56,9 @@ def _domino_cfg(position: tuple[float, float, float], orientation: tuple[float, prim_path="", spawn=sim_utils.CuboidCfg( size=DOMINO_SIZE, - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(density=580.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(density=580.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg( static_friction=1.0, dynamic_friction=1.0, @@ -76,9 +76,9 @@ def _trigger_cfg() -> RigidObjectCfg: prim_path="/World/Dominoes/Trigger", spawn=sim_utils.CuboidCfg( size=(DOMINO_SIZE[1], LOGO_FOOTPRINT[1], DOMINO_SIZE[2]), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(density=20.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(density=20.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg( static_friction=1.0, dynamic_friction=1.0, @@ -97,7 +97,7 @@ class DominoSceneCfg(InteractiveSceneCfg): prim_path="/World/Floor", spawn=sim_utils.CuboidCfg( size=(LOGO_FOOTPRINT[0] + 4.0, LOGO_FOOTPRINT[1] + 4.0, 0.10), - collision_props=sim_utils.CollisionPropertiesCfg(), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg( static_friction=1.0, dynamic_friction=1.0, diff --git a/scripts/demos/pick_and_place.py b/scripts/demos/pick_and_place.py index e240a00faba4..919894a4c767 100644 --- a/scripts/demos/pick_and_place.py +++ b/scripts/demos/pick_and_place.py @@ -59,9 +59,9 @@ class PickAndPlaceSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Robot/Cube", spawn=sim_utils.CuboidCfg( size=(0.4, 0.4, 0.4), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 0.0, 0.8)), ), init_state=RigidObjectCfg.InitialStateCfg(), diff --git a/scripts/demos/sensors/contact_sensor.py b/scripts/demos/sensors/contact_sensor.py index 23e0a64711cb..5e80727c0b5a 100644 --- a/scripts/demos/sensors/contact_sensor.py +++ b/scripts/demos/sensors/contact_sensor.py @@ -66,9 +66,9 @@ class ContactSensorSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( size=(0.5, 0.5, 0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=100.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=100.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=1.0), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), ), diff --git a/scripts/demos/sensors/frame_transformer_sensor.py b/scripts/demos/sensors/frame_transformer_sensor.py index 6d9f6d6fb165..2c085ff61dfd 100644 --- a/scripts/demos/sensors/frame_transformer_sensor.py +++ b/scripts/demos/sensors/frame_transformer_sensor.py @@ -63,9 +63,9 @@ class FrameTransformerSensorSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( size=(1, 1, 1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=100.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=100.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=1.0), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), ), diff --git a/scripts/demos/sensors/multi_mesh_raycaster.py b/scripts/demos/sensors/multi_mesh_raycaster.py index 9b416dd68078..c6fbd83c32f1 100644 --- a/scripts/demos/sensors/multi_mesh_raycaster.py +++ b/scripts/demos/sensors/multi_mesh_raycaster.py @@ -63,6 +63,7 @@ import random import torch +from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg, RigidObjectCfg @@ -86,7 +87,7 @@ if args_cli.flat_ground: ground_spawn_cfg = sim_utils.MeshCuboidCfg( size=(20.0, 20.0, 0.1), - collision_props=sim_utils.CollisionPropertiesCfg(), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ) ground_init_state = AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -0.05)) else: @@ -184,11 +185,9 @@ spawn=sim_utils.MultiAssetSpawnerCfg( assets_cfg=object_assets_cfg, random_choice=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - solver_position_iteration_count=4, solver_velocity_iteration_count=0 - ), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=PhysxRigidBodyCfg(solver_position_iteration_count=4, solver_velocity_iteration_count=0), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), ) diff --git a/scripts/demos/sensors/multi_mesh_raycaster_camera.py b/scripts/demos/sensors/multi_mesh_raycaster_camera.py index 259c56c6df9e..73b80be2f42f 100644 --- a/scripts/demos/sensors/multi_mesh_raycaster_camera.py +++ b/scripts/demos/sensors/multi_mesh_raycaster_camera.py @@ -55,6 +55,7 @@ import random import torch +from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg from pxr import Gf, Sdf @@ -165,11 +166,9 @@ ), ], random_choice=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - solver_position_iteration_count=4, solver_velocity_iteration_count=0 - ), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=PhysxRigidBodyCfg(solver_position_iteration_count=4, solver_velocity_iteration_count=0), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), ) diff --git a/scripts/demos/sensors/newton_raycast_heightfield.py b/scripts/demos/sensors/newton_raycast_heightfield.py index f37ff0d87ef4..553f844f82a6 100644 --- a/scripts/demos/sensors/newton_raycast_heightfield.py +++ b/scripts/demos/sensors/newton_raycast_heightfield.py @@ -70,8 +70,8 @@ class HeightfieldSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/SensorBody", spawn=sim_utils.CuboidCfg( size=(0.4, 0.25, 0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.9, 0.6, 0.1)), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.5)), diff --git a/scripts/demos/sensors/newton_raycast_moving_geometry.py b/scripts/demos/sensors/newton_raycast_moving_geometry.py index 271e765fdbd3..7940c1200b6d 100644 --- a/scripts/demos/sensors/newton_raycast_moving_geometry.py +++ b/scripts/demos/sensors/newton_raycast_moving_geometry.py @@ -55,9 +55,9 @@ def _falling_box_cfg(index: int) -> RigidObjectCfg: prim_path=f"{{ENV_REGEX_NS}}/Box_{index}", spawn=sim_utils.CuboidCfg( size=(0.5, 0.5, 0.5), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.4 + 0.2 * index, 0.9 - 0.3 * index)), ), init_state=RigidObjectCfg.InitialStateCfg(pos=BOX_DROP_POSITIONS[index]), @@ -78,8 +78,8 @@ class MovingGeometrySceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Bar", spawn=sim_utils.CuboidCfg( size=(3.5, 0.3, 0.3), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.9, 0.2, 0.5)), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.8)), @@ -89,8 +89,8 @@ class MovingGeometrySceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/SensorBody", spawn=sim_utils.CuboidCfg( size=(0.3, 0.3, 0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.9, 0.6, 0.1)), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.5)), diff --git a/scripts/demos/sensors/ppisp_camera.py b/scripts/demos/sensors/ppisp_camera.py index ed39ae2b1d45..83c31bd03ace 100644 --- a/scripts/demos/sensors/ppisp_camera.py +++ b/scripts/demos/sensors/ppisp_camera.py @@ -157,9 +157,9 @@ class PpispCameraSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Anchor", spawn=sim_utils.CuboidCfg( size=(0.01, 0.01, 0.01), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=0.001), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=0.001), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 0.0)), ), diff --git a/scripts/demos/sensors/ppisp_camera_ovrtx.py b/scripts/demos/sensors/ppisp_camera_ovrtx.py index 11f7422fee7b..597aed18eeda 100644 --- a/scripts/demos/sensors/ppisp_camera_ovrtx.py +++ b/scripts/demos/sensors/ppisp_camera_ovrtx.py @@ -143,9 +143,9 @@ class PpispCameraOvrtxSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Anchor", spawn=sim_utils.CuboidCfg( size=(0.01, 0.01, 0.01), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=0.001), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=0.001), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=sim_utils.RigidBodyMaterialCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 0.0)), ), diff --git a/scripts/demos/sensors/tacsl_sensor.py b/scripts/demos/sensors/tacsl_sensor.py index 1271daff0256..4699ef54db53 100644 --- a/scripts/demos/sensors/tacsl_sensor.py +++ b/scripts/demos/sensors/tacsl_sensor.py @@ -83,11 +83,8 @@ """Rest everything follows.""" -from isaaclab_physx.sim.schemas import ( - PhysxArticulationRootPropertiesCfg, - PhysxCollisionPropertiesCfg, - PhysxRigidBodyPropertiesCfg, -) +from isaaclab_newton.sim.schemas import NewtonArticulationCfg +from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxCollisionCfg, PhysxRigidBodyCfg from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg import isaaclab.sim as sim_utils @@ -121,19 +118,22 @@ class TactileSensorsSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Robot", spawn=sim_utils.UsdFileWithCompliantContactCfg( usd_path=f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd", - rigid_props=PhysxRigidBodyPropertiesCfg( + rigid_props=PhysxRigidBodyCfg( disable_gravity=True, max_depenetration_velocity=5.0, ), compliant_contact_stiffness=args_cli.tactile_compliance_stiffness, compliant_contact_damping=args_cli.tactile_compliant_damping, physics_material_prim_path="elastomer", - articulation_props=PhysxArticulationRootPropertiesCfg( - enabled_self_collisions=False, - solver_position_iteration_count=12, - solver_velocity_iteration_count=1, - ), - collision_props=PhysxCollisionPropertiesCfg(contact_offset=0.001, rest_offset=-0.0005), + articulation_props=[ + PhysxArticulationCfg( + enabled_self_collisions=False, + solver_position_iteration_count=12, + solver_velocity_iteration_count=1, + ), + NewtonArticulationCfg(self_collision_enabled=False), + ], + collision_props=PhysxCollisionCfg(contact_offset=0.001, rest_offset=-0.0005), ), init_state=ArticulationCfg.InitialStateCfg( pos=(0.0, 0.0, 0.5), @@ -188,9 +188,9 @@ class CubeTactileSceneCfg(TactileSensorsSceneCfg): prim_path="{ENV_REGEX_NS}/contact_object", spawn=sim_utils.CuboidCfg( size=(0.01, 0.01, 0.01), - rigid_props=PhysxRigidBodyPropertiesCfg(disable_gravity=True), - mass_props=sim_utils.MassPropertiesCfg(mass=0.00327211), - collision_props=PhysxCollisionPropertiesCfg(), + rigid_props=PhysxRigidBodyCfg(disable_gravity=True), + mass_props=sim_utils.MassCfg(mass=0.00327211), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=PhysxRigidBodyMaterialCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.1, 0.1)), ), @@ -207,15 +207,15 @@ class NutTactileSceneCfg(TactileSensorsSceneCfg): prim_path="{ENV_REGEX_NS}/contact_object", spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_nut_m16.usd", - rigid_props=PhysxRigidBodyPropertiesCfg( + rigid_props=PhysxRigidBodyCfg( disable_gravity=True, solver_position_iteration_count=12, solver_velocity_iteration_count=1, max_angular_velocity=180.0, ), - mass_props=sim_utils.MassPropertiesCfg(mass=0.1), - collision_props=PhysxCollisionPropertiesCfg(contact_offset=0.005, rest_offset=0), - articulation_props=PhysxArticulationRootPropertiesCfg(articulation_enabled=False), + mass_props=sim_utils.MassCfg(mass=0.1), + collision_props=PhysxCollisionCfg(contact_offset=0.005, rest_offset=0), + articulation_props=PhysxArticulationCfg(articulation_enabled=False), ), init_state=RigidObjectCfg.InitialStateCfg( pos=(0.0, 0.0 + 0.06776, 0.498), diff --git a/scripts/tools/convert_instanceable.py b/scripts/tools/convert_instanceable.py index 7713bdc728f3..527fcc71e54d 100644 --- a/scripts/tools/convert_instanceable.py +++ b/scripts/tools/convert_instanceable.py @@ -126,14 +126,14 @@ def main(): ): # Mass properties if args_cli.mass is not None: - mass_props = schemas_cfg.MassPropertiesCfg(mass=args_cli.mass) - rigid_props = schemas_cfg.RigidBodyPropertiesCfg() + mass_props = schemas_cfg.MassCfg(mass=args_cli.mass) + rigid_props = schemas_cfg.UsdPhysicsRigidBodyCfg() else: mass_props = None rigid_props = None # Collision properties - collision_props = schemas_cfg.CollisionPropertiesCfg( + collision_props = schemas_cfg.UsdPhysicsCollisionCfg( collision_enabled=args_cli.collision_approximation != "none" ) # Mesh converter call diff --git a/scripts/tools/convert_mesh.py b/scripts/tools/convert_mesh.py index 0493bc3f2ae2..fd39f5c2de99 100644 --- a/scripts/tools/convert_mesh.py +++ b/scripts/tools/convert_mesh.py @@ -98,14 +98,16 @@ from isaaclab.utils.assets import check_file_path from isaaclab.utils.dict import print_dict +# Mesh-collision approximation token authored for each collision approximation choice. +# A triangle-mesh collider uses the "none" token (the mesh itself is the collider). collision_approximation_map = { - "convexDecomposition": schemas_cfg.ConvexDecompositionPropertiesCfg, - "convexHull": schemas_cfg.ConvexHullPropertiesCfg, - "triangleMesh": schemas_cfg.TriangleMeshPropertiesCfg, - "meshSimplification": schemas_cfg.TriangleMeshSimplificationPropertiesCfg, - "sdf": schemas_cfg.SDFMeshPropertiesCfg, - "boundingCube": schemas_cfg.BoundingCubePropertiesCfg, - "boundingSphere": schemas_cfg.BoundingSpherePropertiesCfg, + "convexDecomposition": "convexDecomposition", + "convexHull": "convexHull", + "triangleMesh": "none", + "meshSimplification": "meshSimplification", + "sdf": "sdf", + "boundingCube": "boundingCube", + "boundingSphere": "boundingSphere", "none": None, } @@ -125,24 +127,28 @@ def main(): # Mass properties if args_cli.mass is not None: - mass_props = schemas_cfg.MassPropertiesCfg(mass=args_cli.mass) - rigid_props = schemas_cfg.RigidBodyPropertiesCfg() + mass_props = schemas_cfg.MassCfg(mass=args_cli.mass) + rigid_props = schemas_cfg.UsdPhysicsRigidBodyCfg() else: mass_props = None rigid_props = None # Collision properties - collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=args_cli.collision_approximation != "none") + collision_props = schemas_cfg.UsdPhysicsCollisionCfg(collision_enabled=args_cli.collision_approximation != "none") # Create Mesh converter config - cfg_class = collision_approximation_map.get(args_cli.collision_approximation) - if cfg_class is None and args_cli.collision_approximation != "none": + approximation_name = collision_approximation_map.get(args_cli.collision_approximation) + if approximation_name is None and args_cli.collision_approximation != "none": valid_keys = ", ".join(sorted(collision_approximation_map.keys())) raise ValueError( f"Invalid collision approximation type '{args_cli.collision_approximation}'. " f"Valid options are: {valid_keys}." ) - collision_cfg = cfg_class() if cfg_class is not None else None + collision_cfg = ( + schemas_cfg.UsdPhysicsMeshCollisionCfg(mesh_approximation_name=approximation_name) + if approximation_name is not None + else None + ) mesh_converter_cfg = MeshConverterCfg( mass_props=mass_props, diff --git a/scripts/tutorials/01_assets/add_new_robot.py b/scripts/tutorials/01_assets/add_new_robot.py index ca0c477ef587..64f078e68a9b 100644 --- a/scripts/tutorials/01_assets/add_new_robot.py +++ b/scripts/tutorials/01_assets/add_new_robot.py @@ -23,6 +23,8 @@ import numpy as np import torch +from isaaclab_newton.sim.schemas import NewtonArticulationCfg +from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg @@ -39,13 +41,16 @@ DOFBOT_CONFIG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Yahboom/Dofbot/dofbot.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_props=PhysxRigidBodyCfg( disable_gravity=False, max_depenetration_velocity=5.0, ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 - ), + articulation_props=[ + PhysxArticulationCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 + ), + NewtonArticulationCfg(self_collision_enabled=True), + ], ), init_state=ArticulationCfg.InitialStateCfg( joint_pos={ diff --git a/scripts/tutorials/01_assets/run_rigid_object.py b/scripts/tutorials/01_assets/run_rigid_object.py index 266cac7b0b1b..52149192dbb1 100644 --- a/scripts/tutorials/01_assets/run_rigid_object.py +++ b/scripts/tutorials/01_assets/run_rigid_object.py @@ -62,9 +62,9 @@ def design_scene(): spawn=sim_utils.ConeCfg( radius=0.1, height=0.2, - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2), ), init_state=RigidObjectCfg.InitialStateCfg(), diff --git a/scripts/tutorials/03_envs/create_cube_base_env.py b/scripts/tutorials/03_envs/create_cube_base_env.py index 08d203b9e638..b9dc0b43d48a 100644 --- a/scripts/tutorials/03_envs/create_cube_base_env.py +++ b/scripts/tutorials/03_envs/create_cube_base_env.py @@ -51,6 +51,7 @@ """Rest everything follows.""" import torch +from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg import isaaclab.envs.mdp as mdp import isaaclab.sim as sim_utils @@ -181,8 +182,8 @@ class MySceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/cube", spawn=sim_utils.CuboidCfg( size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(max_depenetration_velocity=1.0, disable_gravity=True), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + rigid_props=PhysxRigidBodyCfg(max_depenetration_velocity=1.0, disable_gravity=True), + mass_props=sim_utils.MassCfg(mass=1.0), physics_material=sim_utils.RigidBodyMaterialCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 0.0)), ), diff --git a/scripts/tutorials/04_sensors/run_ray_caster.py b/scripts/tutorials/04_sensors/run_ray_caster.py index 7287df3afc39..d2462e0e9287 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster.py +++ b/scripts/tutorials/04_sensors/run_ray_caster.py @@ -75,9 +75,9 @@ def design_scene() -> dict: prim_path="/World/Origin.*/ball", spawn=sim_utils.SphereCfg( radius=0.25, - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=0.5), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=0.5), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), ), ) diff --git a/scripts/tutorials/04_sensors/run_usd_camera.py b/scripts/tutorials/04_sensors/run_usd_camera.py index 7fa75adb7ada..cbd468b13b95 100644 --- a/scripts/tutorials/04_sensors/run_usd_camera.py +++ b/scripts/tutorials/04_sensors/run_usd_camera.py @@ -140,9 +140,9 @@ def design_scene() -> dict: # choose random prim type prim_type = random.choice(["Cube", "Cone", "Cylinder"]) common_properties = { - "rigid_props": sim_utils.RigidBodyPropertiesCfg(), - "mass_props": sim_utils.MassPropertiesCfg(mass=5.0), - "collision_props": sim_utils.CollisionPropertiesCfg(), + "rigid_props": sim_utils.UsdPhysicsRigidBodyCfg(), + "mass_props": sim_utils.MassCfg(mass=5.0), + "collision_props": sim_utils.UsdPhysicsCollisionCfg(), "visual_material": sim_utils.PreviewSurfaceCfg(diffuse_color=color, metallic=0.5), "semantic_tags": [("class", prim_type)], } diff --git a/scripts/tutorials/05_controllers/run_osc.py b/scripts/tutorials/05_controllers/run_osc.py index f3665010d7ad..7a858830dd72 100644 --- a/scripts/tutorials/05_controllers/run_osc.py +++ b/scripts/tutorials/05_controllers/run_osc.py @@ -80,9 +80,9 @@ class SceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/TiltedWall", spawn=sim_utils.CuboidCfg( size=(2.0, 1.5, 0.01), - collision_props=sim_utils.CollisionPropertiesCfg(), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(kinematic_enabled=True), activate_contact_sensors=True, ), init_state=AssetBaseCfg.InitialStateCfg( diff --git a/source/isaaclab/changelog.d/vidurv-migrate-legacy-source-remaining.skip b/source/isaaclab/changelog.d/vidurv-migrate-legacy-source-remaining.skip new file mode 100644 index 000000000000..8e368b384d8f --- /dev/null +++ b/source/isaaclab/changelog.d/vidurv-migrate-legacy-source-remaining.skip @@ -0,0 +1 @@ +Migrated test scene configurations off the deprecated legacy schema property configs; test-only change. diff --git a/source/isaaclab/test/sensors/test_camera_pose_write_across_backends.py b/source/isaaclab/test/sensors/test_camera_pose_write_across_backends.py index 05fe671ad3dc..a3a3662c3e2d 100644 --- a/source/isaaclab/test/sensors/test_camera_pose_write_across_backends.py +++ b/source/isaaclab/test/sensors/test_camera_pose_write_across_backends.py @@ -50,9 +50,9 @@ class _SceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( size=(0.5, 0.5, 0.5), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.25)), ) diff --git a/source/isaaclab/test/sensors/test_first_frame_textured_rendering.py b/source/isaaclab/test/sensors/test_first_frame_textured_rendering.py index 08d129b510d7..c7a5edc3ad86 100644 --- a/source/isaaclab/test/sensors/test_first_frame_textured_rendering.py +++ b/source/isaaclab/test/sensors/test_first_frame_textured_rendering.py @@ -15,7 +15,6 @@ import pytest import torch from isaaclab_physx.physics import PhysxCfg -from isaaclab_physx.sim.schemas import PhysxCollisionPropertiesCfg, PhysxRigidBodyPropertiesCfg import omni.replicator.core as rep @@ -176,9 +175,9 @@ class CameraObservationsCfg(ObservationGroupCfg): prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( size=(0.3, 0.15, 0.2), - rigid_props=PhysxRigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=PhysxCollisionPropertiesCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.35, 0.0, 0.2)), ) diff --git a/source/isaaclab_newton/changelog.d/vidurv-migrate-legacy-source-remaining.skip b/source/isaaclab_newton/changelog.d/vidurv-migrate-legacy-source-remaining.skip new file mode 100644 index 000000000000..8e368b384d8f --- /dev/null +++ b/source/isaaclab_newton/changelog.d/vidurv-migrate-legacy-source-remaining.skip @@ -0,0 +1 @@ +Migrated test scene configurations off the deprecated legacy schema property configs; test-only change. diff --git a/source/isaaclab_newton/test/assets/test_cable_object.py b/source/isaaclab_newton/test/assets/test_cable_object.py index d4f167fb741e..c2801445702a 100644 --- a/source/isaaclab_newton/test/assets/test_cable_object.py +++ b/source/isaaclab_newton/test/assets/test_cable_object.py @@ -52,9 +52,9 @@ class _ProxyCableSceneCfg(_CableSceneCfg): prim_path="{ENV_REGEX_NS}/Rigid", spawn=sim_utils.CuboidCfg( size=(0.1, 0.1, 0.1), - rigid_props=sim_utils.RigidBodyBaseCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionBaseCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=1.0), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(2.0, 0.0, 1.0)), ) diff --git a/source/isaaclab_newton/test/assets/test_mpm_object.py b/source/isaaclab_newton/test/assets/test_mpm_object.py index 5b87d16728da..92332751b6d1 100644 --- a/source/isaaclab_newton/test/assets/test_mpm_object.py +++ b/source/isaaclab_newton/test/assets/test_mpm_object.py @@ -78,6 +78,8 @@ class MPMSceneCfg(InteractiveSceneCfg): def test_mpm_solver_refreshes_kinematic_rigid_body_transforms(): + from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg # noqa: PLC0415 + import isaaclab.sim as sim_utils # noqa: PLC0415 @configclass @@ -86,12 +88,11 @@ class MPMSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/KinematicBox", spawn=sim_utils.CuboidCfg( size=(0.1, 0.1, 0.1), - rigid_props=sim_utils.NewtonRigidBodyPropertiesCfg( - rigid_body_enabled=True, - kinematic_enabled=True, - disable_gravity=True, - ), - collision_props=sim_utils.NewtonCollisionPropertiesCfg(collision_enabled=True), + rigid_props=[ + sim_utils.UsdPhysicsRigidBodyCfg(rigid_body_enabled=True, kinematic_enabled=True), + PhysxRigidBodyCfg(disable_gravity=True), + ], + collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.2)), ) diff --git a/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py index a26b9201a579..79e056feef12 100644 --- a/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py +++ b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py @@ -60,9 +60,9 @@ class _DistortionSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Anchor", spawn=sim_utils.CuboidCfg( size=(0.01, 0.01, 0.01), - rigid_props=sim_utils.RigidBodyBaseCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=0.001), - collision_props=sim_utils.CollisionBaseCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + mass_props=sim_utils.MassCfg(mass=0.001), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), physics_material=RigidBodyMaterialBaseCfg(), ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, -100.0)), diff --git a/source/isaaclab_ov/changelog.d/vidurv-migrate-legacy-source-remaining.skip b/source/isaaclab_ov/changelog.d/vidurv-migrate-legacy-source-remaining.skip new file mode 100644 index 000000000000..8e368b384d8f --- /dev/null +++ b/source/isaaclab_ov/changelog.d/vidurv-migrate-legacy-source-remaining.skip @@ -0,0 +1 @@ +Migrated test scene configurations off the deprecated legacy schema property configs; test-only change. diff --git a/source/isaaclab_ov/test/physics/test_ovphysx_gravity.py b/source/isaaclab_ov/test/physics/test_ovphysx_gravity.py index 6ac0228d9a10..e9a525813a5e 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_gravity.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_gravity.py @@ -32,9 +32,9 @@ def test_gravity_event_changes_rigid_body_motion(): init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 10.0)), spawn=sim_utils.CuboidCfg( size=(0.5, 0.5, 0.5), - rigid_props=sim_utils.RigidBodyBaseCfg(), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), mass_props=sim_utils.MassCfg(mass=1.0), - collision_props=sim_utils.CollisionBaseCfg(), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), ) ) diff --git a/source/isaaclab_tasks/changelog.d/vidurv-migrate-legacy-source-remaining.rst b/source/isaaclab_tasks/changelog.d/vidurv-migrate-legacy-source-remaining.rst new file mode 100644 index 000000000000..d3813f345497 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/vidurv-migrate-legacy-source-remaining.rst @@ -0,0 +1,12 @@ +Changed +^^^^^^^ + +* Changed the reach table collider, the UR10 particle-push colliders, and the NIST factory Newton + Franka rigid-body properties to author their physics schemas with schema fragments + (:class:`~isaaclab.sim.schemas.UsdPhysicsCollisionCfg`, + :class:`~isaaclab_newton.sim.schemas.NewtonCollisionCfg`, and + :class:`~isaaclab_newton.sim.schemas.MujocoRigidBodyCfg`) instead of the deprecated legacy + property configs, so loading these tasks no longer emits their ``DeprecationWarning``. The + authored USD is unchanged. Configurations that tune these spawner slots in place should select + the fragment that owns the field (e.g. the :class:`~isaaclab_newton.sim.schemas.NewtonCollisionCfg` + entry of the UR10 particle-push ``collision_props`` list for ``contact_margin``). diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/nist/factory_assets_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/nist/factory_assets_cfg.py index a1896c0f615d..851751188ad8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/nist/factory_assets_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/nist/factory_assets_cfg.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab_newton.sim.schemas import NewtonArticulationCfg +from isaaclab_newton.sim.schemas import MujocoRigidBodyCfg, NewtonArticulationCfg from isaaclab_newton.sim.spawners.materials import NewtonMaterialCfg from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxCollisionCfg, PhysxRigidBodyCfg @@ -171,7 +171,7 @@ class _PlugCollisionPropsCfg(PresetCfg): spawn=sim_utils.UsdFileCfg( usd_path=f"{ASSET_DIR}/franka_mimic.usd", activate_contact_sensors=True, - rigid_props=sim_utils.MujocoRigidBodyPropertiesCfg(gravcomp=1.0), + rigid_props=MujocoRigidBodyCfg(gravcomp=1.0), articulation_props=[ PhysxArticulationCfg(enabled_self_collisions=False), NewtonArticulationCfg(self_collision_enabled=False), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/ur10_particle_push/ur10_particle_push_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/ur10_particle_push/ur10_particle_push_env_cfg.py index ebc48be8b13b..0ebf60617cbd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/ur10_particle_push/ur10_particle_push_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/ur10_particle_push/ur10_particle_push_env_cfg.py @@ -15,7 +15,7 @@ from isaaclab_newton.assets import MPMObjectCfg from isaaclab_newton.physics import MJWarpSolverCfg, MPMSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg -from isaaclab_newton.sim.schemas import MujocoJointCfg, NewtonCollisionPropertiesCfg +from isaaclab_newton.sim.schemas import MujocoJointCfg, NewtonCollisionCfg from isaaclab_newton.sim.spawners.mpm import MPMGridCfg, MPMParticleMaterialCfg from isaaclab_visualizers.newton import NewtonGLVisualizerCfg, NewtonRTXVisualizerCfg @@ -31,7 +31,7 @@ from isaaclab.markers import VisualizationMarkersCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationCfg -from isaaclab.sim.schemas import UsdPhysicsRigidBodyCfg +from isaaclab.sim.schemas import UsdPhysicsCollisionCfg, UsdPhysicsRigidBodyCfg from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -281,12 +281,14 @@ def _kinematic_box( rigid_body_enabled=True, kinematic_enabled=True, ), - collision_props=NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=MPM_COLLIDER_MARGIN, - # Implicit MPM consumes shape margin; gap is a rigid-contact parameter. - contact_gap=0.0, - ), + collision_props=[ + UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg( + contact_margin=MPM_COLLIDER_MARGIN, + # Implicit MPM consumes shape margin; gap is a rigid-contact parameter. + contact_gap=0.0, + ), + ], physics_material=RigidBodyMaterialBaseCfg( static_friction=0.8, dynamic_friction=0.7, @@ -312,11 +314,10 @@ def _static_collision_box( init_state=AssetBaseCfg.InitialStateCfg(pos=position), spawn=sim_utils.CuboidCfg( size=size, - collision_props=NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=0.004, - contact_gap=0.002, - ), + collision_props=[ + UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=0.004, contact_gap=0.002), + ], physics_material=RigidBodyMaterialBaseCfg( static_friction=0.8, dynamic_friction=0.7, @@ -370,11 +371,10 @@ class UR10ParticlePushSceneCfg(InteractiveSceneCfg): size=PADDLE_SIZE, rigid_props=UsdPhysicsRigidBodyCfg(rigid_body_enabled=True), mass_props=sim_utils.MassCfg(mass=PADDLE_MASS), - collision_props=NewtonCollisionPropertiesCfg( - collision_enabled=True, - contact_margin=PADDLE_CONTACT_MARGIN, - contact_gap=0.002, - ), + collision_props=[ + UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=PADDLE_CONTACT_MARGIN, contact_gap=0.002), + ], physics_material=RigidBodyMaterialBaseCfg( static_friction=0.8, dynamic_friction=0.7, diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/reach/reach_env_cfg.py index e7cd5d992526..8f0b99fcebe0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reach/reach_env_cfg.py @@ -81,7 +81,7 @@ class ReachSceneCfg(InteractiveSceneCfg): init_state=AssetBaseCfg.InitialStateCfg(pos=(0.5, 0.0, -0.5)), spawn=sim_utils.CuboidCfg( size=(0.9, 1.3, 1.0), - collision_props=sim_utils.CollisionBaseCfg(), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), ), ) From 22bfa0dba7fcd11431f44f1f736b7866fcbb8515 Mon Sep 17 00:00:00 2001 From: ooctipus Date: Wed, 23 Sep 2026 20:12:11 -0700 Subject: [PATCH 07/11] [4C/10] Route rigid rendering transforms through SDP (#7941) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description Route rigid-body transforms from physics to renderers through `SceneDataProvider` (SDP). Physics publishes its native buffer and increments its version after changes. SDP binds that buffer when format and ordering match, or converts it once per requested layout and reuses the result until the next change. Readers never reset the producer's version. ## What changes - **OVRTX:** converts physics poses directly into its matrix format, without first copying them into a Newton render state. - **Newton rendering:** borrows SDP's transform buffer instead of copying into a second buffer. Newton physics and rendering continue sharing their existing model/state. - **Isaac RTX:** shares one registry-owned Fabric binding with Kit, preserves authored scale, and updates the transform hierarchy on GPU. PhysX publishes its native Fabric matrices without fetching packed poses. - **Ownership:** physics refreshes native data; SDP borrows or converts arrays. One `FabricBackend` in `isaaclab_physx` owns the stage/hierarchy handles and shared transform bindings, identified by stage/device. Consumers pass SDP explicitly to updates; it is not part of backend identity. Core `RenderContext` has no Fabric methods or state. `FabricMatrix44` contains only matrix storage. - **Rendering updates:** removes repeated renderer-driven physics refreshes. Writes made between physics steps remain visible on the next render. For Newton → Isaac RTX, cached bindings and GPU hierarchy updates replace repeated binding setup and the CPU hierarchy fallback. This is the main runtime saving measured below. Particle/deformable transport is outside this change. OVRTX retains its existing Newton geometry bridge. ## Transform movement Counts are SDP output-writing passes, not total SDK-internal copies. Conversion, reordering, and scale are combined in one pass; unchanged data reuses the result. | Physics | Renderer | Published → requested format | SDP passes | | --- | --- | --- | ---: | | Newton | Newton Warp | `Transform` → `Transform` | 0 | | Newton | OVRTX | `Transform` → `TransposedMatrix44d` | 1 | | Newton | Isaac RTX | `Transform` → `FabricMatrix44` | 1 | | OVPhysX | Newton Warp | `Transform` → `Transform` | 0 if ordering matches; otherwise 1 | | OVPhysX | OVRTX | `Transform` → `TransposedMatrix44d` | 1 | | Isaac PhysX | Newton Warp | `Transform` → `Transform` | 0 if ordering matches; otherwise 1 | | Isaac PhysX | Isaac RTX | Native Fabric → borrowed Fabric | 0 with `use_fabric=True` | OVPhysX and OVRTX cannot run with Kit. Fabric hierarchy propagation and OVRTX's native attribute write happen after the SDP pass and are not included in these counts. ## Performance Kuka Allegro Camera, 4096 environments, 64×64 RGB, Newton MJWarp → Isaac RTX, RTX 5090, no interactive visualizer. Two warm-cache runs per revision; runtime excludes 25 warmup steps and measures 200 synchronized full environment steps, including rendering and observations. | Metric | PR | Develop | | --- | ---: | ---: | | Warm startup | 135.47 s | 133.19 s | | Runtime step | 172.32 ms | 554.49 ms | | Environment frames/s | 23,770 | 7,387 | The measured step time was **68.9% lower (3.22× throughput)**. No startup improvement was measured. All 4096 camera images were finite and nonconstant. Measured revisions: PR `257e54d5f` and develop `53a7f1c0a`, with identical dependencies. Subsequent cleanup has not been rebenchmarked. ## Migration Custom scene-data backends must initialize `transforms_version=0` and increment it after native pose writes or buffer swaps. The consumer-facing `get_transforms(output)` API binds shared, read-only arrays, including converted outputs. Pass `allow_passthrough=False` for caller-owned writable or preallocated arrays; conversion writes directly into them. Single-format backends keep their existing `transforms` property; the base `get_transforms(output_format)` delegates to it. Multi-format backends may override that method and `native_transform_formats`. ## Validation - Focused CPU tests and GPU 0 Newton/PhysX Fabric tests cover pointer sharing, publication versions, transform formats, ordering, authored scale, buffer reallocation, same-step writes, and resets. The existing cache regression now also checks that independent SDP readers cannot hide producer changes from one another. - All 11 native Newton/Fabric tests and the native PhysX Fabric test passed after moving bindings out of core. They also check one shared RTX/Kit resource and explicit Newton-to-Fabric updates without an RTX camera or Kit viewer. - Removed obsolete synchronization tests and duplicate mock-only checks; native and numerical regressions remain. - Native OVPhysX → OVRTX rendered scale, pose changes, and camera calibration passed for both legacy and ovstage APIs. Matched OVRTX timings have not been collected. ## Type of change - Refactor and bug fixes - Breaking custom scene-data backend interface change, with migration above ## Release backport - [x] Backport to the active release branch ## Checklist - [x] Contribution guidelines reviewed - [x] Changelog fragments and migration documentation updated - [x] Retained focused tests and formatting checks passed after test cleanup - [ ] Full GPU CI passed for the latest revision - [x] Native OVPhysX → OVRTX rendering validated --- .../developer-tools/scene_data_providers.rst | 63 ++- .../sdp-transform-publication.major.rst | 19 + .../isaaclab/renderers/render_context.py | 30 +- .../isaaclab/scene/interactive_scene.py | 3 +- .../isaaclab/isaaclab/scene_data/__init__.pyi | 7 +- .../isaaclab/scene_data/scene_data_backend.py | 30 +- .../scene_data/scene_data_provider.py | 292 ++++++----- .../isaaclab/sim/simulation_context.py | 12 +- .../assets/_articulation_iface_test_utils.py | 2 + ...igid_object_collection_iface_test_utils.py | 2 + .../assets/_rigid_object_iface_test_utils.py | 2 + .../test/envs/test_direct_marl_env.py | 17 - .../test/envs/test_env_rendering_logic.py | 24 - .../test_simulation_render_context.py | 10 +- .../scene_data/test_scene_data_transforms.py | 203 +++++++- ...test_newton_manager_visualization_state.py | 197 +++++--- .../changelog.d/sdp-transform-transport.rst | 11 + .../isaaclab_newton/physics/newton_manager.py | 353 ++++--------- .../renderers/newton_warp_renderer.py | 19 +- .../physics/test_newton_fabric_body_sync.py | 255 ++++------ .../test_newton_manager_abstraction.py | 98 ++-- .../changelog.d/sdp-transform-transport.rst | 10 + .../assets/articulation/articulation.py | 16 + .../assets/articulation/articulation_data.py | 1 + .../assets/rigid_object/rigid_object.py | 4 + .../rigid_object_collection.py | 4 + .../isaaclab_ov/physics/ovphysx_manager.py | 175 +++---- .../isaaclab_ov/renderers/ovrtx_renderer.py | 185 ++----- .../renderers/ovrtx_renderer_kernels.py | 27 - .../test_ovphysx_scene_data_backend.py | 464 +++++------------- .../isaaclab_ov/test/test_ovrtx_clone_plan.py | 59 +-- .../test/test_ovrtx_deformable_bindings.py | 321 ++++-------- .../test/test_ovrtx_renderer_contract.py | 106 +--- .../changelog.d/sdp-transform-publication.rst | 8 + .../assets/articulation/articulation.py | 4 + .../assets/articulation/articulation_data.py | 1 + .../assets/rigid_object/rigid_object.py | 2 + .../rigid_object_collection.py | 2 + .../isaaclab_physx/physics/physx_manager.py | 85 +++- .../isaaclab_physx/renderers/fabric.py | 126 +++++ .../renderers/isaac_rtx_renderer.py | 12 +- .../renderers/isaac_rtx_renderer_utils.py | 19 +- .../test_isaac_rtx_renderer_contract.py | 21 +- .../test_isaac_rtx_renderer_utils.py | 151 ++---- .../test/sim/test_physx_scene_data_backend.py | 94 ++++ .../test/sim/test_views_xform_prim_fabric.py | 65 ++- .../changelog.d/sdp-transform-publication.rst | 6 + .../kit/kit_visualizer.py | 18 +- .../test_kit_visualizer_scene_partitioning.py | 22 + .../test/visualizer_golden_utils.py | 12 +- .../test/visualizer_integration_utils.py | 98 +--- 51 files changed, 1665 insertions(+), 2102 deletions(-) create mode 100644 source/isaaclab/changelog.d/sdp-transform-publication.major.rst create mode 100644 source/isaaclab_newton/changelog.d/sdp-transform-transport.rst create mode 100644 source/isaaclab_ov/changelog.d/sdp-transform-transport.rst create mode 100644 source/isaaclab_physx/changelog.d/sdp-transform-publication.rst create mode 100644 source/isaaclab_physx/isaaclab_physx/renderers/fabric.py create mode 100644 source/isaaclab_visualizers/changelog.d/sdp-transform-publication.rst diff --git a/docs/source/developer-tools/scene_data_providers.rst b/docs/source/developer-tools/scene_data_providers.rst index 0b4a5eeb5407..ae20df95dc06 100644 --- a/docs/source/developer-tools/scene_data_providers.rst +++ b/docs/source/developer-tools/scene_data_providers.rst @@ -31,14 +31,19 @@ The system has three layers: 1. :class:`~isaaclab.scene_data.SceneDataBackend`: a small interface implemented by each physics manager. It exposes the backend's transform array directly as one of the :class:`~isaaclab.scene_data.SceneDataFormat` Warp structs, plus the per-transform prim paths - and total count. There is no per-frame "update" call; the property accessors return live - views into the underlying tensor each time they're read. + and total count. Producers increment ``transforms_version`` after native state writes or buffer swaps; + SDP calls ``get_transforms(output_format)`` before reading the version, since resolving the pointer + can itself detect a swap. The default implementation returns the existing ``transforms`` property. + The version never resets, so independent readers cannot hide changes from one another. - - :attr:`SceneDataBackend.transforms`: current transforms as a Warp struct (one of + - :attr:`SceneDataBackend.transforms`: the native data as a Warp struct (one of :class:`SceneDataFormat.Vec3_Quat`, :class:`SceneDataFormat.Transform`, :class:`SceneDataFormat.Matrix44`, :class:`SceneDataFormat.Vec3_Matrix33`). + - :attr:`SceneDataBackend.transforms_version`: monotonic version of the native transforms. - :attr:`SceneDataBackend.transform_count`: number of transforms. - :attr:`SceneDataBackend.transform_paths`: list of USD prim paths, one per transform. + - :attr:`SceneDataBackend.native_transform_formats`: formats published without conversion. + PhysX publishes either packed poses or Fabric matrices and refreshes only the requested representation. - :attr:`SceneDataBackend.points`: flattened deformable nodal positions as :class:`SceneDataFormat.Points` (optional; rigid-only backends return an empty buffer). - :attr:`SceneDataBackend.point_count`: total number of geometry points. @@ -48,11 +53,10 @@ The system has three layers: 2. :class:`~isaaclab.scene_data.SceneDataProvider`: wraps a backend and offers format conversion plus index re-mapping. - - :meth:`SceneDataProvider.get_transforms`: writes the backend's transforms into a - consumer-provided :class:`SceneDataFormat` struct, optionally converting format - (e.g. ``Vec3_Quat`` to ``Transform``) and applying an index mapping. When the backend - format matches the output format and no mapping is provided, the result is a zero-copy - passthrough. + - :meth:`SceneDataProvider.get_transforms`: binds native arrays when format and ordering match, + or SDP-owned buffers converted once per producer version and destination layout. These shared + arrays are read-only, including when they replace preallocated output fields. Pass + ``allow_passthrough=False`` to write directly into caller-owned arrays instead. - :meth:`SceneDataProvider.create_mapping`: builds a remap array from the backend's prim paths to a consumer's desired ordering. Used when a renderer or visualizer wants transforms indexed by its own body list rather than by the physics view order. @@ -86,10 +90,12 @@ When PhysX is the active physics backend, the provider reads transforms directly The transforms are returned as :class:`SceneDataFormat.Transform` (Warp ``transformf`` array), so consumers that want this format get them zero-copy. -Newton-native consumers (Newton visualizer, Rerun, Viser, Newton Warp renderer, OVRTX renderer) -also need a Newton ``Model``/``State`` to render against. To provide that, -:class:`~isaaclab_newton.physics.NewtonManager` builds a **shadow Newton model** from the USD -stage on first access and updates its ``body_q`` from the PhysX backend each render frame. +Newton-native consumers (Newton visualizer, Rerun, Viser, Newton Warp renderer) also need a +Newton ``Model``/``State``. Their declared cloning contexts construct that representation from +the shared clone plan before initialization. Its rigid ``body_q`` binds to SDP's requested +``Transform`` array; no intermediate per-frame copy into a second state buffer is required. +OVRTX requests ``TransposedMatrix44d`` directly from SDP, including destination ordering and +static scale in the same conversion. It no longer reads Newton state for rigid transforms. When the scene has PhysX or OVPhysX deformables, the shadow model also allocates ``particle_q`` render slots for soft/cloth meshes, syncs simulation nodal positions through :meth:`SceneDataProvider.get_points` with ``allow_passthrough=False`` into a separate @@ -99,8 +105,26 @@ barycentric sim-to-visual remap so Newton Warp and OVRTX render the paired visua than tet simulation topology. The shadow deformable registry exposes render-slot offsets and ``particles_per_body`` counts for OVRTX point bindings. -This is hidden behind :meth:`NewtonManager.get_model` / :meth:`NewtonManager.get_state`, so -renderers don't need to know which physics backend is active. +The deformable and cable geometry bridge remains separate from this rigid-transform path. +OVRTX still uses Newton geometry metadata for those features. + +PhysX owns its native Fabric refresh and publishes the resulting matrices through SDP without +fetching packed poses. ``isaaclab_physx.renderers.fabric.FabricBackend`` owns the shared native stage +and hierarchy handles. Its identity is the stage and device, not the SDP source or attribute type. +``SimulationContext`` declares ``fabric_cfg`` when Kit is available. After physics initializes, Kit, +Isaac RTX, and explicit Fabric synchronization obtain the same resource through +``get_or_create_backend(sim.fabric_cfg)``. Transform bindings are state on that resource, not a +separate backend. Consumers pass the simulation's SDP to ``update_transforms(provider)``; for foreign +physics it converts directly into Fabric local matrices, then propagates the GPU hierarchy. +Core ``RenderContext`` owns no Fabric bindings. +It binds rigid destinations as Fabric-only reset-stack roots because +physics publishes absolute poses, including for nested bodies. Visual descendants still inherit +their body's transform; authored USD is unchanged. Native source indices and world scales are +bound once. Fabric's selection reuse API reports scene-wide structural changes; the resource refreshes +array views without repeating path matching or scale capture. Otherwise GPU propagation +reuses the hierarchy topology. Clean requests never acquire writable Fabric arrays. +Renderers do not select a physics-specific synchronization path. +``FabricMatrix44`` contains only matrix storage, not bindings or native engine handles. Newton backend -------------- @@ -109,11 +133,20 @@ When Newton is the active physics backend, the backend wraps the Newton model's directly. No shadow model or per-frame sync is needed: Newton already owns the authoritative model and state, and the provider exposes that state as :class:`SceneDataFormat.Transform`. +Native reads reconcile pending authored state writes once. A new physics publication does not +itself request forward kinematics. Kit/RTX requests current Fabric transforms through SDP +before rendering, without issuing an additional physics ``forward()``. Headless viewport +capture requests these transforms on demand rather than on every visualizer step. + +Externally replayed CUDA graphs do not call Python write hooks. After writes have been captured, +Newton conservatively republishes transforms when read so an unannounced replay cannot leave +rendering stale. Those reads do not benefit from clean-publication caching. + Data requirements ------------------ Visualizers and renderers declare what they need from the scene data path. This is resolved at -simulation-context construction time and is what triggers the shadow-model build for PhysX: +consumer construction time, before the shared clone plan is built: .. list-table:: :header-rows: 1 diff --git a/source/isaaclab/changelog.d/sdp-transform-publication.major.rst b/source/isaaclab/changelog.d/sdp-transform-publication.major.rst new file mode 100644 index 000000000000..36c90c2c759b --- /dev/null +++ b/source/isaaclab/changelog.d/sdp-transform-publication.major.rst @@ -0,0 +1,19 @@ +Changed +^^^^^^^ + +* **Breaking:** Added ``transforms_version`` to scene-data backends. Custom backends must initialize + it to zero and increment it after native pose writes or buffer swaps. SDP reads the existing + ``transforms`` property through ``get_transforms(output_format)`` without resetting the version. Backends + publishing multiple native formats may override that method and ``native_transform_formats``. + ``SceneDataProvider.get_transforms`` bound shared, + read-only arrays by default: matching layouts aliased native data and other layouts converted + once per publication. Callers requiring their own writable or preallocated arrays must pass + ``allow_passthrough=False``; this wrote directly into the supplied arrays without a staging copy. +* Routed rigid Fabric conversion through SDP while the Kit rendering integration owned destination binding and + GPU hierarchy propagation, preserving native PhysX publication and authored scale. Converted rigid destinations + became Fabric-only reset-stack roots so nested bodies retained their absolute physics poses. + Transform freshness no longer depended on the physics-step counter; + ``RenderContext.reset_scene_state_cadence`` remained available for geometry updates. + ``SimulationContext.fabric_cfg`` declared the shared native Fabric stage/device without allocating bindings. +* Used native Warp structs for Fabric transform bindings, relying on the project-managed Warp + dependency selected by Isaac Lab's Kit launch configuration. diff --git a/source/isaaclab/isaaclab/renderers/render_context.py b/source/isaaclab/isaaclab/renderers/render_context.py index 9ca243c1a671..bba6f7a76bf7 100644 --- a/source/isaaclab/isaaclab/renderers/render_context.py +++ b/source/isaaclab/isaaclab/renderers/render_context.py @@ -60,8 +60,8 @@ def _write_material( class RenderContext: """Orchestrate simulation-owned renderers and own flat runtime material buffers. - Renderer instances are borrowed from the simulation's backend registry. Scene state updates - run at most once per physics step, regardless of how many cameras share a renderer. + Renderer instances are borrowed from the simulation's backend registry. SDP owns transform + freshness, including pose writes that do not advance the physics-step counter. """ __slots__ = ( @@ -70,7 +70,7 @@ class RenderContext: "_physics_initialized", "_prepared_renderer_ids", "_prepared_num_envs", - "_last_scene_state_step", + "_last_geometry_update_step", "_visual_materials", "_visual_material_batches", "_visual_material_batches_by_channel", @@ -88,7 +88,7 @@ def __init__(self, backend_registry: list[tuple[BackendCfg, Any]]) -> None: self._physics_initialized: bool = False # Set to True after the first PHYSICS_READY callback fires. self._prepared_renderer_ids: set[int] = set() self._prepared_num_envs: int | None = None - self._last_scene_state_step: int | None = None + self._last_geometry_update_step: int | None = None # Physics step of the last renderer geometry update. self._visual_materials: list[Any] = [] self._visual_material_batches: tuple[VisualMaterialBatch, ...] = () self._visual_material_batches_by_channel: dict[str, VisualMaterialBatch] = {} @@ -126,7 +126,7 @@ def validate_renderer_cfg(self, cfg: RendererCfg) -> None: def register_renderer(self, cfg: RendererCfg, renderer: BaseRenderer) -> None: """Include a newly registry-owned renderer in cloning and post-physics initialization.""" self.clone_contexts.update(cfg.cloning_contexts) - self._last_scene_state_step = None + self._last_geometry_update_step = None if self._physics_initialized: renderer.initialize() @@ -317,19 +317,15 @@ def ensure_prepare_stage(self, stage: Any, num_envs: int) -> None: self._prepared_num_envs = num_envs def update_scene_state(self, physics_step_count: int) -> None: - """Update scene state on all backends (at most once per step). + """Publish physics state and refresh renderers through SDP's producer versions. - Invokes :meth:`BaseRenderer.update_transforms` and then - :meth:`BaseRenderer.update_geometries` on each registered renderer. + Transforms follow SDP freshness; geometry updates retain their once-per-step cadence. """ - if self._last_scene_state_step == physics_step_count: - return - for _cfg, renderer in self._renderer_entries: renderer.update_transforms() - renderer.update_geometries() - - self._last_scene_state_step = physics_step_count + if self._last_geometry_update_step != physics_step_count: + renderer.update_geometries() + self._last_geometry_update_step = physics_step_count def render_into_camera( self, @@ -349,8 +345,8 @@ def reset_stage_prepare_flag(self) -> None: self._prepared_num_envs = None def reset_scene_state_cadence(self) -> None: - """Clear per-step scene state update dedupe (e.g. a long pause with no physics).""" - self._last_scene_state_step = None + """Invalidate geometry updates after resets that do not advance the physics step.""" + self._last_geometry_update_step = None def close(self) -> None: """Release material writers and lifecycle bookkeeping, not registry-owned renderers. @@ -368,7 +364,7 @@ def close(self) -> None: self.clone_contexts.clear() self._prepared_renderer_ids.clear() self._prepared_num_envs = None - self._last_scene_state_step = None + self._last_geometry_update_step = None self._physics_initialized = False self._visual_materials.clear() self._visual_material_batches = () diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 40099db6b875..ca5d61ccd5ae 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -498,8 +498,7 @@ def update(self, dt: float) -> None: Args: dt: The amount of time passed from last :meth:`update` call. """ - # Scene-wide renderer scene-state sync once per step when all sensors update, - # so per-camera fetches do not own this concern (deduped inside RenderContext). + # Publish transforms before eager sensors read their Fabric-backed poses. if not self.cfg.lazy_sensor_update: self.sim.render_context.update_scene_state(self.sim.get_physics_step_count()) diff --git a/source/isaaclab/isaaclab/scene_data/__init__.pyi b/source/isaaclab/isaaclab/scene_data/__init__.pyi index d4e47a65c698..d43d7e98fd78 100644 --- a/source/isaaclab/isaaclab/scene_data/__init__.pyi +++ b/source/isaaclab/isaaclab/scene_data/__init__.pyi @@ -3,12 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -__all__ = [ - "REQUIRES_STAGE_AND_MODEL", - "SceneDataBackend", - "SceneDataFormat", - "SceneDataProvider", -] +__all__ = ["REQUIRES_STAGE_AND_MODEL", "SceneDataBackend", "SceneDataFormat", "SceneDataProvider"] from .scene_data_backend import SceneDataBackend, SceneDataFormat from .scene_data_provider import REQUIRES_STAGE_AND_MODEL, SceneDataProvider diff --git a/source/isaaclab/isaaclab/scene_data/scene_data_backend.py b/source/isaaclab/isaaclab/scene_data/scene_data_backend.py index af38842da67f..6bbef2b4ccc8 100644 --- a/source/isaaclab/isaaclab/scene_data/scene_data_backend.py +++ b/source/isaaclab/isaaclab/scene_data/scene_data_backend.py @@ -16,6 +16,8 @@ from __future__ import annotations +from typing import Any + import warp as wp # Under Sphinx ``autodoc_mock_imports``, ``wp.struct`` is a ``_MockObject`` @@ -69,6 +71,20 @@ class Matrix44: matrices: wp.array(dtype=wp.mat44f) = None """Per-transform 4x4 homogeneous transform matrices [m].""" + @wp_struct + class TransposedMatrix44d: + """Double-precision row-vector transforms, as consumed by USD renderers.""" + + matrices: wp.array(dtype=wp.mat44d) = None + """World transforms [m], shape [transform_count].""" + + @wp_struct + class FabricMatrix44: + """Double-precision row-vector matrices in native Fabric storage.""" + + matrices: wp.fabricarray(dtype=wp.mat44d) = None + """Transforms [m], shape [transform_count].""" + @wp_struct class Points: """Flat world-space nodal or particle positions.""" @@ -78,13 +94,25 @@ class Points: class SceneDataBackend: + transforms_version: int + """Monotonic producer version, incremented after native writes or buffer swaps; never reset by readers.""" + + @property + def native_transform_formats(self) -> tuple[Any, ...]: + """Formats available without conversion, used when binding consumer destinations.""" + return (self.transforms._cls,) + + def get_transforms(self, output_format: Any) -> Any: + """Publish the requested native format when available, otherwise the primary format.""" + return self.transforms + @property def transforms( self, ) -> ( SceneDataFormat.Vec3_Quat | SceneDataFormat.Transform | SceneDataFormat.Matrix44 | SceneDataFormat.Vec3_Matrix33 ): - """Return the sim backends transforms as one of the SceneDataFormat structs.""" + """Return native transforms without copying; pointer changes must increment ``transforms_version``.""" raise NotImplementedError @property diff --git a/source/isaaclab/isaaclab/scene_data/scene_data_provider.py b/source/isaaclab/isaaclab/scene_data/scene_data_provider.py index 9ebfc0d27031..8ceb1cf95c7c 100644 --- a/source/isaaclab/isaaclab/scene_data/scene_data_provider.py +++ b/source/isaaclab/isaaclab/scene_data/scene_data_provider.py @@ -13,8 +13,7 @@ import numpy as np import warp as wp -import isaaclab.sim as sim_utils - +from .. import sim as sim_utils from .scene_data_backend import SceneDataBackend, SceneDataFormat logger = logging.getLogger(__name__) @@ -37,12 +36,13 @@ def _publication_device(data: Any) -> wp.Device: """Return the common device of a populated scene-data publication.""" - arrays = tuple(array for name in data._cls.vars if (array := getattr(data, name)) is not None) + data_format = data._cls + arrays = tuple(array for name in data_format.vars if (array := getattr(data, name)) is not None) if not arrays: - raise ValueError(f"{data._cls.__name__} contains no published arrays.") + raise ValueError(f"{data_format.__name__} contains no published arrays.") device = arrays[0].device if any(array.device != device for array in arrays[1:]): - raise ValueError(f"{data._cls.__name__} arrays must share one device.") + raise ValueError(f"{data_format.__name__} arrays must share one device.") return device @@ -54,6 +54,8 @@ def _init_output(output: Any, count: int, device: wp.Device) -> None: class SceneDataProvider: + """Borrow or convert published arrays; producers own native refresh, renderers own destination lifecycle.""" + def __init__(self, backend: SceneDataBackend): """Initialize the scene data provider. @@ -63,6 +65,101 @@ def __init__(self, backend: SceneDataBackend): self.backend = backend self._num_envs_cache: int | None = None self._interactive_scene: Any | None = None + self._transform_cache: dict[tuple, tuple[int, Any]] = {} + + def get_transforms( + self, + output: SceneDataFormat.Vec3_Quat + | SceneDataFormat.Transform + | SceneDataFormat.Matrix44 + | SceneDataFormat.Vec3_Matrix33 + | SceneDataFormat.TransposedMatrix44d + | SceneDataFormat.FabricMatrix44, + mapping: wp.array | wp.fabricarray | None = None, + allow_passthrough: bool = True, + *, + count: int | None = None, + scales: wp.array | None = None, + ) -> bool: + """Bind shared transforms or write them directly into caller-owned output arrays. + + With passthrough enabled, matching native arrays are borrowed without a copy; other + layouts share SDP-owned buffers converted once per producer version. Treat these arrays + as read-only. With passthrough disabled, conversion writes directly into ``output``. + Fabric destinations must already be bound by their rendering owner. + + Args: + output: A :class:`SceneDataFormat` struct instance specifying the requested format. + Missing non-Fabric arrays are allocated when passthrough is disabled. + mapping: Native-to-output indices from :meth:`create_mapping`, or identity ordering. + Fabric destinations use their native output-to-source index attribute. + allow_passthrough: Whether to bind shared arrays instead of writing caller-owned arrays. + count: Destination count when remapping, or the native transform count. + scales: Static output scales for ``TransposedMatrix44d``, shape [count], or + source-indexed authored scales for Fabric. Mapping and scales are immutable + for a binding's lifetime; replace their arrays when the layout changes. + + Returns: + True if transforms are available in ``output``, False if no transforms are published + or the format conversion is unsupported. + """ + # Warp exposes the struct's field/type descriptor as _cls, not its Python type. + output_format = output._cls + fabric = output_format is SceneDataFormat.FabricMatrix44 + source = self.backend.get_transforms(output_format) + source_format = source._cls + version = self.backend.transforms_version + native_count = next( + (len(array) for name in source_format.vars if (array := getattr(source, name)) is not None), 0 + ) + if native_count == 0: + return False + count = native_count if count is None else count + if mapping is None and count != native_count: + raise ValueError("A different destination count requires an explicit transform mapping.") + if scales is not None and output_format not in ( + SceneDataFormat.TransposedMatrix44d, + SceneDataFormat.FabricMatrix44, + ): + raise ValueError("Static scales require double-precision row-vector matrix destinations.") + if source_format is output_format and mapping is None and scales is None: + result = source + if not allow_passthrough: + _init_output(output, count, _publication_device(source)) + for name in output_format.vars: + wp.copy(getattr(output, name), getattr(source, name)) + return True + else: + # A Fabric binding keeps its authored scales across selection reallocations. + key = (output_format, scales) if fabric else (output_format, mapping, count, scales) + cached = self._transform_cache.get(key) if allow_passthrough else None + if not allow_passthrough or fabric: + result = output + else: + result = cached[1] if cached is not None else output_format() + if cached is None or cached[0] != version or cached[1] is not result: + # Fabric changes storage and indexing, not the matrix conversion. + format_name = "TransposedMatrix44d" if fabric else output_format.__name__ + kernel = getattr(ConversionKernels, f"convert_{source_format.__name__}_to_{format_name}", None) + if kernel is None: + return False + device = _publication_device(source) + _init_output(result, count, device) + inputs = [source, mapping if mapping is not None else wp.array(dtype=wp.int32)] + if output_format is SceneDataFormat.TransposedMatrix44d or fabric: + inputs.append(scales) + wp.launch( + kernel, + dim=len(result.matrices) if fabric else native_count, + inputs=inputs, + outputs=[result], + device=device, + ) + if allow_passthrough: + self._transform_cache[key] = (version, result) + for name in output_format.vars: + setattr(output, name, getattr(result, name)) + return True def set_interactive_scene(self, scene: Any) -> None: """Attach the active interactive scene for scene-owned sensor discovery.""" @@ -152,65 +249,6 @@ def get_camera_transforms(self) -> dict[str, Any] | None: """ return _walk_camera_prims(self.usd_stage) - def get_transforms( - self, - output: SceneDataFormat.Vec3_Quat - | SceneDataFormat.Transform - | SceneDataFormat.Matrix44 - | SceneDataFormat.Vec3_Matrix33, - mapping: wp.array(dtype=wp.int32) | None = None, - allow_passthrough: bool = True, - ) -> bool: - """Convert sim backend transforms into the requested output format. - - When the backend's native format matches ``output``, data is either passed - through by reference (``allow_passthrough=True``) or deep-copied. Otherwise a - Warp conversion kernel is launched to transform the data, applying ``mapping`` - to reorder the output if provided. - - Args: - output: A pre-allocated :class:`SceneDataFormat` struct that determines the - target format. Uninitialized (``None``) fields are allocated automatically - when a conversion kernel is needed. - mapping: Optional index remapping array produced by - :meth:`create_mapping`. When ``None``, input and output indices are - identical. - allow_passthrough: If ``True`` and the formats already match, the output - struct's fields are set to reference the input arrays directly - (zero-copy). If ``False``, the data is always copied. - - Returns: - ``True`` if the conversion succeeded, ``False`` if no suitable conversion - kernel exists for the input/output format pair. - """ - input = self.backend.transforms - - if mapping is None and type(input) is type(output): - if allow_passthrough: - for field_name in input._cls.vars: - setattr(output, field_name, getattr(input, field_name)) - else: - _init_output(output, self.transform_count, _publication_device(input)) - for field_name in input._cls.vars: - wp.copy(getattr(output, field_name), getattr(input, field_name)) - return True - - conversion_kernel_name = f"convert_{input._cls.__name__}_to_{output._cls.__name__}" - - if conversion_kernel := getattr(ConversionKernels, conversion_kernel_name, None): - device = _publication_device(input) - _init_output(output, self.transform_count, device) - wp.launch( - kernel=conversion_kernel, - dim=self.transform_count, - inputs=[input, mapping], - outputs=[output], - device=device, - ) - return True - - return False - def init_output( self, output: SceneDataFormat.Vec3_Quat @@ -255,7 +293,7 @@ def create_mapping(self, paths: list[str | None]) -> wp.array(dtype=wp.int32) | if out_path not in path_to_out: path_to_out[out_path] = out_idx mapping = [path_to_out.get(path, -1) for path in input_paths] - if not np.array_equal(mapping, np.arange(len(input_paths))): + if len(paths) != len(input_paths) or not np.array_equal(mapping, np.arange(len(input_paths))): input = self.backend.transforms return wp.array(mapping, dtype=wp.int32, device=_publication_device(input)) return None @@ -368,6 +406,66 @@ def get_output_index(tid: wp.int32, mapping: wp.array(dtype=wp.int32)) -> wp.int return mapping[tid] return wp.int32(-1) + @wp.func + def matrix_indices(tid: int, mapping: wp.array(dtype=wp.int32)): + """Return source, destination, and authored-scale indices.""" + index = ConversionKernels.get_output_index(tid, mapping) + return tid, index, index + + @wp.func + def matrix_indices(tid: int, mapping: wp.fabricarray(dtype=wp.int32)): # noqa: F811 - Warp overload + return mapping[tid], tid, mapping[tid] + + @wp.func + def transposed_matrix(matrix: wp.mat44f, scales: wp.array(dtype=wp.vec3f), index: int) -> wp.mat44d: + result = wp.mat44d(wp.transpose(matrix)) + if scales.shape[0]: + scale = scales[index] + for row in range(3): + for column in range(3): + result[row, column] = result[row, column] * wp.float64(scale[row]) + return result + + @wp.kernel(enable_backward=False) + def convert_Transform_to_TransposedMatrix44d( + input: SceneDataFormat.Transform, mapping: Any, scales: wp.array(dtype=wp.vec3f), output: Any + ): + source, index, scale_index = ConversionKernels.matrix_indices(wp.tid(), mapping) + if index > -1: + output.matrices[index] = ConversionKernels.transposed_matrix( + wp.transform_to_matrix(input.transforms[source]), scales, scale_index + ) + + @wp.kernel(enable_backward=False) + def convert_Vec3_Quat_to_TransposedMatrix44d( + input: SceneDataFormat.Vec3_Quat, mapping: Any, scales: wp.array(dtype=wp.vec3f), output: Any + ): + source, index, scale_index = ConversionKernels.matrix_indices(wp.tid(), mapping) + if index > -1: + pose = wp.transformf(input.positions[source], input.orientations[source]) + output.matrices[index] = ConversionKernels.transposed_matrix( + wp.transform_to_matrix(pose), scales, scale_index + ) + + @wp.kernel(enable_backward=False) + def convert_Vec3_Matrix33_to_TransposedMatrix44d( + input: SceneDataFormat.Vec3_Matrix33, mapping: Any, scales: wp.array(dtype=wp.vec3f), output: Any + ): + source, index, scale_index = ConversionKernels.matrix_indices(wp.tid(), mapping) + if index > -1: + pose = wp.transformf(input.positions[source], wp.quat_from_matrix(input.orientations[source])) + output.matrices[index] = ConversionKernels.transposed_matrix( + wp.transform_to_matrix(pose), scales, scale_index + ) + + @wp.kernel(enable_backward=False) + def convert_Matrix44_to_TransposedMatrix44d( + input: SceneDataFormat.Matrix44, mapping: Any, scales: wp.array(dtype=wp.vec3f), output: Any + ): + source, index, scale_index = ConversionKernels.matrix_indices(wp.tid(), mapping) + if index > -1: + output.matrices[index] = ConversionKernels.transposed_matrix(input.matrices[source], scales, scale_index) + @wp.kernel def convert_Vec3_Quat_to_Vec3_Quat( input: SceneDataFormat.Vec3_Quat, mapping: wp.array(dtype=wp.int32), output: SceneDataFormat.Vec3_Quat @@ -634,67 +732,3 @@ def _walk_camera_prims(stage: Usd.Stage | None) -> dict[str, Any] | None: orientations.append(per_world_ori) return {"order": shared_paths, "positions": positions, "orientations": orientations, "num_envs": num_envs} - - -############################ -## Example - -if __name__ == "__main__": - - class ExampleSceneDataBackend(SceneDataBackend): - def __init__(self): - self.__transforms = SceneDataFormat.Transform() - self.__transforms.transforms = wp.array(np.hstack([np.arange(10).reshape(10, 1)] * 7), dtype=wp.transformf) - - @property - def transforms(self) -> SceneDataFormat.Transform: - return self.__transforms - - @property - def transform_count(self) -> int: - return self.__transforms.transforms.shape[0] - - @property - def transform_paths(self): - return [ - "/world/shape_01", - "/world/shape_02", - "/world/shape_03", - "/world/shape_04", - "/world/shape_05", - "/world/shape_06", - "/world/shape_07", - "/world/shape_08", - "/world/shape_09", - "/world/shape_10", - ] - - sim = ExampleSceneDataBackend() - sdp = SceneDataProvider(sim) - - output_data = SceneDataFormat.Vec3_Matrix33() - output_data.positions = wp.empty(sdp.transform_count, dtype=wp.vec3f) - output_data.orientations = wp.empty(sdp.transform_count, dtype=wp.mat33f) - - print(sim.transforms.transforms) - mapping = sdp.create_mapping( - [ - "/world/shape_02", - "/world/shape_01", - "/world/shape_03", - "/world/shape_04", - "/world/shape_05", - None, - None, - "/world/shape_10", - None, - None, - ] - ) - print(mapping) - if sdp.get_transforms(output_data, mapping): - print(output_data.positions) - else: - print("Failed to get transforms!") - - wp.synchronize() diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 1f294f61b65a..a9bb7d566833 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -195,6 +195,12 @@ def __init__(self, cfg: SimulationCfg | None = None): # Construct visualizers before cloning; initialize their runtime bindings after physics is ready. self._scene_data_provider = SceneDataProvider(self.physics_manager.get_scene_data_backend()) + self.fabric_cfg: BackendCfg | None = None + """Native Fabric stage/device configuration, or None without Kit.""" + if use_isaac_sim: + from isaaclab_physx.renderers.fabric import FabricBackendCfg # noqa: PLC0415 + + self.fabric_cfg = FabricBackendCfg(stage=self.stage, device=self.device) self._visualizers: list[BaseVisualizer] = [] self._pending_visualizers: list[BaseVisualizer] = [] self._reset_requested: bool = False @@ -620,11 +626,7 @@ def _resolve_visualizer_cfgs(self) -> list[Any]: f"{install_hints}" ) - # XR auto-start: auto-inject a KitVisualizer when XR is active and no - # Kit visualizer is already present. The KitVisualizer pumps - # app.update() and triggers forward() (via requires_forward_before_step) - # to sync Fabric data so the XR runtime receives up-to-date hand/joint - # transforms each frame. + # XR auto-start needs a Kit visualizer to publish SDP transforms before pumping the app. if self._xr_enabled and bool(self.get_setting("/isaaclab/xr/auto_start")): has_kit = any(getattr(cfg, "visualizer_type", None) == "kit" for cfg in resolved) if not has_kit: diff --git a/source/isaaclab/test/assets/_articulation_iface_test_utils.py b/source/isaaclab/test/assets/_articulation_iface_test_utils.py index a3896243ce9c..8dfa6d01a685 100644 --- a/source/isaaclab/test/assets/_articulation_iface_test_utils.py +++ b/source/isaaclab/test/assets/_articulation_iface_test_utils.py @@ -26,6 +26,7 @@ from isaaclab_physx.assets.articulation.articulation import Articulation as PhysXArticulation from isaaclab_physx.assets.articulation.articulation_data import ArticulationData as PhysXArticulationData from isaaclab_physx.physics import PhysxManager as SimulationManager + from isaaclab_physx.physics.physx_manager import PhysxSceneDataBackend from isaaclab_physx.test.fixtures.views import MockArticulationViewWarp as PhysXMockArticulationViewWarp except ImportError as error: BACKEND_UNAVAILABLE_REASONS["physx"] = f"{type(error).__name__}: {error}" @@ -34,6 +35,7 @@ _mock_physics_sim_view = MagicMock() _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) SimulationManager.get_physics_sim_view = MagicMock(return_value=_mock_physics_sim_view) + SimulationManager._scene_data_backend = PhysxSceneDataBackend() BACKENDS.append("physx") diff --git a/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py b/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py index 92bda8ac518c..271b3dec8b93 100644 --- a/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py +++ b/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py @@ -29,6 +29,7 @@ RigidObjectCollectionData as PhysXRigidObjectCollectionData, ) from isaaclab_physx.physics import PhysxManager as SimulationManager + from isaaclab_physx.physics.physx_manager import PhysxSceneDataBackend from isaaclab_physx.test.fixtures.views import MockRigidBodyViewWarp as PhysXMockRigidBodyViewWarp except ImportError: pass @@ -37,6 +38,7 @@ _mock_physics_sim_view = MagicMock() _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) SimulationManager.get_physics_sim_view = MagicMock(return_value=_mock_physics_sim_view) + SimulationManager._scene_data_backend = PhysxSceneDataBackend() BACKENDS.append("physx") diff --git a/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py b/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py index 3c6d11de4f28..1d9149f2deb2 100644 --- a/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py +++ b/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py @@ -24,6 +24,7 @@ from isaaclab_physx.assets.rigid_object.rigid_object import RigidObject as PhysXRigidObject from isaaclab_physx.assets.rigid_object.rigid_object_data import RigidObjectData as PhysXRigidObjectData from isaaclab_physx.physics import PhysxManager as SimulationManager + from isaaclab_physx.physics.physx_manager import PhysxSceneDataBackend from isaaclab_physx.test.fixtures.views import MockRigidBodyViewWarp as PhysXMockRigidBodyViewWarp except ImportError: pass @@ -32,6 +33,7 @@ _mock_physics_sim_view = MagicMock() _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) SimulationManager.get_physics_sim_view = MagicMock(return_value=_mock_physics_sim_view) + SimulationManager._scene_data_backend = PhysxSceneDataBackend() BACKENDS.append("physx") diff --git a/source/isaaclab/test/envs/test_direct_marl_env.py b/source/isaaclab/test/envs/test_direct_marl_env.py index 935f2ec9bcbd..b1e3ac0abf07 100644 --- a/source/isaaclab/test/envs/test_direct_marl_env.py +++ b/source/isaaclab/test/envs/test_direct_marl_env.py @@ -45,20 +45,3 @@ def test_initialization_and_close(device): assert env._is_closed assert sim_utils.SimulationContext.instance() is None - - -def test_reset_invalidates_renderer_scene_state_cadence(): - """A same-step multi-agent reset must republish renderer scene state.""" - env = None - try: - sim_utils.create_new_stage() - env = DirectMARLEnv(cfg=make_empty_direct_marl_env_cfg()) - env._get_observations = lambda: {} - env.sim.render_context._last_scene_state_step = 7 - - env.reset() - - assert env.sim.render_context._last_scene_state_step is None - finally: - if env is not None: - env.close() diff --git a/source/isaaclab/test/envs/test_env_rendering_logic.py b/source/isaaclab/test/envs/test_env_rendering_logic.py index 8aaf1bf72014..7fb6b5134f49 100644 --- a/source/isaaclab/test/envs/test_env_rendering_logic.py +++ b/source/isaaclab/test/envs/test_env_rendering_logic.py @@ -253,30 +253,6 @@ def wrapped_step(dt): SimulationContext.clear_instance() -@pytest.mark.parametrize("env_type", ["manager_based_env", "manager_based_rl_env", "direct_rl_env"]) -def test_env_reset_invalidates_renderer_scene_state_cadence(env_type): - """A same-step reset must force the next camera read to republish scene state.""" - env = None - try: - sim_utils.create_new_stage() - if env_type == "manager_based_env": - env = create_manager_based_env(render_interval=1) - elif env_type == "manager_based_rl_env": - env = create_manager_based_rl_env(render_interval=1) - else: - env = create_direct_rl_env(render_interval=1) - - env.sim.render_context._last_scene_state_step = 7 - env.reset() - - assert env.sim.render_context._last_scene_state_step is None - finally: - if env is not None: - env.close() - else: - SimulationContext.clear_instance() - - @pytest.mark.parametrize("env_type", ["manager_based_env", "manager_based_rl_env", "direct_rl_env"]) def test_env_render_false_skips_rendering(env_type, physics_callback, render_callback): """Test that setting render_enabled=False skips all rendering while physics continues.""" diff --git a/source/isaaclab/test/renderers/test_simulation_render_context.py b/source/isaaclab/test/renderers/test_simulation_render_context.py index 84a60c13dfda..3ada5ae132d9 100644 --- a/source/isaaclab/test/renderers/test_simulation_render_context.py +++ b/source/isaaclab/test/renderers/test_simulation_render_context.py @@ -142,6 +142,7 @@ def test_close_backend_removes_renderer_from_orchestration(sim): sim.render_context.update_scene_state(2) replacement.prepare_stage.assert_called_once_with(None, 4) replacement.update_transforms.assert_called_once_with() + replacement.update_geometries.assert_called_once_with() sim.render_context.close() renderer.close.assert_called_once_with() replacement.close.assert_not_called() @@ -160,15 +161,17 @@ def test_prepare_stage_is_idempotent_and_checks_env_count_until_reset(sim): assert renderer.prepare_stage.call_args_list == [call(None, 4), call(None, 8)] -def test_scene_state_updates_once_per_step_until_cadence_reset(sim): +def test_scene_state_does_not_skip_writes_within_a_physics_step(sim): renderer = sim.get_or_create_backend(RendererCfg(class_type=_renderer)) for step in (1, 1, 2): sim.render_context.update_scene_state(step) - assert renderer.update_transforms.call_count == renderer.update_geometries.call_count == 2 + assert renderer.update_transforms.call_count == 3 + assert renderer.update_geometries.call_count == 2 sim.render_context.reset_scene_state_cadence() sim.render_context.update_scene_state(2) - assert renderer.update_transforms.call_count == renderer.update_geometries.call_count == 3 + assert renderer.update_transforms.call_count == 4 + assert renderer.update_geometries.call_count == 3 @pytest.mark.parametrize("profile", [False, True]) @@ -186,6 +189,7 @@ def test_render_into_camera_call_order_and_profile_output(sim, capsys, profile): call.update_geometries(), call.render(data), call.read_output(data, camera), + call.update_transforms(), call.render(data), call.read_output(data, camera), ] diff --git a/source/isaaclab/test/scene_data/test_scene_data_transforms.py b/source/isaaclab/test/scene_data/test_scene_data_transforms.py index 94539be89ccd..b7b8ef8e36b1 100644 --- a/source/isaaclab/test/scene_data/test_scene_data_transforms.py +++ b/source/isaaclab/test/scene_data/test_scene_data_transforms.py @@ -8,13 +8,21 @@ from __future__ import annotations from types import SimpleNamespace +from unittest.mock import Mock import numpy as np import pytest import warp as wp -from isaaclab.scene_data.scene_data_backend import SceneDataFormat +from isaaclab.scene_data.scene_data_backend import SceneDataBackend, SceneDataFormat from isaaclab.scene_data.scene_data_provider import SceneDataProvider +from isaaclab.test.utils import test_devices + + +class _Backend(SimpleNamespace, SceneDataBackend): + transforms = None + transform_count = 0 + transform_paths = () @pytest.mark.skipif( @@ -27,8 +35,9 @@ def test_get_transforms_matches_backend_device_when_warp_default_is_cuda(): [[x, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] for x in range(3)], dtype=wp.transformf, device="cpu" ) provider = SceneDataProvider( - SimpleNamespace( + _Backend( transforms=transforms, + transforms_version=0, transform_count=3, transform_paths=["/World/a", "/World/b", "/World/c"], ) @@ -45,3 +54,193 @@ def test_get_transforms_matches_backend_device_when_warp_default_is_cuda(): assert str(output.positions.device) == "cpu" assert str(output.orientations.device) == "cpu" assert np.allclose(output.positions.numpy()[:, 0], [2.0, 0.0, 1.0]) + + +def test_publication_aliases_native_pointer_and_converts_once_per_write(monkeypatch): + """Clean reads share conversions; no provider can hide a publication from another.""" + data = SceneDataFormat.Transform() + data.transforms = wp.array([[1, 2, 3, 0, 0, 0, 1]], dtype=wp.transformf, device="cpu") + backend = _Backend(transforms=data, transforms_version=0, transform_count=1) + provider = SceneDataProvider(backend) + native = SceneDataFormat.Transform() + converted = SceneDataFormat.Vec3_Quat() + other = SceneDataFormat.Vec3_Quat() + with pytest.raises(ValueError, match="destination count"): + provider.get_transforms(native, count=2) + launch = Mock(wraps=wp.launch) + monkeypatch.setattr(wp, "launch", launch) + assert provider.get_transforms(native) + assert native.transforms is data.transforms + launch.assert_not_called() + assert provider.get_transforms(converted) + assert provider.get_transforms(other) + assert other.positions is converted.positions + assert launch.call_count == 1 + np.testing.assert_array_equal(converted.positions.numpy(), [[1, 2, 3]]) + # A consumer can rebind its wrapper without changing another consumer's arrays. + converted.positions = None + assert provider.get_transforms(converted) + assert converted.positions is other.positions + + data.transforms.assign([[4, 5, 6, 0, 0, 0, 1]]) + backend.transforms_version += 1 + assert provider.get_transforms(converted) + assert converted.positions is other.positions + assert launch.call_count == 2 + np.testing.assert_array_equal(converted.positions.numpy(), [[4, 5, 6]]) + + data.transforms = wp.array([[7, 8, 9, 0, 0, 0, 1]], dtype=wp.transformf, device="cpu") + backend.transforms_version += 1 + assert provider.get_transforms(native) + assert native.transforms is data.transforms + assert provider.get_transforms(converted) + assert converted.positions is other.positions + assert launch.call_count == 3 + np.testing.assert_array_equal(converted.positions.numpy(), [[7, 8, 9]]) + + peer = SceneDataProvider(backend) + peer_output = SceneDataFormat.Vec3_Quat() + assert peer.get_transforms(peer_output) + for position in ([10, 11, 12], [13, 14, 15]): + data.transforms.assign([position + [0, 0, 0, 1]]) + backend.transforms_version += 1 + assert provider.get_transforms(converted) + assert peer.get_transforms(peer_output) + np.testing.assert_array_equal(converted.positions.numpy(), [position]) + np.testing.assert_array_equal(peer_output.positions.numpy(), [position]) + + +@pytest.mark.parametrize("format_name", ["Transform", "Vec3_Quat"]) +def test_owned_transform_buffers_are_written_directly_and_do_not_alias_cache(format_name, monkeypatch): + data = SceneDataFormat.Transform() + data.transforms = wp.array([[1, 2, 3, 0, 0, 0, 1]], dtype=wp.transformf, device="cpu") + backend = _Backend(transforms=data, transforms_version=0, transform_count=1) + provider = SceneDataProvider(backend) + shared, owned = (getattr(SceneDataFormat, format_name)() for _ in range(2)) + assert provider.get_transforms(shared) + provider.init_output(owned) + arrays = [getattr(owned, name) for name in owned._cls.vars] + launch, copy = Mock(wraps=wp.launch), Mock(wraps=wp.copy) + monkeypatch.setattr(wp, "launch", launch) + monkeypatch.setattr(wp, "copy", copy) + for x in (4, 7): + data.transforms.assign([[x, 5, 6, 0, 0, 0, 1]]) + backend.transforms_version += 1 + launch.reset_mock() + copy.reset_mock() + assert provider.get_transforms(owned, allow_passthrough=False) + assert launch.call_count == int(format_name != "Transform") + assert copy.call_count == int(format_name == "Transform") + assert provider.get_transforms(shared) + for name, array in zip(owned._cls.vars, arrays): + assert getattr(owned, name) is array + assert array is not getattr(shared, name) + np.testing.assert_array_equal(array.numpy(), getattr(shared, name).numpy()) + + +def test_mapping_preserves_unmapped_destination_slots(): + data = SceneDataFormat.Transform() + data.transforms = wp.array([[1, 2, 3, 0, 0, 0, 1], [4, 5, 6, 0, 0, 0, 1]], dtype=wp.transformf, device="cpu") + provider = SceneDataProvider( + _Backend(transforms=data, transforms_version=0, transform_count=2, transform_paths=["/a", "/b"]) + ) + mapping = provider.create_mapping(["/a", "/b", None]) + output = SceneDataFormat.Transform() + output.transforms = wp.zeros(3, dtype=wp.transformf, device="cpu") + assert provider.get_transforms(output, mapping, allow_passthrough=False, count=3) + np.testing.assert_array_equal(output.transforms.numpy()[:2], data.transforms.numpy()) + np.testing.assert_array_equal(output.transforms.numpy()[2], np.zeros(7)) + + +@pytest.mark.parametrize("format_name", ["Transform", "Vec3_Quat", "Vec3_Matrix33", "Matrix44"]) +@pytest.mark.parametrize("scaled", [False, True]) +def test_transposed_matrices_fuse_format_mapping_and_scale(format_name, scaled): + """All native formats produce the same row-vector matrices, with output-indexed scale.""" + poses = np.array([[1, 2, 3, 0, 0, 0, 1], [4, 5, 6, 1, 2, 2, 2]], dtype=np.float32) + poses[1, 3:] /= np.sqrt(13) + # Independent quaternion-to-matrix reference; a non-axis rotation exposes transpose/scale ordering errors. + rotations = np.array([np.eye(3), np.array([[-3, -4, 12], [12, 3, 4], [-4, 12, 3]]) / 13], dtype=np.float32) + matrices = np.broadcast_to(np.eye(4), (2, 4, 4)).copy() + matrices[:, :3, :3] = rotations + matrices[:, :3, 3] = poses[:, :3] + data = getattr(SceneDataFormat, format_name)() + if format_name == "Transform": + data.transforms = wp.array(poses, dtype=wp.transformf, device="cpu") + elif format_name == "Matrix44": + data.matrices = wp.array(matrices, dtype=wp.mat44f, device="cpu") + else: + data.positions = wp.array(poses[:, :3], dtype=wp.vec3f, device="cpu") + data.orientations = wp.array( + poses[:, 3:] if format_name == "Vec3_Quat" else rotations, + dtype=wp.quatf if format_name == "Vec3_Quat" else wp.mat33f, + device="cpu", + ) + provider = SceneDataProvider(_Backend(transforms=data, transforms_version=0, transform_count=2)) + mapping = wp.array([1, 0], dtype=wp.int32, device="cpu") + scales = wp.array([[2, 3, 4], [5, 6, 7]], dtype=wp.vec3f, device="cpu") if scaled else None + output = SceneDataFormat.TransposedMatrix44d() + assert provider.get_transforms(output, mapping, scales=scales) + expected = matrices[::-1].transpose(0, 2, 1).copy() + if scaled: + expected[:, :3, :3] *= scales.numpy()[:, :, None] + np.testing.assert_allclose(output.matrices.numpy(), expected, rtol=1.0e-6, atol=1.0e-6) + matrices = output.matrices + assert provider.get_transforms(output, mapping, scales=scales) + assert output.matrices is matrices + + +@pytest.mark.parametrize("format_name", ["Transform", "Vec3_Quat", "Vec3_Matrix33", "Matrix44"]) +@pytest.mark.parametrize("device", test_devices()) +def test_fabric_conversion_preserves_scale_and_refreshes_reallocated_destinations(format_name, device, monkeypatch): + """Fabric conversion skips solver-only bodies and preserves scales across buffer reallocations.""" + assert set(SceneDataFormat.FabricMatrix44.vars) == {"matrices"} + poses = np.array([[1, 2, 3, 0, 0, 0, 1], [7, 8, 9, 0, 0, 0, 1], [4, 5, 6, 1, 2, 2, 2]], dtype=np.float32) + poses[2, 3:] /= np.sqrt(13) + data = SceneDataFormat.Transform() + data.transforms = wp.array(poses, dtype=wp.transformf, device=device) + native = SceneDataProvider(_Backend(transforms=data, transforms_version=0, transform_count=len(poses))) + source = getattr(SceneDataFormat, format_name)() + assert native.get_transforms(source) + provider = SceneDataProvider(_Backend(transforms=source, transforms_version=0, transform_count=len(poses))) + authored_scales = np.array([[5, 6, 7], [1, 1, 1], [2, 3, 4]], dtype=np.float32) + scales = wp.array(authored_scales, dtype=wp.vec3f, device=device) + expected = np.array([np.eye(4), np.diag([5, 6, 7, 1])], dtype=np.float64) + rotation = np.array([[-3, -4, 12], [12, 3, 4], [-4, 12, 3]]) / 13 + expected[0, :3, :3] = np.diag([2, 3, 4]) @ rotation.T + expected[:, 3, :3] = [[4, 5, 6], [1, 2, 3]] + indices = wp.array([len(poses) - 1, 0], dtype=wp.int32, device=device) + launch = Mock(wraps=wp.launch) + monkeypatch.setattr(wp, "launch", launch) + for _ in range(2): + matrices = wp.empty(2, dtype=wp.mat44d, device=device) + interface = { + "version": 1, + "device": device, + "attribs": { + "mapping": { + "type": (True, "i4", 1, 0, ""), + "access": 1, + "pointers": [indices.ptr], + "counts": [2], + }, + "matrices": { + "type": (True, "f8", 16, 0, "matrix"), + "access": 2, + "pointers": [matrices.ptr], + "counts": [2], + }, + }, + } + storage = SimpleNamespace(__fabric_arrays_interface__=interface) + output = SceneDataFormat.FabricMatrix44() + output.matrices = wp.fabricarray(storage, "matrices") + mapping = wp.fabricarray(storage, "mapping") + destination = output.matrices + launch.reset_mock() + assert provider.get_transforms(output, mapping, scales=scales) + assert provider.get_transforms(output, mapping, scales=scales) + assert output.matrices is destination + launch.assert_called_once() + assert len(provider._transform_cache) == 1 + np.testing.assert_allclose(matrices.numpy(), expected, rtol=1.0e-6, atol=1.0e-6) + np.testing.assert_array_equal(scales.numpy(), authored_scales) diff --git a/source/isaaclab/test/sim/test_newton_manager_visualization_state.py b/source/isaaclab/test/sim/test_newton_manager_visualization_state.py index f5ca56085da3..9b5be2fa473a 100644 --- a/source/isaaclab/test/sim/test_newton_manager_visualization_state.py +++ b/source/isaaclab/test/sim/test_newton_manager_visualization_state.py @@ -236,6 +236,7 @@ def test_visualization_model_is_built_during_clone_and_allocated_on_physics_read monkeypatch, body_count, particle_count ): """Cloning owns parsing; READY owns native allocation; getters never discover or allocate.""" + import warp as wp from isaaclab_newton.cloner import NewtonReplicateContext from isaaclab_newton.cloner import replicate as replicate_module from isaaclab_newton.physics import NewtonManager @@ -244,6 +245,7 @@ def test_visualization_model_is_built_during_clone_and_allocated_on_physics_read from pxr import Usd, UsdGeom from isaaclab.physics import PhysicsEvent, PhysicsManager + from isaaclab.scene_data import SceneDataFormat, SceneDataProvider from isaaclab.sim import SimulationContext class ForeignPhysicsManager(PhysicsManager): @@ -265,15 +267,30 @@ class ForeignPhysicsManager(PhysicsManager): UsdGeom.Xform.Define(sim.stage, "/Scene/Source") sim.physics_manager = ForeignPhysicsManager sim._backend_registry = [] - sim._scene_data_provider = SimpleNamespace(backend=object(), point_count=0) + body_paths = [f"/Scene/Body_{index}" for index in range(body_count)] + transforms = SceneDataFormat.Transform() + transforms.transforms = wp.zeros(body_count, dtype=wp.transformf, device="cpu") + sim._scene_data_provider = SceneDataProvider( + SimpleNamespace( + transforms=transforms, + get_transforms=lambda _format: transforms, + transforms_version=0, + transform_paths=body_paths, + transform_count=body_count, + point_count=0, + ) + ) monkeypatch.setattr(SimulationContext, "_instance", sim) finalize = Mock( side_effect=lambda device: SimpleNamespace( body_count=body_count, + body_label=body_paths, particle_count=particle_count, world_count=2, - state=lambda: SimpleNamespace(body_q=None, particle_q=None), + state=lambda: SimpleNamespace( + body_q=wp.empty(body_count, dtype=wp.transformf, device="cpu") if body_count else None, particle_q=None + ), ) ) monkeypatch.setattr(ModelBuilder, "finalize", finalize) @@ -291,6 +308,8 @@ class ForeignPhysicsManager(PhysicsManager): assert not sim._backend_registry ForeignPhysicsManager.dispatch_event(PhysicsEvent.PHYSICS_READY) + if body_count: + assert NewtonManager.get_state_0().body_q is transforms.transforms first_model = NewtonManager.get_model() first_state = NewtonManager.get_state() ForeignPhysicsManager.dispatch_event(PhysicsEvent.PHYSICS_READY) @@ -310,67 +329,65 @@ class ForeignPhysicsManager(PhysicsManager): ForeignPhysicsManager.dispatch_event(PhysicsEvent.STOP) -def test_update_visualization_state_noop_when_backend_is_newton(monkeypatch): - """When sim backend is Newton, update_visualization_state is a no-op.""" - from isaaclab_newton.physics import NewtonManager - - _reset_newton_manager_state() - monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls, scene_data_provider=None: True)) - monkeypatch.setattr(NewtonManager, "get_scene_data_provider", classmethod(lambda cls: SimpleNamespace())) - - # Pre-set sentinel values to ensure update doesn't touch them. - monkeypatch.setattr(NewtonManager, "backend", SimpleNamespace(model="live-model", state_0="live-state")) - NewtonManager.update_visualization_state() - assert NewtonManager.backend.model == "live-model" - assert NewtonManager.backend.state_0 == "live-state" - - -@pytest.mark.parametrize("newton_active", [True, False]) -def test_get_state_forwards_only_for_live_newton_state(monkeypatch, newton_active): - """PhysX shadow state keeps its visualization update without entering Newton FK.""" - from isaaclab_newton.physics import NewtonManager - - events: list[str] = [] - state = object() - monkeypatch.setattr(NewtonManager, "_fk_reset_mask", object(), raising=False) - monkeypatch.setattr( - NewtonManager, - "_backend_is_newton", - classmethod(lambda cls, provider=None: newton_active), - ) - monkeypatch.setattr(NewtonManager, "forward", classmethod(lambda cls: events.append("forward"))) - monkeypatch.setattr( - NewtonManager, - "update_visualization_state", - classmethod(lambda cls, provider=None: events.append("visualization")), - ) - monkeypatch.setattr(NewtonManager, "get_state_0", classmethod(lambda cls: state)) - - assert NewtonManager.get_state() is state - expected = ["forward", "visualization"] if newton_active else ["visualization"] - assert events == expected - - -def test_scene_data_reads_through_public_state_boundary(monkeypatch): - """SceneData does not bypass the coherent Newton state accessor.""" +@pytest.mark.parametrize("invalidate", ["invalidate_body_state", "invalidate_fk"]) +def test_native_publication_reuses_clean_fk_and_refreshes_writes_and_swaps(monkeypatch, invalidate): + """Clean native reads reuse FK and conversions; writes and solver-buffer swaps refresh their values.""" import warp as wp - from isaaclab_newton.physics import NewtonManager - from isaaclab_newton.physics import newton_manager as nm + from isaaclab_newton.physics import NewtonManager, NewtonXPBDManager + from isaaclab_newton.physics.newton_manager import NewtonSceneDataBackend + + from isaaclab.physics import PhysicsManager + from isaaclab.scene_data import SceneDataFormat, SceneDataProvider - events: list[str] = [] - body_q = wp.zeros(1, dtype=wp.transformf, device="cpu") - state = SimpleNamespace(body_q=body_q) - backend = nm.NewtonSceneDataBackend() + _reset_newton_manager_state() + monkeypatch.setattr(PhysicsManager, "_device", "cpu") + state = SimpleNamespace(body_q=wp.array([[0, 0, 0, 0, 0, 0, 1]], dtype=wp.transformf, device="cpu")) + backend = NewtonSceneDataBackend() + provider = SceneDataProvider(backend) monkeypatch.setattr( - NewtonManager, - "get_state", - classmethod(lambda cls, provider=None: events.append("state") or state), + NewtonManager, "backend", SimpleNamespace(model=SimpleNamespace(body_count=1, world_count=1), state_0=state) ) - - transforms = backend.transforms - - assert events == ["state"] - assert transforms.transforms is body_q + monkeypatch.setattr(NewtonManager, "_scene_data_backend", backend) + monkeypatch.setattr(NewtonManager, "_world_reset_mask", wp.zeros(2, dtype=wp.bool, device="cpu")) + monkeypatch.setattr(NewtonManager, "_fk_reset_mask", wp.zeros(1, dtype=wp.bool, device="cpu")) + # Fabric may bind between native allocation and the solver's FK-hook initialization. + assert backend.transforms.transforms is state.body_q + monkeypatch.setattr(NewtonManager, "_eval_fk", Mock()) + monkeypatch.setattr(NewtonManager, "_reset_solver_internals_delegate", Mock()) + monkeypatch.setattr(wp, "launch", Mock(wraps=wp.launch)) + + output = SceneDataFormat.Matrix44() + assert provider.get_transforms(output) + matrices = output.matrices + NewtonManager.pre_render() + NewtonManager._eval_fk.assert_not_called() + NewtonManager.get_state(provider) + assert provider.get_transforms(output) + assert output.matrices is matrices + assert wp.launch.call_count == 1 + NewtonManager._eval_fk.assert_not_called() + + state.body_q.assign([[1, 2, 3, 0, 0, 0, 1]]) + getattr(NewtonXPBDManager, invalidate)() + assert provider.get_transforms(output) + assert output.matrices is matrices + np.testing.assert_allclose(output.matrices.numpy()[0, :3, 3], [1, 2, 3]) + NewtonManager._eval_fk.assert_called_once() + assert provider.get_transforms(output) + assert output.matrices is matrices + NewtonManager.pre_render() + NewtonManager._eval_fk.assert_called_once() + assert wp.launch.call_count == 2 + + replacement = wp.array([[3, 2, 1, 0, 0, 0, 1]], dtype=wp.transformf, device="cpu") + NewtonManager.backend.state_0 = SimpleNamespace(body_q=replacement) + native = SceneDataFormat.Transform() + assert provider.get_transforms(native) + assert native.transforms is replacement + assert provider.get_transforms(output) + assert output.matrices is matrices + assert wp.launch.call_count == 3 + np.testing.assert_allclose(output.matrices.numpy()[0, :3, 3], [3, 2, 1]) def test_resolve_scene_data_body_paths_uses_joint_body_targets(): @@ -392,8 +409,9 @@ def test_resolve_scene_data_body_paths_uses_joint_body_targets(): assert resolved_paths == ["/World/envs/env_0/Robot/robot0_forearm"] -def test_update_visualization_state_copies_identity_mapped_transforms(monkeypatch): - """Identity-mapped transforms update the persistent Newton shadow buffer.""" +@pytest.mark.parametrize("layout", ["identity", "reordered", "missing", "duplicate"]) +def test_update_visualization_state_shares_sdp_transforms(monkeypatch, layout): + """Native and reordered layouts bind shared output once and refresh only on publication.""" import numpy as np import warp as wp from isaaclab_newton.physics import NewtonManager @@ -404,6 +422,12 @@ def test_update_visualization_state_copies_identity_mapped_transforms(monkeypatc monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls, provider=None: False)) body_paths = ["/World/envs/env_0/Object", "/World/envs/env_1/Object"] + render_paths = { + "identity": body_paths, + "reordered": body_paths[::-1], + "missing": [body_paths[0], "/World/Missing"], + "duplicate": [body_paths[0], body_paths[0]], + }[layout] source_transforms = wp.array( [ [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], @@ -414,31 +438,46 @@ def test_update_visualization_state_copies_identity_mapped_transforms(monkeypatc ) source_data = SceneDataFormat.Transform() source_data.transforms = source_transforms - provider_impl = SceneDataProvider( - SimpleNamespace(transforms=source_data, transform_paths=body_paths, transform_count=len(body_paths)) - ) - provider = SimpleNamespace( - usd_stage=None, - create_mapping=provider_impl.create_mapping, - get_transforms=provider_impl.get_transforms, - point_count=0, + provider = SceneDataProvider( + SimpleNamespace( + transforms=source_data, + get_transforms=lambda _format: source_data, + transforms_version=0, + transform_paths=body_paths, + transform_count=len(body_paths), + point_count=0, + ) ) + monkeypatch.setattr(SceneDataProvider, "usd_stage", property(lambda self: None)) destination = wp.zeros(len(body_paths), dtype=wp.transformf, device="cpu") monkeypatch.setattr( NewtonManager, "backend", SimpleNamespace( - model=SimpleNamespace(body_label=body_paths, body_count=len(body_paths)), + model=SimpleNamespace(body_label=render_paths, body_count=len(body_paths)), state_0=SimpleNamespace(body_q=destination, particle_q=None), ), ) + if layout in ("missing", "duplicate"): + with pytest.raises(ValueError, match="one unique SDP transform path"): + NewtonManager.update_visualization_state(provider) + return + + remapped = layout == "reordered" NewtonManager.update_visualization_state(provider) + shared = NewtonManager.get_state(provider).body_q + assert (shared is source_transforms) is not remapped + np.testing.assert_allclose(shared.numpy(), source_transforms.numpy()[:: -1 if remapped else 1]) + assert NewtonManager.get_state(provider).body_q is shared - assert NewtonManager.backend.state_0.body_q is destination - assert NewtonManager._scene_data.transforms is destination - np.testing.assert_allclose(destination.numpy(), source_transforms.numpy()) + source_data.transforms = wp.array(source_transforms.numpy() + 1.0, dtype=wp.transformf, device="cpu") + provider.backend.transforms_version += 1 + NewtonManager.update_visualization_state(provider) + np.testing.assert_allclose( + NewtonManager.backend.state_0.body_q.numpy(), source_data.transforms.numpy()[:: -1 if remapped else 1] + ) def test_update_visualization_state_syncs_shadow_particle_q(monkeypatch): @@ -460,14 +499,13 @@ def test_update_visualization_state_syncs_shadow_particle_q(monkeypatch): [2], ) ) - monkeypatch.setattr(provider, "get_transforms", lambda output, mapping=None, allow_passthrough=True: True) particle_q = wp.zeros(2, dtype=wp.vec3f, device="cpu") NewtonManager = _prepare_physx_shadow_sync( monkeypatch, provider, model=SimpleNamespace(body_label=["/World/envs/env_0/Robot"]), - state_0=SimpleNamespace(body_q=wp.zeros(1, dtype=wp.transformf, device="cpu"), particle_q=particle_q), + state_0=SimpleNamespace(body_q=None, particle_q=particle_q), entities=[_make_shadow_entity(cloth_path, sim_particle_count=2)], sim_particle_count=2, ) @@ -504,14 +542,13 @@ def test_update_visualization_state_remaps_volume_vis_positions(monkeypatch): [4], ) ) - monkeypatch.setattr(provider, "get_transforms", lambda output, mapping=None, allow_passthrough=True: True) particle_q = wp.zeros(1, dtype=wp.vec3f, device="cpu") NewtonManager = _prepare_physx_shadow_sync( monkeypatch, provider, model=SimpleNamespace(body_label=["/World/envs/env_0/Robot"]), - state_0=SimpleNamespace(body_q=wp.zeros(1, dtype=wp.transformf, device="cpu"), particle_q=particle_q), + state_0=SimpleNamespace(body_q=None, particle_q=particle_q), entities=[ _make_shadow_entity( soft_path, @@ -542,7 +579,6 @@ def test_sync_skips_unmapped_deformable_rest_pose(monkeypatch): [2], ) ) - monkeypatch.setattr(provider, "get_transforms", lambda output, mapping=None, allow_passthrough=True: True) particle_q = wp.array( [ @@ -558,7 +594,7 @@ def test_sync_skips_unmapped_deformable_rest_pose(monkeypatch): monkeypatch, provider, model=SimpleNamespace(body_label=["/World/envs/env_0/Robot"]), - state_0=SimpleNamespace(body_q=wp.zeros(1, dtype=wp.transformf, device="cpu"), particle_q=particle_q), + state_0=SimpleNamespace(body_q=None, particle_q=particle_q), entities=[ _make_shadow_entity("/World/envs/env_0/ClothA", sim_particle_count=2), _make_shadow_entity( @@ -596,7 +632,6 @@ def test_sync_skips_mismatched_volume_without_remap(monkeypatch): [4], ) ) - monkeypatch.setattr(provider, "get_transforms", lambda output, mapping=None, allow_passthrough=True: True) # Vis-sized render buffer initialized to a sentinel rest pose. particle_q = wp.array([wp.vec3(9.0, 9.0, 9.0)], dtype=wp.vec3f, device="cpu") @@ -604,7 +639,7 @@ def test_sync_skips_mismatched_volume_without_remap(monkeypatch): monkeypatch, provider, model=SimpleNamespace(body_label=["/World/envs/env_0/Robot"]), - state_0=SimpleNamespace(body_q=wp.zeros(1, dtype=wp.transformf, device="cpu"), particle_q=particle_q), + state_0=SimpleNamespace(body_q=None, particle_q=particle_q), entities=[ _make_shadow_entity( soft_path, diff --git a/source/isaaclab_newton/changelog.d/sdp-transform-transport.rst b/source/isaaclab_newton/changelog.d/sdp-transform-transport.rst new file mode 100644 index 000000000000..a31eaeb5f264 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/sdp-transform-transport.rst @@ -0,0 +1,11 @@ +Changed +^^^^^^^ + +* Shared Newton rigid-body transforms through SceneDataProvider publications, including solver state-buffer + swaps, and moved Fabric bindings into the Kit rendering integration. Explicit Fabric synchronization + continued to work without a Kit viewer or RTX camera. Newton render-only states under foreign + physics now reference shared SDP transforms instead of copying them; consumers must treat their + ``body_q`` arrays as read-only. Particle and cable synchronization remained unchanged. +* Reconciled authored state writes only while pending, instead of re-running forward kinematics for + every new transform publication. Rendering requested rigid Fabric updates through SDP rather + than the physics pre-render hook. Captured external writes retained conservative reconciliation. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 217acca0084c..94007a30365c 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -142,53 +142,6 @@ def _compile_label_pattern(expr: str | list[str] | None) -> re.Pattern[str] | No # _LocalSite: (None, [[env0_idx, ...], ...]) — per-world site indices -@wp.kernel(enable_backward=False) -def _capture_fabric_scales( - fabric_transforms: wp.fabricarray(dtype=wp.mat44d), - newton_indices: wp.fabricarray(dtype=wp.uint32), - body_scales: wp.array(dtype=wp.vec3f), -): - """Capture initialized Fabric world scales by Newton body index.""" - i = int(wp.tid()) - idx = int(newton_indices[i]) - matrix = wp.mat44f(fabric_transforms[i]) - body_scales[idx] = wp.vec3f( - wp.length(wp.vec3f(matrix[0, 0], matrix[0, 1], matrix[0, 2])), - wp.length(wp.vec3f(matrix[1, 0], matrix[1, 1], matrix[1, 2])), - wp.length(wp.vec3f(matrix[2, 0], matrix[2, 1], matrix[2, 2])), - ) - - -@wp.kernel(enable_backward=False) -def _set_fabric_transforms( - fabric_transforms: wp.fabricarray(dtype=wp.mat44d), - newton_indices: wp.fabricarray(dtype=wp.uint32), - newton_body_q: wp.array(ndim=1, dtype=wp.transformf), - body_scales: wp.array(dtype=wp.vec3f), -): - """Write Newton body poses to Fabric world matrices with their initialized scale. - - For each Fabric prim at thread ``i``, reads the Newton body transform at - ``newton_body_q[newton_indices[i]]`` and combines its translation and rotation - with the corresponding scale captured from the initialized Fabric world matrix. - Newton transforms do not carry scale, so reapplying the captured value prevents - authored USD scale from being overwritten with unit scale during rendering sync. - """ - i = int(wp.tid()) - idx = int(newton_indices[i]) - transform = newton_body_q[idx] - scale = body_scales[idx] - fabric_transforms[i] = wp.mat44d( - wp.transpose( - wp.transform_compose( - wp.transform_get_translation(transform), - wp.transform_get_rotation(transform), - scale, - ) - ) - ) - - @wp.kernel(enable_backward=False) def _sync_particle_points( fabric_points: wp.fabricarrayarray(dtype=wp.vec3f), @@ -351,13 +304,17 @@ class NewtonSceneDataBackend(SceneDataBackend): """ def __init__(self): - self._scene_data = SceneDataFormat.Transform() + self._transforms = SceneDataFormat.Transform() + self.transforms_version = 0 @property def transforms(self) -> SceneDataFormat.Transform: - """Return the current Newton rigid body transforms as :class:`SceneDataFormat.Transform`.""" - self._scene_data.transforms = self.state.body_q - return self._scene_data + """Publish the authoritative native pointer, including solver state-buffer swaps.""" + transforms = self.state.body_q + if self._transforms.transforms is not transforms: + self._transforms.transforms = transforms + self.transforms_version += 1 + return self._transforms @property def transform_count(self) -> int: @@ -377,8 +334,13 @@ def model(self) -> Model: @property def state(self) -> State: - """Return Newton state after applying pending forward kinematics.""" - return NewtonManager.get_state() + """Return native physics state without entering the rendering consumer path.""" + if NewtonManager._transforms_may_change_on_graph_replay: + # Raw external graph replays bypass Python invalidation, so these reads must stay conservative. + self.transforms_version += 1 + if NewtonManager._eval_fk is not _eval_fk_unbound: + NewtonManager.forward() + return NewtonManager.get_state_0() def _eval_fk_unbound(world_reset_mask: wp.array | None, fk_mask: wp.array | None) -> None: @@ -465,6 +427,7 @@ class NewtonManager(PhysicsManager): # Newton reserves the final slot for global entities in world -1. _world_reset_mask: wp.array | None = None # (num_envs + 1,) wp.bool _fk_reset_mask: wp.array | None = None # (articulation_count,) wp.bool — for eval_fk(mask=...) + _reconciliation_pending: bool = False # Solver-specialized FK delegate. Bound in initialize_solver() to the active subclass's choice of FK implementation. _eval_fk: Callable[[wp.array | None, wp.array | None], None] = _eval_fk_unbound # Solver-specialized reset delegate. Like _eval_fk, this must dispatch correctly through the base manager. @@ -500,13 +463,8 @@ class NewtonManager(PhysicsManager): _sensor_bvh_shape_flags: ShapeFlags = ShapeFlags.VISIBLE # USD/Fabric sync - _newton_stage_path = None _usdrt_stage = None - _newton_index_attr = "newton:index" - # Body-indexed world scales captured before Newton first overwrites Fabric transforms. - _fabric_body_scales: wp.array | None = None _clone_physics_only = False - _transforms_dirty: bool = False _transforms_may_change_on_graph_replay: bool = False _particles_dirty: bool = False _cables_dirty: bool = False @@ -518,15 +476,6 @@ class NewtonManager(PhysicsManager): _newton_particle_count_attr = "newton:particleCount" _particle_visual_prims: dict[str, _ParticleVisualPrim] = {} - # Cached after the first fabric sync that probes IFabricHierarchy GPU APIs. - _use_fabric_gpu_hierarchy: bool | None = None - - # Set to True after sync_transforms_to_fabric() successfully writes body positions for - # the first time in each simulation session. Reset to False in clear(). Polled by - # test drain helpers to know when the GPU has propagated the newton:index Fabric - # attribute and body_q values are valid. - _newton_fabric_ready: bool = False - # Model changes (callbacks use unified system from PhysicsManager) _model_changes: set[int] = set() @@ -536,8 +485,8 @@ class NewtonManager(PhysicsManager): # Visualization-only state used when the sim backend is PhysX. Populated # from the clone plan in :meth:`_initialize_visualization_model` and updated each render # frame in :meth:`update_visualization_state`. - _scene_data: SceneDataFormat.Transform | None = None _scene_data_mapping: wp.array | None = None + _scene_data_version: int | None = None _scene_data_points: SceneDataFormat.Points | None = None _scene_data_geometry_mapping: wp.array | None = None _shadow_deformable_entities: list | None = None @@ -606,7 +555,7 @@ def initialize(cls, sim_context: SimulationContext) -> None: cameras_enabled = bool(get_settings_manager().get("/isaaclab/cameras_enabled", False)) cls._clone_physics_only = not has_kit() or ("kit" not in requested and not cameras_enabled) - cls._scene_data_backend = NewtonSceneDataBackend() + NewtonManager._scene_data_backend = NewtonSceneDataBackend() @classmethod def reset(cls, soft: bool = False) -> None: @@ -633,6 +582,8 @@ def reset(cls, soft: bool = False) -> None: NewtonManager._collision_pipeline = None NewtonManager._contacts = None NewtonManager._solver = None + NewtonManager._eval_fk = _eval_fk_unbound + NewtonManager._reset_solver_internals_delegate = _reset_solver_internals_unbound NewtonManager._adapter = None cls._invalidate_sensor_graph() NewtonManager._sensor_state = None @@ -675,12 +626,17 @@ def forward(cls) -> None: data layer invokes ``NewtonManager.forward()`` on the base class, where ``cls`` is the base ``NewtonManager``; the bound delegate dispatches to the concrete subclass override. """ + if cls._eval_fk is not _eval_fk_unbound and not ( + cls._reconciliation_pending or cls._transforms_may_change_on_graph_replay + ): + return cls._reset_solver_internals_delegate(cls._world_reset_mask) cls._eval_fk(cls._world_reset_mask, cls._fk_reset_mask) if cls._fk_reset_mask is not None: cls._fk_reset_mask.zero_() if cls._world_reset_mask is not None: cls._world_reset_mask.zero_() + NewtonManager._reconciliation_pending = False cls._mark_sensor_state_dirty() @classmethod @@ -690,142 +646,17 @@ def video_capture_backend(cls) -> str: @classmethod def pre_render(cls) -> None: - """Refresh derived Newton state before cameras and visualizers read it.""" - if cls._fk_reset_mask is not None: - cls.forward() - if NewtonManager._transforms_may_change_on_graph_replay: - cls._mark_transforms_dirty() - cls.sync_transforms_to_fabric() + """Refresh legacy cable and particle geometry; rigid transforms are requested through SDP.""" cls.sync_cables_to_usd() cls.sync_particles_to_usd() @classmethod def sync_transforms_to_fabric(cls) -> None: - """Write Newton body_q to Fabric world matrices for Kit viewport / RTX rendering. - - The write lands in Fabric only. Authored USD attributes are left untouched, so the - poses are visible to the RTX renderer but absent from a stage export or save. - - No-op when ``_usdrt_stage`` is None (i.e. Kit visualizer is not active) - or when transforms have not changed since the last sync. - - Called at render cadence by :meth:`pre_render` (via - :meth:`~isaaclab.sim.SimulationContext.render`). - Physics stepping marks transforms dirty via :meth:`_mark_transforms_dirty` - so that the expensive Fabric hierarchy update only runs once per render - frame rather than after every physics step. - - Uses ``wp.fabricarray`` directly (no ``isaacsim.physics.newton`` extension needed). - On the first successful sync, a Warp kernel captures each initialized Fabric - world scale by Newton body index. The pose kernel then combines that scale with - ``state_0.body_q[newton_index[i]]`` and writes the corresponding ``mat44d`` to - ``omni:fabric:worldMatrix`` for each prim. - - When ``IFabricHierarchy.update_world_xforms_gpu_with_options`` is - available the method mirrors PhysX's ``DirectGpuHelper`` pattern: pause - Fabric change tracking, write transforms, resume tracking, then run the - GPU hierarchy update with ``RIGID_BODY | FORCE_UPDATE`` so Newton-authored - world matrices stay authoritative on rigid-body prims. Otherwise it - falls back to the CPU ``update_world_xforms()`` path. - """ + """Publish rigid-body poses through SDP to Fabric, leaving authored USD untouched.""" if cls._usdrt_stage is None or cls.backend is None: return - if not cls._transforms_dirty: - return - try: - import usdrt - - fabric_hierarchy = None - gpu_opts_cls = None - if hasattr(usdrt, "hierarchy"): - fabric_hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( - cls._usdrt_stage.GetFabricId(), cls._usdrt_stage.GetStageIdAsStageId() - ) - gpu_opts_cls = getattr(usdrt.hierarchy, "FabricHierarchyGpuUpdateOptions", None) - - if cls._use_fabric_gpu_hierarchy is None and hasattr(usdrt, "hierarchy"): - # Probe the pybind class once so a transient null hierarchy handle does - # not permanently disable the GPU path for the session. - NewtonManager._use_fabric_gpu_hierarchy = gpu_opts_cls is not None and hasattr( - usdrt.hierarchy.IFabricHierarchy, "update_world_xforms_gpu_with_options" - ) - if cls._use_fabric_gpu_hierarchy: - logger.info("Fabric GPU transform hierarchy enabled via IFabricHierarchy") - else: - logger.info("Fabric GPU transform hierarchy unavailable; falling back to update_world_xforms()") - - use_gpu_hierarchy = bool( - cls._use_fabric_gpu_hierarchy and fabric_hierarchy is not None and gpu_opts_cls is not None - ) - - # Pause hierarchy change tracking BEFORE SelectPrims. - # SelectPrims with ReadWrite access calls getAttributeArrayGpu - # internally, which marks Fabric buffers dirty. If tracking is - # still active at that point the hierarchy records the change and - # Kit's updateWorldXforms will do an expensive connectivity - # rebuild every frame. PhysX avoids this via ScopedUSDRT which - # pauses tracking before any Fabric writes. - if use_gpu_hierarchy: - fabric_hierarchy.track_world_xform_changes(False) - fabric_hierarchy.track_local_xform_changes(False) - - try: - selection = cls._usdrt_stage.SelectPrims( - require_attrs=[ - (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.ReadWrite), - (usdrt.Sdf.ValueTypeNames.UInt, cls._newton_index_attr, usdrt.Usd.Access.Read), - ], - device=str(PhysicsManager._device), - ) - if selection.GetCount() == 0: - # The newton:index attribute is written CPU-side by start_simulation() but - # GPU propagation is deferred. Keep _transforms_dirty=True so the next - # pre_render() retries once initialize_solver() has completed (FK delegate - # bound) and body_q holds valid values. - if cls._eval_fk is _eval_fk_unbound: - NewtonManager._transforms_dirty = False - return - - fabric_transforms = wp.fabricarray(selection, "omni:fabric:worldMatrix") - newton_indices = wp.fabricarray(selection, cls._newton_index_attr) - if cls._fabric_body_scales is None: - NewtonManager._fabric_body_scales = wp.empty( - cls.backend.model.body_count, - dtype=wp.vec3f, - device=PhysicsManager._device, - ) - wp.launch( - _capture_fabric_scales, - dim=newton_indices.shape[0], - inputs=[fabric_transforms, newton_indices, cls._fabric_body_scales], - device=PhysicsManager._device, - ) - wp.launch( - _set_fabric_transforms, - dim=newton_indices.shape[0], - inputs=[fabric_transforms, newton_indices, cls.backend.state_0.body_q, cls._fabric_body_scales], - device=PhysicsManager._device, - ) - wp.synchronize_device(PhysicsManager._device) - - NewtonManager._newton_fabric_ready = True - NewtonManager._transforms_dirty = False - - if use_gpu_hierarchy: - # RIGID_BODY: inverse-propagate on PhysicsRigidBodyAPI buckets - # (keep Newton world matrices, derive local). FORCE_UPDATE: - # bypass the change-listener dirty check after tracking pause. - fabric_hierarchy.update_world_xforms_gpu_with_options( - gpu_opts_cls.RIGID_BODY | gpu_opts_cls.FORCE_UPDATE - ) - elif fabric_hierarchy is not None: - fabric_hierarchy.update_world_xforms() - finally: - if use_gpu_hierarchy: - fabric_hierarchy.track_world_xform_changes(True) - fabric_hierarchy.track_local_xform_changes(True) - except Exception: - logger.exception("[NewtonManager] sync_transforms_to_fabric FAILED") + sim = PhysicsManager._sim + sim.get_or_create_backend(sim.fabric_cfg).update_transforms(sim.get_scene_data_provider()) @classmethod def sync_transforms_to_usd(cls) -> None: @@ -844,7 +675,7 @@ def sync_transforms_to_usd(cls) -> None: @classmethod def sync_cables_to_usd(cls) -> None: """Write Newton cable segment endpoints to Fabric curve points.""" - if not cls._cables_dirty: + if not (cls._cables_dirty or cls._transforms_may_change_on_graph_replay): return if cls._usdrt_stage is None or cls._cable_shape_ids is None: NewtonManager._cables_dirty = False @@ -865,7 +696,7 @@ def sync_cables_to_usd(cls) -> None: NewtonManager._cables_dirty = False return _, _, body_q, _, _ = cls._cable_sync_cpu_buffers - wp.copy(body_q, cls.backend.state_0.body_q) + wp.copy(body_q, cls._scene_data_backend.state.body_q) wp.launch( _sync_cable_points, dim=selection.GetCount(), @@ -960,15 +791,11 @@ def _sync_particle_points_prims(cls) -> bool: return len(due) < len(cls._particle_visual_prims) @classmethod - def _mark_transforms_dirty(cls) -> None: - """Flag that rigid-body transforms have changed and Fabric needs re-sync. - - The actual sync is deferred to :meth:`sync_transforms_to_fabric`, - which runs at render cadence via :meth:`pre_render`. - """ - NewtonManager._transforms_dirty = True + def _mark_transforms_changed(cls) -> None: + """Publish authored rigid-body changes and invalidate cable geometry.""" + if NewtonManager._scene_data_backend is not None: + NewtonManager._scene_data_backend.transforms_version += 1 NewtonManager._cables_dirty = True - device = PhysicsManager._device if device is not None: device = wp.get_device(device) @@ -984,16 +811,6 @@ def _mark_particles_dirty(cls) -> None: """ NewtonManager._particles_dirty = True - @classmethod - def _mark_state_dirty(cls) -> None: - """Flag that all physics state has changed and Fabric needs re-sync. - - Convenience method that marks both transforms and particles dirty. - Called by :meth:`_simulate` after stepping. - """ - cls._mark_transforms_dirty() - cls._mark_particles_dirty() - @classmethod def register_particle_visual_prim( cls, prim_path: str, particle_offset: int, particle_count: int, sync_frequency: int = 1 @@ -1117,9 +934,8 @@ def step(cls) -> None: cls._simulate_physics_only() PhysicsManager._sim_time += physics_dt - if cls._usdrt_stage is not None: - cls._mark_state_dirty() - elif cls._particle_visual_prims: + cls._mark_transforms_changed() + if cls._usdrt_stage is not None or cls._particle_visual_prims: cls._mark_particles_dirty() cls._mark_sensor_state_dirty() @@ -1168,8 +984,6 @@ def clear(cls): NewtonManager._visualization_stop_callback = None if callback is not None: callback.deregister() - NewtonManager._use_fabric_gpu_hierarchy = None - NewtonManager._newton_fabric_ready = False NewtonManager._num_envs = None NewtonManager._builder = None NewtonManager._solver = None @@ -1200,6 +1014,7 @@ def clear(cls): # Per-world reset masks NewtonManager._world_reset_mask = None NewtonManager._fk_reset_mask = None + NewtonManager._reconciliation_pending = False NewtonManager._graph = None NewtonManager._graph_capture_pending = False NewtonManager._sensor_tasks = {} @@ -1207,12 +1022,8 @@ def clear(cls): NewtonManager._invalidate_sensor_graph() NewtonManager._sensor_state = None NewtonManager._sensor_state_dirty = True - NewtonManager._sensor_graph_capture_failed = False NewtonManager._sensor_bvh_shape_flags = ShapeFlags.VISIBLE - NewtonManager._newton_stage_path = None NewtonManager._usdrt_stage = None - NewtonManager._fabric_body_scales = None - NewtonManager._transforms_dirty = False NewtonManager._transforms_may_change_on_graph_replay = False NewtonManager._particles_dirty = False NewtonManager._cables_dirty = False @@ -1223,8 +1034,8 @@ def clear(cls): NewtonManager._deformable_registry = [] NewtonManager._per_world_builder_hooks = [] NewtonManager._up_axis = "Z" - NewtonManager._scene_data = None NewtonManager._scene_data_mapping = None + NewtonManager._scene_data_version = None NewtonManager._scene_data_points = None NewtonManager._scene_data_geometry_mapping = None NewtonManager._shadow_deformable_entities = None @@ -1509,10 +1320,11 @@ def invalidate_fk( index. Shape ``(world_count, count_per_world)``. Obtained from ``ArticulationView.articulation_ids``. """ - cls._mark_transforms_dirty() + cls._mark_transforms_changed() if cls._world_reset_mask is None or cls._fk_reset_mask is None: return + NewtonManager._reconciliation_pending = True if articulation_ids is not None and env_mask is not None: wp.launch( @@ -1547,9 +1359,10 @@ def invalidate_body_state( env_ids: Integer indices of dirtied environments. Used by index write methods. env_mask: Boolean mask of dirtied environments. Used by mask write methods. """ - cls._mark_transforms_dirty() + cls._mark_transforms_changed() if cls._world_reset_mask is None: return + NewtonManager._reconciliation_pending = True if env_mask is not None: wp.launch( _or_world_reset_mask_from_mask, @@ -1685,14 +1498,10 @@ def start_simulation(cls) -> None: NewtonManager._world_reset_mask = wp.zeros(cls.backend.model.world_count + 1, dtype=wp.bool, device=device) NewtonManager._fk_reset_mask = wp.zeros(cls.backend.model.articulation_count, dtype=wp.bool, device=device) - logger.info("Dispatching PHYSICS_READY callbacks") - cls.dispatch_event(PhysicsEvent.PHYSICS_READY) - # Setup USD/Fabric sync for Kit viewport rendering if not cls._clone_physics_only: import usdrt - NewtonManager._fabric_body_scales = None body_paths = list(cls.backend.model.body_label) NewtonManager._usdrt_stage = get_current_stage(fabric=True) body_bindings = NewtonManager._cl_fabric_body_bindings @@ -1706,6 +1515,12 @@ def start_simulation(cls) -> None: NewtonManager._initialize_fabric_body_prims(cls._usdrt_stage, fabric_hierarchy, usdrt, body_bindings) NewtonManager._initialize_fabric_cable_prims(cls._usdrt_stage, fabric_hierarchy, usdrt) + + logger.info("Dispatching PHYSICS_READY callbacks") + cls.dispatch_event(PhysicsEvent.PHYSICS_READY) + + # MPM assets register their particle visualizations during PHYSICS_READY. + if not cls._clone_physics_only: NewtonManager._initialize_fabric_particle_prims( cls._usdrt_stage, fabric_hierarchy, @@ -1713,15 +1528,15 @@ def start_simulation(cls) -> None: NewtonManager._particle_visual_prims, ) - cls._mark_state_dirty() - cls.sync_transforms_to_fabric() + cls._mark_transforms_changed() + cls._mark_particles_dirty() cls.sync_cables_to_usd() cls.sync_particles_to_usd() @staticmethod def _initialize_fabric_body_prims(stage, fabric_hierarchy, usdrt, body_bindings: Sequence[tuple[str, int]]) -> None: """Initialize Fabric body prims used by Newton transform sync.""" - for prim_path, body_index in body_bindings: + for prim_path, _ in body_bindings: prim = stage.GetPrimAtPath(prim_path) if prim.IsValid(): xformable_prim = usdrt.Rt.Xformable(prim) @@ -1731,11 +1546,7 @@ def _initialize_fabric_body_prims(stage, fabric_hierarchy, usdrt, body_bindings: xformable_prim = usdrt.Rt.Xformable(prim) xformable_prim.CreateFabricHierarchyWorldMatrixAttr() - prim.CreateAttribute(NewtonManager._newton_index_attr, usdrt.Sdf.ValueTypeNames.UInt, custom=True) - prim.GetAttribute(NewtonManager._newton_index_attr).Set(body_index) - # Tag with PhysicsRigidBodyAPI so FabricHierarchyGpuUpdateOptions.RIGID_BODY - # applies Inverse propagation (preserves Newton's world transforms and derives - # local) instead of Forward. + # Include native bodies absent from USD in the SDP rigid-transform binding. prim.AddAppliedSchema("PhysicsRigidBodyAPI") fabric_hierarchy.update_world_xforms() @@ -2403,7 +2214,7 @@ def initialize_solver(cls) -> None: # solver-specialized FK delegate, now that the solver and the delegate both exist. # Runs before graph capture below so the capture warmup sees a valid body_q. cls._eval_fk(None, None) - cls._mark_transforms_dirty() + cls._mark_transforms_changed() # Fully graphable Newton actuators defer capture until ``set_decimation`` # provides the environment's final decimation value. Other paths capture @@ -2743,9 +2554,12 @@ def get_state(cls, scene_data_provider: SceneDataProvider | None = None) -> Stat observe stale transforms. Under the Newton sim backend, pending forward kinematics is applied before returning the live state. """ - if cls._fk_reset_mask is not None and cls._backend_is_newton(scene_data_provider): - cls.forward() - cls.update_visualization_state(scene_data_provider) + if scene_data_provider is None: + scene_data_provider = cls.get_scene_data_provider() + if cls._backend_is_newton(scene_data_provider): + scene_data_provider.get_transforms(SceneDataFormat.Transform()) + else: + cls.update_visualization_state(scene_data_provider) return cls.get_state_0() @classmethod @@ -2936,6 +2750,7 @@ def _initialize_visualization_model(cls, cfg: NewtonBackendCfg, geometry: tuple[ NewtonManager._num_envs = cls.backend.model.num_envs shadow_entities, registry_groups = geometry NewtonManager._scene_data_mapping = None + NewtonManager._scene_data_version = None NewtonManager._shadow_deformable_entities = shadow_entities NewtonManager._scene_data_geometry_mapping = None NewtonManager._mapped_sim_particle_offsets = None @@ -2947,6 +2762,7 @@ def _initialize_visualization_model(cls, cfg: NewtonBackendCfg, geometry: tuple[ NewtonManager._sim_particle_q = None NewtonManager._deformable_registry = [] populate_shadow_deformable_registry(cls, registry_groups) + cls.update_visualization_state() NewtonManager._visualization_stop_callback = sim.physics_manager.register_callback( lambda _payload: NewtonManager.clear(), PhysicsEvent.STOP, @@ -2968,17 +2784,10 @@ def update_visualization_state(cls, scene_data_provider: SceneDataProvider | Non Newton sim backend: no-op — ``_state_0`` is the live, authoritative state already advanced by :meth:`step` / forward kinematics. - PhysX / OVPhysX sim backend: pull rigid-body transforms and deformable - nodal positions from the :class:`~isaaclab.scene_data.SceneDataProvider` - and write them into the shadow ``_state_0.body_q`` / ``particle_q`` so - Newton-native consumers (Newton renderer, Newton/Rerun/Viser visualizers, - OVRTX renderer, Newton GL video) see fresh poses and mesh points. - - Calls use ``allow_passthrough=False`` so identity mappings still copy into - the pre-bound shadow buffers. Passthrough would rebind the temporary - :class:`~isaaclab.scene_data.SceneDataFormat` fields away from - ``_state_0``, leaving OVRTX and other ``get_state()`` consumers on stale - rest-pose particle / body state. + PhysX / OVPhysX sim backend: bind shared SDP rigid-body transforms to + ``state_0.body_q`` without copying. Consumers must treat this array as + read-only. Deformable points still copy into the shadow ``particle_q`` + buffer, including simulation-to-visual mesh remapping. Invoked lazily from :meth:`get_state` so consumers do not need to coordinate the sync explicitly. @@ -2996,23 +2805,25 @@ def update_visualization_state(cls, scene_data_provider: SceneDataProvider | Non return if cls.backend.state_0.body_q is not None: - if cls._scene_data is None: - cls._scene_data = SceneDataFormat.Transform() - - # Invalidate stale mapping when the model's body count changed (e.g. tiled → viewport - # test within the same process where _model was rebuilt from a different stage). - if cls._scene_data_mapping is not None and cls._scene_data_mapping.shape[0] != cls.backend.model.body_count: - cls._scene_data_mapping = None - - if cls._scene_data_mapping is None: + if cls._scene_data_version is None: body_labels = list(cls.backend.model.body_label) body_paths = cls._resolve_scene_data_body_paths(body_labels, scene_data_provider.usd_stage) + if len(set(body_paths)) != cls.backend.model.body_count or not set(body_paths).issubset( + scene_data_provider.backend.transform_paths + ): + raise ValueError("Every Newton render body must have one unique SDP transform path.") cls._scene_data_mapping = scene_data_provider.create_mapping(body_paths) - cls._scene_data.transforms = cls.backend.state_0.body_q - scene_data_provider.get_transforms( - cls._scene_data, mapping=cls._scene_data_mapping, allow_passthrough=False - ) + transforms = SceneDataFormat.Transform() + if scene_data_provider.get_transforms( + transforms, mapping=cls._scene_data_mapping, count=cls.backend.model.body_count + ): + if cls.backend.state_0.body_q is not transforms.transforms: + cls.backend.state_0.body_q = transforms.transforms + cls._invalidate_sensor_graph() + if cls._scene_data_version != scene_data_provider.backend.transforms_version: + cls._mark_sensor_state_dirty() + cls._scene_data_version = scene_data_provider.backend.transforms_version if cls.backend.state_0.particle_q is not None and scene_data_provider.point_count > 0: if cls._scene_data_points is None: @@ -3058,7 +2869,7 @@ def update_visualization_state(cls, scene_data_provider: SceneDataProvider | Non allow_passthrough=False, ) - cls._mark_sensor_state_dirty() + cls._mark_sensor_state_dirty() @classmethod def _geometry_mapped_sim_offsets(cls, scene_data_provider: SceneDataProvider) -> set[int]: diff --git a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py index f07011905e7c..2b67a73b2175 100644 --- a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py +++ b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py @@ -483,12 +483,12 @@ def __init__(self, cfg: NewtonWarpRendererCfg): def initialize(self) -> None: """Post-physics setup: read the built Newton model and construct the sensor.""" - self._newton_model = NewtonManager.get_model() - if self._newton_model is None: + model = NewtonManager.get_model() + if model is None: raise RuntimeError("NewtonWarpRenderer requires a clone-built model before initialization.") self.newton_sensor = newton.sensors.SensorTiledCamera( - self._newton_model, + model, default_render_config=newton.sensors.SensorTiledCamera.RenderConfig( enable_textures=self.cfg.enable_textures, enable_shadows=self.cfg.enable_shadows, @@ -562,7 +562,7 @@ def create_render_data(self, spec: CameraRenderSpec) -> RenderData: ): if self._seg_mapper is None: clone_plan = SimulationContext.instance().get_clone_plan() - self._seg_mapper = NewtonSegmentationMapper(self._newton_model, self._stage, self.cfg, clone_plan) + self._seg_mapper = NewtonSegmentationMapper(self.newton_sensor.model, self._stage, self.cfg, clone_plan) if RenderBufferKind.SEMANTIC_SEGMENTATION in spec.cfg.data_types: self._seg_mapper.build_mapping( RenderBufferKind.SEMANTIC_SEGMENTATION, bool(self.cfg.colorize_semantic_segmentation) @@ -579,12 +579,9 @@ def set_outputs(self, render_data: RenderData, output_data: dict[str, ProxyArray """Store output buffers. See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.set_outputs`.""" render_data.set_outputs(output_data) - def update_transforms(self): - """Sync Newton scene state before rendering. - See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.update_transforms`.""" - sim = SimulationContext.instance() - sim.physics_manager.forward() - NewtonManager.update_visualization_state() + def update_transforms(self) -> None: + """No-op: the shared sensor pipeline refreshes transforms immediately before rendering.""" + pass def update_geometries(self) -> None: """No-op for Newton Warp - geometry is read directly from Newton state during render. @@ -631,7 +628,7 @@ def render(self, render_data: RenderData): if render_data.sensor_task_name is None: render_data.sensor_task_name = f"newton_warp_render:{id(render_data)}" - tri_indices = self._newton_model.tri_indices + tri_indices = self.newton_sensor.model.tri_indices # Warp mesh refits allocate graph nodes and are not supported inside a conditional graph body. graph_capturable = tri_indices is None or tri_indices.shape[0] == 0 NewtonManager._register_sensor_task( diff --git a/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py b/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py index 8f89fdda4088..2123fd766a64 100644 --- a/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py +++ b/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py @@ -18,15 +18,20 @@ import torch import warp as wp from isaaclab_newton.physics import NewtonCfg, NewtonManager, VBDSolverCfg, XPBDSolverCfg +from isaaclab_newton.renderers import NewtonWarpRendererCfg +from isaaclab_physx.renderers import IsaacRtxRendererCfg +from isaaclab_physx.renderers.fabric import FabricBackend, FabricBackendCfg from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg +from isaaclab_visualizers.kit import KitVisualizerCfg from pxr import Gf as UsdGf from pxr import UsdGeom from usdrt import Gf, Rt import isaaclab.sim as sim_utils -from isaaclab.assets import CableObjectCfg, RigidObjectCfg +from isaaclab.assets import AssetBaseCfg, CableObjectCfg, RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import CameraCfg from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.sim.spawners.materials import CableMaterialCfg from isaaclab.sim.spawners.shapes import CableCfg @@ -36,6 +41,14 @@ @configclass class _RenderSceneCfg(InteractiveSceneCfg): + camera = CameraCfg( + prim_path="{ENV_REGEX_NS}/Camera", + height=16, + width=16, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(), + renderer_cfg=IsaacRtxRendererCfg(), + ) cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( @@ -133,127 +146,6 @@ def _expected_cable_points_world(cable, env_id: int = 0) -> torch.Tensor: return torch.stack(points) -class _FakeAttribute: - def __init__(self, value_type, custom): - self.value_type = value_type - self.custom = custom - self.value = None - - def Set(self, value): - self.value = value - - -class _FakePrim: - def __init__(self, valid=True): - self.valid = valid - self.attributes = {} - self.applied_schemas = [] - self.created_world_matrix_attrs = 0 - self.set_world_xform_from_usd = 0 - - def IsValid(self): - return self.valid - - def CreateAttribute(self, name, value_type, custom=False): - self.attributes[name] = _FakeAttribute(value_type, custom) - return self.attributes[name] - - def GetAttribute(self, name): - return self.attributes[name] - - def AddAppliedSchema(self, schema): - self.applied_schemas.append(schema) - - -class _FakeStage: - def __init__(self, prims=None): - self.prims = prims or {} - self.defined_prims = [] - - def GetPrimAtPath(self, path): - return self.prims.get(path, _FakePrim(valid=False)) - - def DefinePrim(self, path, prim_type): - prim = _FakePrim() - self.prims[path] = prim - self.defined_prims.append((path, prim_type)) - return prim - - -class _FakeXformable: - def __init__(self, prim): - self.prim = prim - - def SetWorldXformFromUsd(self): - self.prim.set_world_xform_from_usd += 1 - - def CreateFabricHierarchyWorldMatrixAttr(self): - self.prim.created_world_matrix_attrs += 1 - - -class _FakeFabricHierarchy: - def __init__(self): - self.update_world_xforms_count = 0 - - def update_world_xforms(self): - self.update_world_xforms_count += 1 - - -class _FakeRt: - Xformable = _FakeXformable - - -class _FakeValueTypeNames: - UInt = "UInt" - - -class _FakeSdf: - ValueTypeNames = _FakeValueTypeNames - - -class _FakeUsdrt: - Rt = _FakeRt - Sdf = _FakeSdf - - -def test_initialize_fabric_body_prims_uses_existing_fabric_prim(): - prim = _FakePrim() - stage = _FakeStage({"/World/envs/env_0/Robot/base": prim}) - fabric_hierarchy = _FakeFabricHierarchy() - - NewtonManager._initialize_fabric_body_prims( - stage, fabric_hierarchy, _FakeUsdrt, [("/World/envs/env_0/Robot/base", 3)] - ) - - assert stage.defined_prims == [] - assert prim.set_world_xform_from_usd == 1 - assert prim.created_world_matrix_attrs == 0 - assert prim.GetAttribute("newton:index").value_type == "UInt" - assert prim.GetAttribute("newton:index").custom is True - assert prim.GetAttribute("newton:index").value == 3 - assert prim.applied_schemas == ["PhysicsRigidBodyAPI"] - assert fabric_hierarchy.update_world_xforms_count == 1 - - -def test_initialize_fabric_body_prims_creates_missing_body_as_xform(): - stage = _FakeStage() - fabric_hierarchy = _FakeFabricHierarchy() - - NewtonManager._initialize_fabric_body_prims( - stage, fabric_hierarchy, _FakeUsdrt, [("/World/envs/env_1/Robot/joints/forearm", 7)] - ) - - prim = stage.prims["/World/envs/env_1/Robot/joints/forearm"] - assert stage.defined_prims == [("/World/envs/env_1/Robot/joints/forearm", "Xform")] - assert prim.set_world_xform_from_usd == 0 - assert prim.created_world_matrix_attrs == 1 - assert prim.GetAttribute("newton:index").value_type == "UInt" - assert prim.GetAttribute("newton:index").custom is True - assert prim.GetAttribute("newton:index").value == 7 - assert prim.applied_schemas == ["PhysicsRigidBodyAPI"] - assert fabric_hierarchy.update_world_xforms_count == 1 - - @pytest.mark.isaacsim_ci @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") def test_root_pose_write_is_visible_on_next_render_without_step(): @@ -269,6 +161,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): device=device, gravity=(0.0, 0.0, 0.0), physics=NewtonCfg(solver_cfg=XPBDSolverCfg(), use_cuda_graph=False), + visualizer_cfgs=[KitVisualizerCfg(headless=True)], ) with build_simulation_context(sim_cfg=sim_cfg) as sim: @@ -278,7 +171,11 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): try: sim.reset() scene.reset() - sim.render() + _render(sim, scene) + + fabric = sim.get_or_create_backend(FabricBackendCfg(stage=sim.stage, device=sim.device)) + assert sim.visualizers[0]._fabric is scene["camera"]._renderer._fabric is fabric + assert sum(isinstance(resource, FabricBackend) for _, resource in sim._backend_registry) == 1 cube = scene["cube"] body_path = "/World/envs/env_0/Cube" @@ -291,8 +188,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): cube.write_root_link_pose_to_sim_index(root_pose=target_pose) physics_steps = sim.get_physics_step_count() - sim.render() - wp.synchronize_device(device) + _render(sim, scene) assert sim.get_physics_step_count() == physics_steps torch.testing.assert_close( @@ -309,13 +205,13 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): env_mask = wp.ones(1, dtype=wp.bool, device=device) pose_buffer = target_pose.clone() cube.write_root_link_pose_to_sim_mask(root_pose=pose_buffer, env_mask=env_mask) - sim.render() + _render(sim, scene) torch.cuda.synchronize(device) with wp.ScopedCapture(device=device) as capture: cube.write_root_link_pose_to_sim_mask(root_pose=pose_buffer, env_mask=env_mask) - sim.render() + _render(sim, scene) replay_targets = ( torch.tensor([2.5, 0.5, 1.25, 0.0, 0.0, 0.0, 1.0], device=device), @@ -328,8 +224,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): wp.synchronize_device(device) physics_steps = sim.get_physics_step_count() - sim.render() - wp.synchronize_device(device) + _render(sim, scene) assert sim.get_physics_step_count() == physics_steps torch.testing.assert_close( @@ -344,9 +239,13 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): @pytest.mark.isaacsim_ci @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") -def test_root_pose_sync_preserves_authored_scale(): +@pytest.mark.parametrize( + ("device", "renderer_cfg"), + [("cpu", IsaacRtxRendererCfg()), ("cuda:0", IsaacRtxRendererCfg()), ("cuda:0", NewtonWarpRendererCfg())], + ids=["rtx-cpu", "rtx-cuda", "newton-warp"], +) +def test_root_pose_sync_preserves_authored_scale(device, renderer_cfg): """Newton body pose synchronization must preserve authored USD scale in Kit/RTX.""" - device = "cuda:0" sim_cfg = SimulationCfg( device=device, gravity=(0.0, 0.0, 0.0), @@ -355,7 +254,9 @@ def test_root_pose_sync_preserves_authored_scale(): with build_simulation_context(sim_cfg=sim_cfg) as sim: sim._app_control_on_stop_handle = None - scene = InteractiveScene(_RenderSceneCfg(num_envs=1, env_spacing=2.0)) + scene_cfg = _RenderSceneCfg(num_envs=1, env_spacing=2.0) + scene_cfg.camera.renderer_cfg = renderer_cfg + scene = InteractiveScene(scene_cfg) sim.register_interactive_scene(scene) try: body_path = "/World/envs/env_0/Cube" @@ -365,8 +266,7 @@ def test_root_pose_sync_preserves_authored_scale(): sim.reset() scene.reset() - sim.render() - wp.synchronize_device(device) + _render(sim, scene) torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) @@ -376,8 +276,10 @@ def test_root_pose_sync_preserves_authored_scale(): device=device, ) scene["cube"].write_root_link_pose_to_sim_index(root_pose=target_pose) - sim.render() - wp.synchronize_device(device) + if isinstance(renderer_cfg, NewtonWarpRendererCfg): + assert not sim.visualizers + NewtonManager.sync_transforms_to_fabric() + _render(sim, scene) torch.testing.assert_close(_fabric_position(body_path), target_pose[0, :3].cpu(), rtol=0.0, atol=1.0e-4) torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) @@ -385,6 +287,40 @@ def test_root_pose_sync_preserves_authored_scale(): sim.register_interactive_scene(None) +@pytest.mark.isaacsim_ci +@pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") +def test_nested_bodies_keep_independent_world_poses(): + """A nested rigid body must not inherit its parent's independently published motion.""" + sim_cfg = SimulationCfg( + device="cuda:0", + gravity=(0.0, 0.0, 0.0), + physics=NewtonCfg(solver_cfg=XPBDSolverCfg(), use_cuda_graph=False), + ) + scene_cfg = _RenderSceneCfg(num_envs=1, env_spacing=2.0) + scene_cfg.child = scene_cfg.cube.replace(prim_path="{ENV_REGEX_NS}/Cube/Child") + scene_cfg.cube = AssetBaseCfg(prim_path=scene_cfg.cube.prim_path, spawn=scene_cfg.cube.spawn) + with build_simulation_context(sim_cfg=sim_cfg) as sim: + sim._app_control_on_stop_handle = None + scene = InteractiveScene(scene_cfg) + sim.register_interactive_scene(scene) + try: + sim.reset() + scene.reset() + _render(sim, scene) + paths = ["/World/envs/env_0/Cube", "/World/envs/env_0/Cube/Child"] + targets = torch.tensor([[1.5, -0.75, 2.0], [-0.25, 1.0, 3.0]], device=sim.device) + state = wp.to_torch(NewtonManager.get_state_0().body_q) + indices = [NewtonManager.get_model().body_label.index(path) for path in paths] + state[indices, :3] = targets + NewtonManager.invalidate_body_state() + _render(sim, scene) + for path, target in zip(paths, targets.cpu()): + _assert_position(_fabric_position(path), target) + assert not UsdGeom.Xformable(sim.stage.GetPrimAtPath(path)).GetResetXformStack() + finally: + sim.register_interactive_scene(None) + + @pytest.mark.isaacsim_ci @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") def test_periodic_cable_is_skipped_by_fabric_sync(): @@ -498,16 +434,17 @@ def _frame_scene(frame_path: str, translation, device: str = "cuda:0"): view = FrameView(frame_path, device=device) sim.reset() scene.reset() - _render(sim, device) + _render(sim, scene) yield sim, scene, view finally: sim.register_interactive_scene(None) -def _render(sim, device: str = "cuda:0") -> None: - """Render and wait for the write to land in Fabric.""" +def _render(sim, scene) -> None: + """Render through the camera's public data path and wait for Fabric writes.""" sim.render() - wp.synchronize_device(device) + scene["camera"].update(sim.get_rendering_dt(), force_recompute=True) + wp.synchronize_device(sim.device) def _world_pose(position: torch.Tensor) -> tuple[wp.array, wp.array]: @@ -535,25 +472,6 @@ def _assert_position(actual: torch.Tensor, expected: torch.Tensor) -> None: torch.testing.assert_close(actual, expected, rtol=0.0, atol=1.0e-4) -@pytest.mark.isaacsim_ci -@pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") -def test_frame_view_pose_write_reaches_fabric(): - """A world-attached FrameView pose write reaches the transform Kit/RTX renders.""" - device = "cuda:0" - frame_path = "/World/Frame" - spawn_position = torch.tensor([0.0, 0.0, 2.0]) - target_position = torch.tensor([1.0, -0.5, 8.0]) - - with _frame_scene(frame_path, tuple(spawn_position.tolist()), device) as (sim, _, view): - _assert_position(_fabric_position(frame_path), spawn_position) - - _write_frame_world_position(view, target_position.to(device)) - _render(sim, device) - - _assert_position(_reported_position(view), target_position) - _assert_position(_fabric_position(frame_path), target_position) - - @pytest.mark.isaacsim_ci @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") def test_frame_view_pose_write_reaches_fabric_when_the_scope_raises(): @@ -562,13 +480,14 @@ def test_frame_view_pose_write_reaches_fabric_when_the_scope_raises(): frame_path = "/World/Frame" target_position = torch.tensor([1.0, -0.5, 8.0]) - with _frame_scene(frame_path, (0.0, 0.0, 2.0), device) as (sim, _, view): + with _frame_scene(frame_path, (0.0, 0.0, 2.0), device) as (sim, scene, view): with pytest.raises(RuntimeError, match="boom"): # noqa: PT012 -- the raise is the scenario with view.xform_world_space_writer() as writer: writer.set_poses(*_world_pose(target_position.to(device))) raise RuntimeError("boom") - _render(sim, device) + _render(sim, scene) + _assert_position(_reported_position(view), target_position) _assert_position(_fabric_position(frame_path), target_position) @@ -584,13 +503,13 @@ def test_frame_view_pose_write_on_body_child_survives_body_motion(): body_start = torch.tensor([0.0, 0.0, 1.0]) written_position = body_start + torch.tensor([0.5, 0.0, 0.0]) _write_frame_world_position(view, written_position.to(device)) - _render(sim, device) + _render(sim, scene) _assert_position(_fabric_position(frame_path), written_position) body_pose = torch.tensor([[1.5, -0.75, 2.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device) scene["cube"].write_root_link_pose_to_sim_index(root_pose=body_pose) - _render(sim, device) + _render(sim, scene) expected = body_pose[0, :3].cpu() + (written_position - body_start) _assert_position(_reported_position(view), expected) @@ -609,15 +528,19 @@ def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): body_pose = torch.tensor([[1.5, -0.75, 2.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device) body_target = body_pose[0, :3].cpu() scene["cube"].write_root_link_pose_to_sim_index(root_pose=body_pose) - _render(sim, device) + _render(sim, scene) _assert_position(_fabric_position(body_path), body_target) + late_child = f"{frame_path}/LateChild" + offset = torch.tensor([0.0, 0.0, 0.25]) + sim_utils.create_prim(late_child, "Xform", translation=tuple(offset.tolist())) written_position = body_target + torch.tensor([0.5, 0.0, 0.0]) _write_frame_world_position(view, written_position.to(device)) - _render(sim, device) + _render(sim, scene) _assert_position(_fabric_position(body_path), body_target) _assert_position(_fabric_position(frame_path), written_position) + _assert_position(_fabric_position(late_child), written_position + offset) @pytest.mark.isaacsim_ci @@ -638,7 +561,7 @@ def test_frame_view_pose_write_after_unrendered_steps_reaches_fabric(): target_position = torch.tensor([0.0, 0.0, 1.5]) _write_frame_world_position(view, target_position.to(device)) - _render(sim, device) + _render(sim, scene) _assert_position(_reported_position(view), target_position) _assert_position(_fabric_position(frame_path), target_position) diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index 82b9ae9e1c4d..f2731a773e41 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -33,6 +33,7 @@ import textwrap from inspect import signature from types import SimpleNamespace +from unittest.mock import Mock import isaaclab_newton.physics.newton_manager as newton_manager_module import numpy as np @@ -66,12 +67,14 @@ XPBDSolverCfg, ) from isaaclab_newton.physics.mpm_manager import _make_solver_config +from isaaclab_newton.renderers.newton_warp_renderer import NewtonWarpRenderer from newton import JointTargetMode, JointType, ModelBuilder, ShapeFlags from newton.selection import ArticulationView from newton.solvers import SolverFeatherstone, SolverImplicitMPM, SolverKamino, SolverMuJoCo, SolverVBD, SolverXPBD from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.physics import PhysicsManager +from isaaclab.physics import PhysicsEvent, PhysicsManager +from isaaclab.scene_data import SceneDataFormat from isaaclab.sim import SimulationCfg, build_simulation_context # --------------------------------------------------------------------------- @@ -299,16 +302,17 @@ def test_refit_sensor_bvh_rejects_missing_sensor_state(monkeypatch): def test_sensor_task_builds_and_refits_bvhs_before_rendering(monkeypatch): - """Shape and particle BVHs are built and refit before a render task runs.""" + """One state refresh precedes BVH refits and rendering, including explicit transform updates.""" state = object() - status = {"state_refreshed": False, "shape_refit": False, "particle_refit": False, "rendered": False} + status = {"state_refreshes": 0, "shape_refit": False, "particle_refit": False, "rendered": False} class FakeModel: shape_count = 1 particle_count = 1 bvh_shapes = None bvh_particles = None + tri_indices = None def bvh_build_shapes(self, current_state): assert current_state is state @@ -329,7 +333,7 @@ def bvh_refit_particles(self, current_state): model = FakeModel() def render(): - assert status["state_refreshed"] + assert status["state_refreshes"] == 1 assert model.bvh_shapes is not None assert model.bvh_particles is not None assert status["shape_refit"] @@ -337,7 +341,7 @@ def render(): status["rendered"] = True def get_state(cls): - status["state_refreshed"] = True + status["state_refreshes"] += 1 return state monkeypatch.setattr(NewtonManager, "get_model", classmethod(lambda cls: model)) @@ -353,16 +357,21 @@ def get_state(cls): monkeypatch.setattr(NewtonManager, "_sensor_graph_capture_failed", False, raising=False) monkeypatch.setattr(PhysicsManager, "_cfg", SimpleNamespace(use_cuda_graph=False), raising=False) - NewtonManager._register_sensor_task("render", render) - NewtonManager._update_sensor_tasks("render") + renderer = object.__new__(NewtonWarpRenderer) + renderer.newton_sensor = SimpleNamespace(model=model) + monkeypatch.setattr(renderer, "_launch_render", lambda _data: render()) + renderer.update_transforms() + renderer.render(SimpleNamespace(sensor_task_name=None, ppisp_pipeline=None)) assert status["rendered"] -def test_non_graph_capturable_sensor_task_runs_eagerly(monkeypatch): - """Sensor tasks with allocation-backed work should not attempt CUDA graph capture.""" +def test_newton_warp_renderer_runs_triangle_mesh_refit_eagerly(monkeypatch): + """Allocation-backed triangle-mesh rendering runs without attempting CUDA graph capture.""" state = object() - model = SimpleNamespace(shape_count=0, particle_count=0, bvh_shapes=None, bvh_particles=None) + model = SimpleNamespace( + shape_count=0, particle_count=0, bvh_shapes=None, bvh_particles=None, tri_indices=SimpleNamespace(shape=(1, 3)) + ) calls: list[str] = [] monkeypatch.setattr(NewtonManager, "get_model", classmethod(lambda cls: model)) @@ -385,44 +394,12 @@ def test_non_graph_capturable_sensor_task_runs_eagerly(monkeypatch): classmethod(lambda cls: pytest.fail("Non-graph-capturable task attempted CUDA graph capture.")), ) - NewtonManager._register_sensor_task("render", lambda: calls.append("render"), graph_capturable=False) - NewtonManager._update_sensor_tasks("render") - - assert calls == ["render"] - assert NewtonManager._sensor_graph is None - assert NewtonManager._sensor_graph_capture_failed is False - - -@pytest.mark.parametrize( - ("triangle_count", "expected_graph_capturable"), - [ - pytest.param(None, True, id="no-triangle-array"), - pytest.param(0, True, id="empty-triangle-array"), - pytest.param(1, False, id="deformable-triangle-mesh"), - ], -) -def test_newton_warp_renderer_marks_triangle_mesh_refit_as_eager( - monkeypatch, triangle_count, expected_graph_capturable -): - """Deformable triangle-mesh rendering should opt out of conditional CUDA graph capture.""" - from isaaclab_newton.renderers.newton_warp_renderer import NewtonWarpRenderer - - registration: dict[str, object] = {} - - def register_task(cls, name, update_fn, *, graph_capturable=True): - registration.update(name=name, update_fn=update_fn, graph_capturable=graph_capturable) - - monkeypatch.setattr(NewtonManager, "_register_sensor_task", classmethod(register_task)) - monkeypatch.setattr(NewtonManager, "_update_sensor_tasks", classmethod(lambda cls, *names: None)) - - tri_indices = None if triangle_count is None else SimpleNamespace(shape=(triangle_count, 3)) renderer = object.__new__(NewtonWarpRenderer) - renderer._newton_model = SimpleNamespace(tri_indices=tri_indices) - render_data = SimpleNamespace(sensor_task_name=None, ppisp_pipeline=None) - - renderer.render(render_data) + renderer.newton_sensor = SimpleNamespace(model=model) + monkeypatch.setattr(renderer, "_launch_render", lambda _data: calls.append("render")) + renderer.render(SimpleNamespace(sensor_task_name=None, ppisp_pipeline=None)) - assert registration["graph_capturable"] is expected_graph_capturable + assert calls == ["render"] def test_sensor_bvh_shape_flags_are_fixed_before_builder_creation(monkeypatch): @@ -1298,7 +1275,7 @@ def test_fixed_root_pose_write_updates_solver(monkeypatch, asset_class, writer, def test_forward_consumes_existing_reset_masks(monkeypatch): - """The existing device masks are the complete input to masked FK and the solver reset hook.""" + """Authored-state masks are consumed once, without rerunning clean FK or solver reset.""" world_mask = wp.array([False, True], dtype=wp.bool, device="cpu") fk_mask = wp.array([True, False], dtype=wp.bool, device="cpu") observed: list[tuple[list[bool], list[bool]]] = [] @@ -1313,6 +1290,8 @@ def reset(self, state, world_mask=None, flags=0): monkeypatch.setattr(NewtonManager, "_world_reset_mask", world_mask, raising=False) monkeypatch.setattr(NewtonManager, "_fk_reset_mask", fk_mask, raising=False) + monkeypatch.setattr(NewtonManager, "_reconciliation_pending", True, raising=False) + monkeypatch.setattr(NewtonManager, "_transforms_may_change_on_graph_replay", False) monkeypatch.setattr(NewtonManager, "_eval_fk", record_fk, raising=False) monkeypatch.setattr(NewtonManager, "backend", SimpleNamespace(state_0=object())) monkeypatch.setattr(NewtonManager, "_solver", _RecordingSolver(), raising=False) @@ -1323,6 +1302,7 @@ def reset(self, state, world_mask=None, flags=0): raising=False, ) + NewtonManager.forward() NewtonManager.forward() assert observed == [([False, True], [True, False])] @@ -1342,6 +1322,7 @@ def reset(self, state, world_mask=None, flags=0): monkeypatch.setattr(NewtonManager, "_world_reset_mask", world_mask, raising=False) monkeypatch.setattr(NewtonManager, "_fk_reset_mask", fk_mask, raising=False) + monkeypatch.setattr(NewtonManager, "_reconciliation_pending", True, raising=False) monkeypatch.setattr(NewtonManager, "_eval_fk", lambda worlds, articulations: None, raising=False) monkeypatch.setattr(NewtonManager, "_solver", _RejectingSolver(), raising=False) monkeypatch.setattr( @@ -1447,7 +1428,7 @@ def count_actuator_resolutions(name_keys, names, *args, **kwargs): def test_initialize_solver_prepares_picking_before_graph_capture( monkeypatch, native_path_active, native_graphable, expected_events ): - """Viewer setup precedes initial capture, which only graphable native actuators defer.""" + """Initial and hard resets realize native layouts before consumers, then prepare picking and capture.""" events: list[str] = [] sim_cfg = SimulationCfg( dt=1.0 / 120.0, @@ -1457,6 +1438,19 @@ def test_initialize_solver_prepares_picking_before_graph_capture( with build_simulation_context(sim_cfg=sim_cfg) as sim: build_solver = NewtonMJWarpManager._build_solver + monkeypatch.setitem(sys.modules, "usdrt", Mock()) + monkeypatch.setattr(NewtonMJWarpManager, "_clone_physics_only", False) + monkeypatch.setattr(newton_manager_module, "get_current_stage", lambda **kwargs: Mock()) + for kind in ("body", "cable", "particle"): + monkeypatch.setattr( + NewtonManager, + f"_initialize_fabric_{kind}_prims", + staticmethod(lambda *args, kind=kind: events.append(kind)), + ) + + def on_physics_ready(_): + events.append("ready") + sim.get_scene_data_provider().get_transforms(SceneDataFormat.Transform()) def build_solver_with_actuator_mode(cls, model, solver_cfg): build_solver(model, solver_cfg) @@ -1478,10 +1472,16 @@ def build_solver_with_actuator_mode(cls, model, solver_cfg): "_capture_or_defer_graph", classmethod(lambda cls: events.append("capture")), ) + sim.physics_manager.register_callback( + on_physics_ready, + PhysicsEvent.PHYSICS_READY, + wrap_weak_ref=False, + ) sim.reset() + sim.reset() - assert events == expected_events + assert events == ["body", "cable", "ready", "particle", *expected_events] * 2 def test_abstract_build_solver_raises(): diff --git a/source/isaaclab_ov/changelog.d/sdp-transform-transport.rst b/source/isaaclab_ov/changelog.d/sdp-transform-transport.rst new file mode 100644 index 000000000000..6a36d439a88a --- /dev/null +++ b/source/isaaclab_ov/changelog.d/sdp-transform-transport.rst @@ -0,0 +1,10 @@ +Changed +^^^^^^^ + +* Published OVPhysX rigid poses directly into shared scene-data storage and invalidated cached transforms after + physics steps and manual pose writes. Binding failures were surfaced instead of publishing incomplete poses. +* Routed OVRTX rigid transforms through cached SDP matrix requests, preserving authored scales without a Newton + rigid-state intermediary. Existing renderer configurations remained valid; Newton-backed deformable, particle, + and cable geometry transport remained unchanged. +* Captured OVRTX authored scales from clone-plan prototypes and shared roots, including bodies outside the + default environment namespace. diff --git a/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py b/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py index 2df2dade3aee..6559048006e4 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation.py @@ -536,6 +536,8 @@ def write_root_link_pose_to_sim_index( self._root_view.set_attribute( TT.ROOT_POSE, self.data._root_link_pose_w.data.view(wp.float32), indices=sim_env_ids ) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_link_pose_to_sim_mask( self, @@ -575,6 +577,8 @@ def write_root_link_pose_to_sim_mask( if not skip_forward: self.data._reset_pose() self._root_view.set_attribute(TT.ROOT_POSE, self.data._root_link_pose_w.data.view(wp.float32), mask=env_mask_wp) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_com_pose_to_sim_index( self, @@ -618,6 +622,8 @@ def write_root_com_pose_to_sim_index( self._root_view.set_attribute( TT.ROOT_POSE, self.data._root_link_pose_w.data.view(wp.float32), indices=sim_env_ids ) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_com_pose_to_sim_mask( self, @@ -658,6 +664,8 @@ def write_root_com_pose_to_sim_mask( if not skip_forward: self.data._reset_pose(from_link=False) self._root_view.set_attribute(TT.ROOT_POSE, self.data._root_link_pose_w.data.view(wp.float32), mask=env_mask_wp) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_velocity_to_sim_index( self, @@ -973,6 +981,8 @@ def write_joint_state_to_sim_index( self._data._reset_pose() self._data._reset_velocity() self._root_view.set_attribute(TT.DOF_POSITION, joint_pos_backend, indices=sim_env_ids) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 self._root_view.set_attribute(TT.DOF_VELOCITY, joint_vel_backend, indices=sim_env_ids) def write_joint_position_to_sim_index( @@ -1023,6 +1033,8 @@ def write_joint_position_to_sim_index( self._data._reset_pose() self._data._reset_velocity() self._root_view.set_attribute(TT.DOF_POSITION, joint_pos_backend, indices=sim_env_ids) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_joint_position_to_sim_mask( self, @@ -1074,6 +1086,8 @@ def write_joint_position_to_sim_mask( self._data._reset_pose() self._data._reset_velocity() self._root_view.set_attribute(TT.DOF_POSITION, joint_pos_backend, mask=env_mask_wp) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_joint_velocity_to_sim_index( self, @@ -1244,6 +1258,8 @@ def write_joint_state_to_sim_mask( self._data._reset_pose() self._data._reset_velocity() self._root_view.set_attribute(TT.DOF_POSITION, joint_pos_backend, mask=env_mask_wp) + OvPhysxManager._kinematics_dirty = True + OvPhysxManager._scene_data_backend.transforms_version += 1 self._root_view.set_attribute(TT.DOF_VELOCITY, joint_vel_backend, mask=env_mask_wp) """ diff --git a/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation_data.py b/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation_data.py index ae87c4fdefab..4dea95271e4c 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation_data.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/articulation/articulation_data.py @@ -200,6 +200,7 @@ def _ensure_fk_fresh(self) -> None: physx_instance = OvPhysxManager.get_physx_instance() if physx_instance is not None: physx_instance.update_articulations_kinematic() + OvPhysxManager._kinematics_dirty = False self._fk_timestamp = self._sim_timestamp def _reset_pose(self, from_link: bool = True) -> None: diff --git a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py index 262c2f57c214..468ec45722f3 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object/rigid_object.py @@ -375,6 +375,7 @@ def write_root_link_pose_to_sim_index( self._root_view.set_attribute( TT.RIGID_BODY_POSE, self.data._root_link_pose_w.data.view(wp.float32), indices=sim_env_ids ) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_link_pose_to_sim_mask( self, @@ -415,6 +416,7 @@ def write_root_link_pose_to_sim_mask( self._root_view.set_attribute( TT.RIGID_BODY_POSE, self.data._root_link_pose_w.data.view(wp.float32), mask=env_mask_wp ) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_com_pose_to_sim_index( self, @@ -457,6 +459,7 @@ def write_root_com_pose_to_sim_index( self._root_view.set_attribute( TT.RIGID_BODY_POSE, self.data._root_link_pose_w.data.view(wp.float32), indices=sim_env_ids ) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_com_pose_to_sim_mask( self, @@ -498,6 +501,7 @@ def write_root_com_pose_to_sim_mask( self._root_view.set_attribute( TT.RIGID_BODY_POSE, self.data._root_link_pose_w.data.view(wp.float32), mask=env_mask_wp ) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_root_com_velocity_to_sim_index( self, diff --git a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py index e57ffedfc181..8b4d34d43bf6 100644 --- a/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_ov/isaaclab_ov/assets/rigid_object_collection/rigid_object_collection.py @@ -418,6 +418,7 @@ def write_body_link_pose_to_sim_index( self.data._reset_pose() # set into simulation self._binding_write(TT.LINK_POSE, self.data._body_link_pose_w.data, env_ids=env_ids) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_body_link_pose_to_sim_mask( self, @@ -469,6 +470,7 @@ def write_body_link_pose_to_sim_mask( self.data._reset_pose() # set into simulation self._binding_write(TT.LINK_POSE, self.data._body_link_pose_w.data, env_ids=env_ids) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_body_com_pose_to_sim_index( self, @@ -515,6 +517,7 @@ def write_body_com_pose_to_sim_index( self.data._reset_pose(from_link=False) # set into simulation (OVPhysX only exposes the link frame) self._binding_write(TT.LINK_POSE, self.data._body_link_pose_w.data, env_ids=env_ids) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_body_com_pose_to_sim_mask( self, @@ -569,6 +572,7 @@ def write_body_com_pose_to_sim_mask( self.data._reset_pose(from_link=False) # set into simulation (OVPhysX only exposes the link frame) self._binding_write(TT.LINK_POSE, self.data._body_link_pose_w.data, env_ids=env_ids) + OvPhysxManager._scene_data_backend.transforms_version += 1 def write_body_com_velocity_to_sim_index( self, diff --git a/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py b/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py index a412a0a313ac..661272c0da6b 100644 --- a/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py +++ b/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py @@ -90,39 +90,15 @@ def _prepare_default_cache_dir(cache_dir: str) -> str: class OvPhysxSceneDataBackend(SceneDataBackend): """Scene-data backend for the OVPhysX physics manager. - Mirrors the contract of ``PhysxSceneDataBackend`` but adapts to the - ovphysx wheel's one-pattern-per-binding API: each distinct env-wildcard - rigid-body prim path produces its own ``TT.RIGID_BODY_POSE`` binding. - :attr:`transforms` reads each binding into its pre-allocated float32 - staging buffer and concatenates them into a single ``wp.transformf`` - array. - - The merged-buffer + staging-buffer separation is required because the - wheel's ``TensorBinding.read(dst)`` writes into ``dst`` only when - ``dst.shape == binding.shape``, so we cannot read directly into a slice - of the merged buffer. - - Unlike PhysX -- which receives a live :class:`omni.physics.tensors.SimulationView` - via a ``simulation_view`` property setter and discovers prims lazily -- - OVPhysX wires bindings through an explicit :meth:`setup` call that - takes the live ``ovphysx.PhysX`` handle and the USD stage. The wheel - exposes a ``physx + stage`` pair rather than a single ``SimulationView``, - so a property setter would have to either bundle the two or fire on the - second assignment; the explicit call keeps the lifecycle obvious. + Each rigid-body binding reads directly into its portion of one native pose + buffer. Pointer aliases preserve the binding shape without staging or merging. """ def __init__(self): - self._physx = None - # Each entry: ``{"pattern": str, "pose": TensorBinding, - # "pose_buf": wp.array (float32, (N, 7)), - # "pose_buf_transformf": wp.array (transformf, (N,)), - # "row_offset": int, "row_count": int}``. - # The ``pose_buf_transformf`` view aliases ``pose_buf`` via zero-copy - # ``wp.array(ptr=...)``; cached at setup time so per-step reads in - # :attr:`transforms` don't churn Python allocations. - self._rigid_bindings: list[dict[str, Any]] = [] - self._merged_transforms: wp.array | None = None - self._scene_data = SceneDataFormat.Transform() + self._rigid_bindings: list[tuple[OvPhysxView, wp.array]] = [] + self._transforms = SceneDataFormat.Transform() + self.transforms_version = 0 + self._poses_version = -1 self._points_data = SceneDataFormat.Points() self._deformable_bindings: list[dict[str, Any]] = [] self._geometry_paths: list[str] = [] @@ -131,16 +107,14 @@ def __init__(self): @property def transform_count(self) -> int: - """Sum of per-binding row counts.""" - return sum(int(entry["row_count"]) for entry in self._rigid_bindings) + """Number of poses in the native publication.""" + poses = self._transforms.transforms + return 0 if poses is None else len(poses) @property def transform_paths(self) -> list[str]: """Concatenated ``prim_paths`` across all bindings, in registration order.""" - paths: list[str] = [] - for entry in self._rigid_bindings: - paths.extend(list(entry["pose"].prim_paths)) - return paths + return [path for view, _ in self._rigid_bindings for path in view.prim_paths] def setup(self, physx, stage, device: str) -> None: """Discover RigidBodyAPI prims, dedup by env-wildcard form, create one binding per pattern. @@ -148,13 +122,13 @@ def setup(self, physx, stage, device: str) -> None: Args: physx: Live ``ovphysx.PhysX`` instance (the wheel handle). stage: USD stage to traverse for RigidBodyAPI prims. - device: Warp device string used to allocate the staging and merged buffers. + device: Warp device string used to allocate the published buffers. """ from isaaclab_ov import tensor_types as TT # local: keep heavy ovphysx out of module load - self._physx = physx self._rigid_bindings = [] - self._merged_transforms = None + self._transforms.transforms = None + self.transforms_version += 1 self._deformable_bindings = [] self._geometry_paths = [] self._geometry_counts = [] @@ -169,49 +143,30 @@ def setup(self, physx, stage, device: str) -> None: if prim.HasAPI(UsdPhysics.RigidBodyAPI): patterns.add(re.sub(r"/World/envs/env_\d+", "/World/envs/env_*", prim.GetPath().pathString)) - # Rigid discovery may be empty for deformable-only scenes; still set up - # deformable nodal bindings so SceneData geometry export stays available. - if patterns: - # One pose binding per distinct pattern. - total_count = 0 - for pattern in sorted(patterns): - try: - view = OvPhysxView(physx, pattern=pattern, device=device) - pose_binding = view.binding_for(TT.RIGID_BODY_POSE) - except Exception as exc: - logger.warning("Failed to create RIGID_BODY_POSE binding for %s: %s", pattern, exc) - continue - row_count = int(pose_binding.shape[0]) - if row_count == 0: - logger.debug("Pattern %s matched 0 rigid bodies; skipping.", pattern) - view.close() - continue - pose_buf = wp.zeros(pose_binding.shape, dtype=wp.float32, device=device) - # Zero-copy reinterpret of the (N, 7) float32 staging buffer as (N,) wp.transformf. - # Same pointer + layout; transformf is 7 float32s (pos.xyz + quat.xyzw). Cached - # so per-step ``transforms`` reads don't reallocate the view object. - pose_buf_transformf = wp.array( - ptr=pose_buf.ptr, - shape=(row_count,), + views = [] + for pattern in sorted(patterns): + view = OvPhysxView(physx, pattern=pattern, device=device) + view.binding_for(TT.RIGID_BODY_POSE) + if view.count == 0: + logger.debug("Pattern %s matched 0 rigid bodies; skipping.", pattern) + view.close() + continue + views.append(view) + + if views: + poses = wp.empty(sum(view.count for view in views), dtype=wp.transformf, device=device) + self._transforms.transforms = poses + offset = 0 + for view in views: + buffer = wp.array( + ptr=poses.ptr + offset * wp.types.type_size_in_bytes(wp.transformf), + shape=(view.count,), dtype=wp.transformf, - device=str(pose_buf.device), + device=device, copy=False, ) - self._rigid_bindings.append( - { - "pattern": pattern, - "view": view, - "pose": pose_binding, - "pose_buf": pose_buf, - "pose_buf_transformf": pose_buf_transformf, - "row_offset": total_count, - "row_count": row_count, - } - ) - total_count += row_count - - if total_count > 0: - self._merged_transforms = wp.zeros((total_count,), dtype=wp.transformf, device=device) + self._rigid_bindings.append((view, buffer)) + offset += view.count self._setup_deformable_bindings(physx, stage, device) @@ -355,40 +310,13 @@ def geometry_counts(self) -> list[int]: @property def transforms(self) -> SceneDataFormat.Transform: - """Read all bindings into the merged buffer; return as ``SceneDataFormat.Transform``. - - Each binding's float32 ``(N, 7)`` read buffer is reinterpreted as ``(N,)`` of - ``wp.transformf`` (zero-copy via ``wp.array(ptr=..., dtype=wp.transformf)``, - cached on the entry at setup time) and copied into the merged buffer at the - binding's ``row_offset``. - - Returns: - ``SceneDataFormat.Transform`` whose ``transforms`` field is a - ``wp.array(dtype=wp.transformf)`` of length :attr:`transform_count`. - Each ``wp.transformf`` row carries position [m] followed by - quaternion (xyzw, unit). ``transforms`` is ``None`` when no - bindings are wired. - """ - if self._merged_transforms is None or not self._rigid_bindings: - self._scene_data.transforms = self._merged_transforms - return self._scene_data - - for entry in self._rigid_bindings: - try: - entry["view"].read_into("rigid_body_pose", entry["pose_buf"]) - except Exception as exc: - logger.warning("RIGID_BODY_POSE read failed for %s: %s", entry["pattern"], exc) - continue - wp.copy( - self._merged_transforms, - entry["pose_buf_transformf"], - dest_offset=int(entry["row_offset"]), - src_offset=0, - count=int(entry["row_count"]), - ) - - self._scene_data.transforms = self._merged_transforms - return self._scene_data + """Publish native rigid-body poses [m, xyzw].""" + if self._poses_version != self.transforms_version: + OvPhysxManager.pre_render() + for view, buffer in self._rigid_bindings: + view.read_into("rigid_body_pose", buffer) + self._poses_version = self.transforms_version + return self._transforms class OvPhysxBackend: @@ -493,6 +421,7 @@ class OvPhysxManager(PhysicsManager): _pending_clones: ClassVar[list[tuple[str, list[str], list[CloneTransform]]]] = [] _atexit_registered: ClassVar[bool] = False _scene_data_backend: ClassVar[OvPhysxSceneDataBackend | None] = None + _kinematics_dirty: ClassVar[bool] = False # Gravity currently applied to the running scene [m/s^2]. Seeded from ``SimulationCfg.gravity`` # in :meth:`initialize` and refreshed by :meth:`set_gravity`. ``cfg.gravity`` stays the nominal # value that randomization terms resample from, so live updates must not be written back to it. @@ -624,6 +553,7 @@ def initialize(cls, sim_context: SimulationContext) -> None: # and the USD stage are live. Matches PhysX's pattern of constructing # the backend during ``initialize()``. cls._scene_data_backend = OvPhysxSceneDataBackend() + cls._kinematics_dirty = False @classmethod def reset(cls, soft: bool = False) -> None: @@ -645,11 +575,23 @@ def reset(cls, soft: bool = False) -> None: cls.dispatch_event(PhysicsEvent.STOP, payload={}) cls._warmup_and_load() cls.dispatch_event(PhysicsEvent.PHYSICS_READY, payload={}) + cls._kinematics_dirty = True + cls._scene_data_backend.transforms_version += 1 @classmethod def forward(cls) -> None: - """No-op -- ovphysx does not have a fabric/rendering pipeline.""" - pass + """Evaluate and publish state changes made without stepping physics.""" + if cls.backend is not None and cls.backend.physx is not None: + cls.backend.physx.update_articulations_kinematic() + cls._kinematics_dirty = False + cls._scene_data_backend.transforms_version += 1 + + @classmethod + def pre_render(cls) -> None: + """Finish native kinematics before SDP publishes manually written joint poses.""" + if cls._kinematics_dirty and cls.backend is not None and cls.backend.physx is not None: + cls.backend.physx.update_articulations_kinematic() + cls._kinematics_dirty = False @classmethod def step(cls) -> None: @@ -659,6 +601,8 @@ def step(cls) -> None: dt = cls.get_physics_dt() cls.backend.physx.step_sync(dt=dt) cls.backend.physx.update_articulations_kinematic() + cls._kinematics_dirty = False + cls._scene_data_backend.transforms_version += 1 PhysicsManager._sim_time += dt @staticmethod @@ -694,6 +638,7 @@ def close(cls) -> None: # belong to the runtime instance just released. The next # SimulationContext re-creates it in initialize(). cls._scene_data_backend = None + cls._kinematics_dirty = False cls._next_control_ordinal = 2 @classmethod diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py index 426933899eb5..2fa6b26c3b98 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py @@ -69,6 +69,7 @@ from isaaclab.cloner import ClonePlan from isaaclab.cloner import query as clone_query from isaaclab.renderers import BaseRenderer, RenderBufferKind, RenderBufferSpec +from isaaclab.scene_data import SceneDataFormat from isaaclab.sim import SimulationContext from isaaclab.utils.warp.warp_math import convert_camera_frame_orientation_convention_wp @@ -86,7 +87,6 @@ create_camera_transforms_kernel, extract_all_tiles_kernel, generate_random_colors_from_ids_kernel, - sync_newton_transforms_kernel, ) from isaaclab_ov.renderers.ovrtx_shader_cache import redirect_shader_cache from isaaclab_ov.renderers.ovrtx_usd import ( @@ -388,7 +388,8 @@ def __init__(self, cfg: OVRTXRendererCfg): # Shared by both paths. The legacy-only binding handles that pair with these live in # _init_fields_legacy instead; the ovstage path drives the same offsets and counts # through its stage queries. - self._object_newton_indices: wp.array | None = None + self._sdp = SimulationContext.instance().get_scene_data_provider() + self._transform_version = -1 self._object_scales: wp.array | None = None self._object_scales_by_path: dict[str, tuple[float, float, float]] = {} self._deformable_particle_offsets: list[int] = [] @@ -502,10 +503,10 @@ def _clone_targets_env_roots(self) -> bool: return any(destination.format(0) == "/World/envs/env_0" for destination in self._clone_plan.destinations) def _capture_object_scales(self, stage: Any, plan: ClonePlan) -> None: - """Record composed world scales of scaled environment prims before the stage is exported. + """Record composed world scales beneath the plan's prototypes and shared roots before export. - The per-frame object transform write rebuilds each body's matrix from a Newton - ``transformf``, which carries only translation and rotation, so any scale authored on the + The per-frame object transform write rebuilds each body's matrix from an SDP + pose, which carries only translation and rotation, so any scale authored on the USD prim is lost once that write lands. Capturing the composed scale here, while the full stage is still live, lets :meth:`_create_object_scale_array` fold it back in. @@ -521,18 +522,15 @@ def _capture_object_scales(self, stage: Any, plan: ClonePlan) -> None: from pxr import Gf, Usd, UsdGeom - envs_prim = stage.GetPrimAtPath("/World/envs") - if not envs_prim.IsValid(): - return - xform_cache = UsdGeom.XformCache() - for prim in Usd.PrimRange(envs_prim): - if not prim.IsA(UsdGeom.Xformable): - continue - scale = Gf.Transform(xform_cache.GetLocalToWorldTransform(prim)).GetScale() - scale = (float(scale[0]), float(scale[1]), float(scale[2])) - if not all(math.isclose(axis, 1.0, rel_tol=1e-6, abs_tol=1e-6) for axis in scale): - self._object_scales_by_path[str(prim.GetPath())] = scale + for root in (*plan.sources, *plan.global_paths): + for prim in Usd.PrimRange(stage.GetPrimAtPath(root)): + if not prim.IsA(UsdGeom.Xformable): + continue + scale = Gf.Transform(xform_cache.GetLocalToWorldTransform(prim)).GetScale() + scale = (float(scale[0]), float(scale[1]), float(scale[2])) + if not all(math.isclose(axis, 1.0, rel_tol=1e-6, abs_tol=1e-6) for axis in scale): + self._object_scales_by_path[str(prim.GetPath())] = scale # OVRTX creates non-source rows after this stage is exported, so those destination prims # cannot be traversed above. Clone queries retain the plan's nearest-owner semantics. @@ -543,10 +541,10 @@ def _capture_object_scales(self, stage: Any, plan: ClonePlan) -> None: self._object_scales_by_path.setdefault(clone_path, scale) def _create_object_scale_array(self, object_paths: list[str]) -> wp.array: - """Build the device scale array aligned with the Newton body binding order. + """Build the device scale array aligned with the published body binding order. Args: - object_paths: Bound body prim paths, ordered to match the Newton index array. + object_paths: Bound body prim paths, ordered to match the SDP publication. Returns: Per-body scale factors, shape ``[len(object_paths)]``, unit where no scale was authored. @@ -557,14 +555,10 @@ def _create_object_scale_array(self, object_paths: list[str]) -> wp.array: def _init_fields_legacy(self) -> None: """Initialize the legacy-path instance fields. - Counterpart to :meth:`_init_fields_ovstage`. Only fields the ovstage path never touches live - here: the ``bind_attribute``/``bind_array_attribute`` handles and the caller-owned object - transform buffer. State shared by both paths (``_object_newton_indices``, the particle - offset/count lists) stays in :meth:`__init__`. + Only binding handles live here; geometry offsets and counts are shared with ovstage. """ self._camera_xform_binding = None self._object_xform_binding = None - self._object_transform_buffer: wp.array | None = None self._deformable_points_binding = None self._particle_points_binding = None self._particle_workaround_applied = False @@ -715,37 +709,9 @@ def _update_scene_partitions_after_clone(self, num_envs: int): logger.info("Written omni:scenePartition to %d cameras", num_envs) def _setup_xform_bindings_legacy(self): - """Setup OVRTX bindings for scene objects to sync with Newton physics.""" - try: - from isaaclab_newton.physics import NewtonManager - except ImportError: - logger.debug("NewtonManager not available, skipping object bindings") - return - - if SimulationContext.instance() is None: - logger.info("No active simulation context, will not set up ovrtx object bindings for newton") - return - - newton_model = NewtonManager.get_model() - if newton_model is None: - logger.debug("Newton model not available, skipping object bindings") - return - - all_body_paths = getattr(newton_model, "body_label", None) - if all_body_paths is None: - logger.info("Newton model has no body_label, skipping object bindings") - return - - object_paths = [] - newton_indices = [] - camera_rel_path = self._camera_prim_path.removeprefix("/World/envs/env_0/") - for idx, path in enumerate(all_body_paths): - if "/World/envs/" in path and camera_rel_path not in path and "GroundPlane" not in path: - object_paths.append(path) - newton_indices.append(idx) - - if len(object_paths) == 0: - logger.info("No dynamic objects found for binding") + """Bind the body paths published through SDP.""" + object_paths = self._sdp.backend.transform_paths + if not object_paths: return self._object_xform_binding = self.backend.renderer.bind_attribute( @@ -764,9 +730,7 @@ def _setup_xform_bindings_legacy(self): if self._object_xform_binding is None: raise RuntimeError("Failed to create OVRTX object bindings") - self._object_newton_indices = wp.array(newton_indices, dtype=wp.int32, device=self._device) self._object_scales = self._create_object_scale_array(object_paths) - self._object_transform_buffer = wp.zeros(len(newton_indices), dtype=wp.mat44d, device=self._device) def _setup_deformable_bindings_legacy(self, num_envs: int): """Setup OVRTX bindings for Newton deformable bodies. @@ -1113,41 +1077,23 @@ def set_outputs(self, render_data: OVRTXCameraRenderData, output_data: dict[str, ) def _update_transforms_legacy(self) -> None: - """Sync transforms to OVRTX.""" - if ( - self._object_xform_binding is None - or self._object_newton_indices is None - or self._object_scales is None - or self._object_transform_buffer is None - ): + """Write SDP's requested matrix layout without another conversion.""" + if self._object_xform_binding is None: return - - # If self._object_newton_indices is not None, then Newton's the current physics backend - - from isaaclab_newton.physics import NewtonManager - - newton_state = NewtonManager.get_state() - if newton_state is None: - raise RuntimeError("Newton state should not be None") - - body_q = getattr(newton_state, "body_q", None) - if body_q is None: + transforms = SceneDataFormat.TransposedMatrix44d() + if not self._sdp.get_transforms(transforms, scales=self._object_scales): + return + if self._transform_version == self._sdp.backend.transforms_version: return - - wp.launch( - kernel=sync_newton_transforms_kernel, - dim=len(self._object_newton_indices), - inputs=[self._object_transform_buffer, self._object_newton_indices, body_q, self._object_scales], - device=self._device, - ) # Blocking ``write()`` so the buffer stays valid until OVRTX finishes reading it. # ``DataAccess.ASYNC`` + the Warp CUDA stream let OVRTX read in place and wait # on-GPU for the kernel; ``SYNC`` is rejected for GPU buffers. self._object_xform_binding.write( - self._object_transform_buffer, + transforms.matrices, data_access=DataAccess.ASYNC, cuda_stream=self._warp_device.stream.cuda_stream, ) + self._transform_version = self._sdp.backend.transforms_version def _update_geometries_legacy(self) -> None: """Sync geometries to OVRTX.""" @@ -1713,7 +1659,6 @@ def _safe_unbind(binding, name: str) -> None: self._camera_xform_binding = None _safe_unbind(self._object_xform_binding, "object transforms") self._object_xform_binding = None - self._object_transform_buffer = None _safe_unbind(self._deformable_points_binding, "deformable points") self._deformable_points_binding = None _safe_unbind(self._particle_points_binding, "particle points") @@ -2124,37 +2069,9 @@ def _update_scene_partitions_after_clone_ovstage(self, num_envs: int): logger.info("Written omni:scenePartition to %d cameras", num_envs) def _setup_xform_bindings_ovstage(self) -> None: - """Setup OVRTX bindings for scene objects to sync with Newton physics (ovstage path).""" - try: - from isaaclab_newton.physics import NewtonManager - except ImportError: - logger.debug("NewtonManager not available, skipping object bindings") - return - - if SimulationContext.instance() is None: - logger.info("No active simulation context, will not set up ovrtx object bindings for newton") - return - - newton_model = NewtonManager.get_model() - if newton_model is None: - logger.debug("Newton model not available, skipping object bindings") - return - - all_body_paths = getattr(newton_model, "body_label", None) - if all_body_paths is None: - logger.info("Newton model has no body_label, skipping object bindings") - return - - object_paths = [] - newton_indices = [] - camera_rel_path = self._camera_prim_path.removeprefix("/World/envs/env_0/") - for idx, path in enumerate(all_body_paths): - if "/World/envs/" in path and camera_rel_path not in path and "GroundPlane" not in path: - object_paths.append(path) - newton_indices.append(idx) - - if len(object_paths) == 0: - logger.info("No dynamic objects found for binding") + """Bind the body paths published through SDP.""" + object_paths = self._sdp.backend.transform_paths + if not object_paths: return self._object_paths_list = self.backend.paths.create_path_list_from_strings(object_paths) @@ -2171,7 +2088,6 @@ def _setup_xform_bindings_ovstage(self) -> None: if self._object_xform_query is None: raise RuntimeError("Failed to create OVRTX object bindings") - self._object_newton_indices = wp.array(newton_indices, dtype=wp.int32, device=self._device) self._object_scales = self._create_object_scale_array(object_paths) def _setup_deformable_bindings_ovstage(self, num_envs: int) -> None: @@ -2363,45 +2279,25 @@ def _setup_particle_bindings_ovstage(self) -> None: raise RuntimeError("Failed to create OVRTX particle point bindings") def _update_transforms_ovstage(self) -> None: - if self._object_xform_query is None or self._object_newton_indices is None or self._object_scales is None: + """Write SDP's matrix layout through the active ovstage ordinal.""" + if self._object_xform_query is None: return - - # If self._object_newton_indices is not None, then Newton's the current physics backend - - from isaaclab_newton.physics import NewtonManager - - newton_state = NewtonManager.get_state() - if newton_state is None: - raise RuntimeError("Newton state should not be None") - - body_q = getattr(newton_state, "body_q", None) - if body_q is None: + transforms = SceneDataFormat.TransposedMatrix44d() + if not self._sdp.get_transforms(transforms, scales=self._object_scales): return - - num_objects = len(self._object_newton_indices) - object_transforms = wp.empty(num_objects, dtype=wp.mat44d, device=self._device) - wp.launch( - kernel=sync_newton_transforms_kernel, - dim=num_objects, - inputs=[object_transforms, self._object_newton_indices, body_q, self._object_scales], - device=self._device, - ) - # The tensor is handed over zero-copy, so ovstage reads ``object_transforms`` in place and - # must not do so until the kernel above has landed. Passing the producing Warp stream as - # ``cuda_stream`` gives producer ordering: ovstage drains the work already queued on that - # stream before it touches the tensor. That replaces the device-wide - # ``wp.synchronize_device()`` with stream-scoped ordering and removes the host copy; it is - # not a nonblocking handoff, and the ``.wait()`` below can still block the calling thread. - # A GPU-side wait would need the event-based API instead. + if self._transform_version == self._sdp.backend.transforms_version: + return + # Stream-ordered zero-copy handoff; wait until OVStage has consumed the shared buffer. self.backend.stage.write_attribute( self._object_xform_query, "omni:xform", ordinal=self._current_ordinal, - tensors=xform_tensor_from_warp(object_transforms), + tensors=xform_tensor_from_warp(transforms.matrices), is_array=False, semantic=ovstage.AttributeSemantic.MATRIX, cuda_stream=self._warp_device.stream.cuda_stream, ).wait() + self._transform_version = self._sdp.backend.transforms_version def _update_geometries_ovstage(self) -> None: if self._deformable_points_query is not None or self._particle_points_query is not None: @@ -2611,7 +2507,6 @@ def _safe_destroy_path_list(path_list, name: str) -> None: _safe_destroy_path_list(self._cable_paths_list, "cable paths") self._cable_paths_list = None - self._object_newton_indices = None self._object_scales = None self._object_scales_by_path = {} # Descriptors alias ``_cable_points``; drop them before the buffer so no cached diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py index 9d83de2e4757..d5e598ebb5ef 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py @@ -175,33 +175,6 @@ def generate_random_colors_from_ids_kernel( output_colors[i, j, k] = random_color_from_id_wp(input_ids[i, j, k]) -@wp.kernel -def sync_newton_transforms_kernel( - ovrtx_transforms: wp.array(dtype=wp.mat44d), # type: ignore - newton_body_indices: wp.array(dtype=wp.int32), # type: ignore - newton_body_q: wp.array(dtype=wp.transformf), # type: ignore - object_scales: wp.array(dtype=wp.vec3f), # type: ignore -): - """Sync Newton physics body transforms to OVRTX 4x4 column-major matrices. - - A Newton ``transformf`` holds only translation and rotation, so the authored USD scale is - reapplied here to keep it from being overwritten with unit scale. - """ - i = wp.tid() - body_idx = newton_body_indices[i] - transform = newton_body_q[body_idx] - scale = object_scales[i] - ovrtx_transforms[i] = wp.mat44d( - wp.transpose( - wp.transform_compose( - wp.transform_get_translation(transform), - wp.transform_get_rotation(transform), - scale, - ) - ) - ) - - @wp.func def _cable_capsule_endpoint_world( shape_id: int, diff --git a/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py b/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py index aee9f584ac93..1787f108cb13 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py @@ -23,12 +23,14 @@ @pytest.fixture(autouse=True) def _native_backend(monkeypatch): - from isaaclab_ov.physics.ovphysx_manager import OvPhysxBackend, OvPhysxManager + from isaaclab_ov.physics.ovphysx_manager import OvPhysxBackend, OvPhysxManager, OvPhysxSceneDataBackend backend = OvPhysxBackend.__new__(OvPhysxBackend) backend.physx = None backend.stage = None monkeypatch.setattr(OvPhysxManager, "backend", backend) + monkeypatch.setattr(OvPhysxManager, "_scene_data_backend", OvPhysxSceneDataBackend()) + monkeypatch.setattr(OvPhysxManager, "_kinematics_dirty", False) @pytest.fixture(autouse=True) @@ -371,9 +373,12 @@ def test_manager_forced_rewarm_invalidates_bindings_before_loading(monkeypatch): lambda event, payload=None: calls.append(event), ) + version = OvPhysxManager._scene_data_backend.transforms_version OvPhysxManager.reset() assert calls == [PhysicsEvent.STOP, "warmup", PhysicsEvent.PHYSICS_READY] + assert OvPhysxManager._scene_data_backend.transforms_version > version + assert OvPhysxManager._kinematics_dirty @pytest.mark.parametrize( @@ -431,6 +436,7 @@ def pinned_config(*, num_threads=None, cooked_collider_cache_dir=None, carbonite OvPhysxManager.backend.physx = physx monkeypatch.setattr(OvPhysxManager, "get_physics_dt", lambda: 0.02) monkeypatch.setattr(PhysicsManager, "_sim_time", 0.0) + version = OvPhysxManager._scene_data_backend.transforms_version OvPhysxManager.step() OvPhysxManager._prepare_physx_for_stage_reuse() @@ -440,6 +446,36 @@ def pinned_config(*, num_threads=None, cooked_collider_cache_dir=None, carbonite assert physx.constructor["config"].cooked_collider_cache_dir == cache_dir assert physx.calls == [("step_sync", 0.02), ("update_articulations_kinematic",), ("reset_stage",), ("wait_op", 23)] assert PhysicsManager._sim_time == 0.02 + assert OvPhysxManager._scene_data_backend.transforms_version > version + assert not OvPhysxManager._kinematics_dirty + + +def test_transforms_finish_dirty_kinematics_before_native_reads(monkeypatch): + """Direct SDP consumers refresh pending FK once, before reading native poses.""" + import warp as wp + from isaaclab_ov.physics import OvPhysxManager + + from isaaclab.scene_data import SceneDataFormat, SceneDataProvider + + calls = [] + OvPhysxManager.backend.physx = SimpleNamespace(update_articulations_kinematic=lambda: calls.append("fk")) + backend = OvPhysxManager._scene_data_backend + poses = wp.zeros(1, dtype=wp.transformf, device="cpu") + backend._transforms.transforms = poses + backend._rigid_bindings = [(SimpleNamespace(read_into=lambda *args: calls.append("read")), poses)] + sdp = SceneDataProvider(backend) + monkeypatch.setattr(OvPhysxManager, "_kinematics_dirty", True) + sdp.get_transforms(SceneDataFormat.Transform()) + sdp.get_transforms(SceneDataFormat.Transform()) + assert calls == ["fk", "read"] + assert not OvPhysxManager._kinematics_dirty + + version = backend.transforms_version + OvPhysxManager.forward() + assert backend.transforms_version > version + sdp.get_transforms(SceneDataFormat.Transform()) + sdp.get_transforms(SceneDataFormat.Transform()) + assert calls == ["fk", "read", "fk", "read"] def test_manager_serializes_env0_only_stage_in_memory(caplog): @@ -839,352 +875,98 @@ def _stop_at_stage_creation(): assert SimulationContext.instance() is None -def _make_stub_binding(prim_paths: list[str]) -> SimpleNamespace: - """Stub an ovphysx ``TensorBinding`` exposing ``shape``, ``count``, ``prim_paths``, and ``read(dst)``.""" - n = len(prim_paths) - return SimpleNamespace( - shape=(n, 7), - count=n, - prim_paths=list(prim_paths), - read=lambda dst: None, # no-op write; transform_count/paths don't trigger reads. - ) - - -def _bare_backend(): - """Construct an ``OvPhysxSceneDataBackend`` instance bypassing the live-wheel ``__init__``. - - Tests seed ``_rigid_bindings`` and the merged buffer directly, mirroring the - bypass-init pattern used in ``test_newton_manager_visualization_state.py``. - """ - from isaaclab_ov.physics.ovphysx_manager import OvPhysxSceneDataBackend - - return object.__new__(OvPhysxSceneDataBackend) - - -def test_transform_count_sums_across_bindings(): - """``transform_count`` returns the sum of each binding's row count.""" - b = _bare_backend() - b._rigid_bindings = [ - { - "pose": _make_stub_binding(["/World/envs/env_0/Cube", "/World/envs/env_1/Cube"]), - "pose_buf": None, - "row_offset": 0, - "row_count": 2, - }, - {"pose": _make_stub_binding(["/World/envs/env_0/Pole"]), "pose_buf": None, "row_offset": 2, "row_count": 1}, - ] - assert b.transform_count == 3 - - -def test_transform_paths_concatenates_prim_paths(): - """``transform_paths`` concatenates each binding's ``prim_paths`` in registration order.""" - b = _bare_backend() - b._rigid_bindings = [ - { - "pose": _make_stub_binding(["/World/envs/env_0/Cube", "/World/envs/env_1/Cube"]), - "pose_buf": None, - "row_offset": 0, - "row_count": 2, - }, - {"pose": _make_stub_binding(["/World/envs/env_0/Pole"]), "pose_buf": None, "row_offset": 2, "row_count": 1}, - ] - assert b.transform_paths == [ - "/World/envs/env_0/Cube", - "/World/envs/env_1/Cube", - "/World/envs/env_0/Pole", - ] - - -def test_transform_count_zero_when_no_bindings(): - """``transform_count`` returns 0 when the bindings list is empty.""" - b = _bare_backend() - b._rigid_bindings = [] - assert b.transform_count == 0 - - -def test_transform_paths_empty_when_no_bindings(): - """``transform_paths`` returns an empty list when the bindings list is empty.""" - b = _bare_backend() - b._rigid_bindings = [] - assert b.transform_paths == [] - - -def test_setup_creates_one_binding_per_distinct_pattern(monkeypatch): - """``setup(physx, stage, device)`` buckets RigidBodyAPI prims by env-wildcard form. - - For cartpole-shaped scenes (``cart``, ``pole``), expect 2 bindings — one - per distinct env-relative prim path. - """ - from isaaclab_ov.physics.ovphysx_manager import OvPhysxSceneDataBackend - - b = OvPhysxSceneDataBackend() - - # Stage stub: traversal yields four RigidBodyAPI prims (cart/pole across two envs). - paths = [ - "/World/envs/env_0/Robot/cart", - "/World/envs/env_0/Robot/pole", - "/World/envs/env_1/Robot/cart", - "/World/envs/env_1/Robot/pole", - ] - - def fake_traverse(): - for p in paths: - yield _fake_rigid_body_prim(p) +def test_transforms_read_native_slices_only_when_dirty(monkeypatch): + """Native bindings fill one shared pose buffer directly and skip clean publications.""" + import isaaclab_ov.physics.ovphysx_manager as module + import numpy as np + import warp as wp - stage = SimpleNamespace(Traverse=fake_traverse) + from isaaclab.scene_data import SceneDataFormat, SceneDataProvider - created: list[SimpleNamespace] = [] + expected = np.array([[1, 2, 3, 0, 0, 0, 1], [4, 5, 6, 0, 0, 0, 1], [7, 8, 9, 0, 0, 0, 1]], dtype=np.float32) + paths = ["/World/envs/env_0/Cart", "/World/envs/env_1/Cart", "/World/envs/env_0/Pole"] + reads = [] class FakePhysX: def create_tensor_binding(self, pattern, tensor_type): - shape = (2, 7) # 2 envs match each pattern - b = SimpleNamespace( - pattern=pattern, - tensor_type=tensor_type, - shape=shape, - count=2, - prim_paths=[], - read=lambda dst: None, - destroy=lambda: None, - ) - created.append(b) - return b - - # Patch UsdPhysics so HasAPI in the test doesn't depend on the real PXR module. - import isaaclab_ov.physics.ovphysx_manager as om_mod - - monkeypatch.setattr(om_mod, "UsdPhysics", SimpleNamespace(RigidBodyAPI=object())) - # Rigid-body setup uses a SimpleNamespace stage stub; skip deformable discovery. - monkeypatch.setattr(om_mod, "discover_deformables_on_stage", lambda stage: []) - - b.setup(FakePhysX(), stage, "cpu") - - # Cartpole = 2 distinct env-wildcard patterns -> 2 bindings. - assert len(created) == 2 - assert {c.pattern for c in created} == { - "/World/envs/env_*/Robot/cart", - "/World/envs/env_*/Robot/pole", - } - # Per-binding row counts sum to 4. - assert b.transform_count == 4 - + start, end = (0, 2) if pattern.endswith("/Cart") else (2, 3) -def test_transforms_reads_each_binding_and_returns_transform_format(): - """``transforms`` writes each binding's poses into the merged buffer at its offset. + def read(dst): + reads.append((start, dst.ptr)) + wp.copy(dst, wp.array(expected[start:end], dtype=wp.float32, device="cpu")) - The returned struct is ``SceneDataFormat.Transform`` with ``transforms`` set to - the merged ``wp.transformf`` array. - """ - import warp as _wp - - _wp.init() - - from isaaclab_ov.physics.ovphysx_manager import OvPhysxSceneDataBackend - - b = OvPhysxSceneDataBackend() - b._merged_transforms = _wp.zeros((3,), dtype=_wp.transformf, device="cpu") - - # Two bindings: first with 2 rows, second with 1 row. - buf_a = _wp.zeros((2, 7), dtype=_wp.float32, device="cpu") - buf_b = _wp.zeros((1, 7), dtype=_wp.float32, device="cpu") - - def fake_read_a(dst): - # Fill with row-distinct sentinel transforms (pos.x = row index, quat = identity). - import numpy as np - - host = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], dtype=np.float32) - _wp.copy(dst, _wp.from_numpy(host, dtype=_wp.float32, device="cpu").reshape((2, 7))) - - def fake_read_b(dst): - import numpy as np - - host = np.array([[3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], dtype=np.float32) - _wp.copy(dst, _wp.from_numpy(host, dtype=_wp.float32, device="cpu").reshape((1, 7))) - - # ``pose_buf_transformf`` is the zero-copy transformf view over the float32 staging - # buffer; production code caches it at setup time. Tests mirror that shape here. - buf_a_tf = _wp.array(ptr=buf_a.ptr, shape=(2,), dtype=_wp.transformf, device="cpu", copy=False) - buf_b_tf = _wp.array(ptr=buf_b.ptr, shape=(1,), dtype=_wp.transformf, device="cpu", copy=False) - b._rigid_bindings = [ - { - "pattern": "/World/envs/env_*/Cube", - "pose": SimpleNamespace(read=fake_read_a, prim_paths=["/Cube0", "/Cube1"]), - "view": SimpleNamespace(read_into=lambda name, dst, _r=fake_read_a: _r(dst)), - "pose_buf": buf_a, - "pose_buf_transformf": buf_a_tf, - "row_offset": 0, - "row_count": 2, - }, - { - "pattern": "/World/envs/env_*/Pole", - "pose": SimpleNamespace(read=fake_read_b, prim_paths=["/Pole"]), - "view": SimpleNamespace(read_into=lambda name, dst, _r=fake_read_b: _r(dst)), - "pose_buf": buf_b, - "pose_buf_transformf": buf_b_tf, - "row_offset": 2, - "row_count": 1, - }, - ] - - out = b.transforms - assert out is b._scene_data - assert out.transforms is b._merged_transforms - - merged_host = out.transforms.numpy() # (3,) of transformf -> view as float32 (3, 7) for assertion - # Each transformf is 7 floats (pos.xyz + quat.xyzw). Verify row 0 / 1 / 2 contents. - flat = merged_host.view(" OVRTXRenderer: renderer._exported_usd_string = None renderer._initialized_scene = False renderer._use_ovstage = False + renderer._sdp = SimpleNamespace(backend=SimpleNamespace(transform_paths=[])) renderer._object_scales = None renderer._object_scales_by_path = {} return renderer @@ -307,17 +306,6 @@ def _record_xforms(value: np.ndarray) -> str: np.testing.assert_array_equal(xforms[0], expected) -def test_write_file_creates_parent_directory_and_writes_utf8(tmp_path: Path): - """_write_file creates nested directories and writes UTF-8 content.""" - output_dir = tmp_path / "nested" / "usd" - - _write_file(output_dir, "stage.usda", "#usda 1.0\n") - - output_path = output_dir / "stage.usda" - assert output_path.is_file() - assert output_path.read_text(encoding="utf-8") == "#usda 1.0\n" - - @pytest.mark.parametrize( "clone_plan", [ @@ -360,31 +348,31 @@ def test_prepare_stage_rejects_non_dense_environment_ids(monkeypatch: pytest.Mon _make_ovrtx_renderer_without_backend().prepare_stage(_make_multi_env_stage(2), 2) -def test_capture_object_scales_populates_source_and_destination_scale_array(): - """Projected source scales reach the body array without replacing a real destination scale.""" +@pytest.mark.parametrize("env_template", ["/World/envs/env_{}", "/World/Instances/World_{}"]) +def test_capture_object_scales_populates_source_and_destination_scale_array(env_template): + """Only declared prototype and shared scales reach the body array, independent of namespace.""" stage = Usd.Stage.CreateInMemory() - UsdGeom.Xform.Define(stage, "/World") - UsdGeom.Xform.Define(stage, "/World/envs") - UsdGeom.Xform.Define(stage, "/World/envs/env_0") - UsdGeom.Xform.Define(stage, "/World/envs/env_1") - UsdGeom.Xform.Define(stage, "/World/envs/env_2") - UsdGeom.Xform.Define(stage, "/World/envs/env_0/Object").AddScaleOp().Set(Gf.Vec3d(1.0, 1.0, 8.0)) - UsdGeom.Xform.Define(stage, "/World/envs/env_1/Object").AddScaleOp().Set(Gf.Vec3d(1.0, 1.0, 4.0)) + UsdGeom.Xform.Define(stage, f"{env_template.format(0)}/Object").AddScaleOp().Set(Gf.Vec3d(1, 1, 8)) + UsdGeom.Xform.Define(stage, f"{env_template.format(1)}/Object").AddScaleOp().Set(Gf.Vec3d(1, 1, 4)) + UsdGeom.Xform.Define(stage, "/World/Shared").AddScaleOp().Set(Gf.Vec3d(2, 3, 4)) + UsdGeom.Xform.Define(stage, "/World/envs/Unplanned").AddScaleOp().Set(Gf.Vec3d(5, 6, 7)) renderer = _make_ovrtx_renderer_without_backend() renderer._device = "cpu" plan = ClonePlan( - sources=("/World/envs/env_0",), - destinations=("/World/envs/env_{}",), - clone_mask=torch.ones((1, 3), dtype=torch.bool), - env_ids=torch.arange(3), + sources=(env_template.format(0), env_template.format(1)), + destinations=(env_template, env_template), + clone_mask=np.array([[True, False, True], [False, True, False]]), + env_ids=np.arange(3), + global_paths=("/World/Shared",), ) renderer._capture_object_scales(stage, plan) scales = renderer._create_object_scale_array( - ["/World/envs/env_0/Object", "/World/envs/env_1/Object", "/World/envs/env_2/Object"] + [f"{env_template.format(index)}/Object" for index in range(3)] + ["/World/Shared"] ) - np.testing.assert_allclose(scales.numpy(), np.array([[1.0, 1.0, 8.0], [1.0, 1.0, 4.0], [1.0, 1.0, 8.0]])) + np.testing.assert_allclose(scales.numpy(), [[1, 1, 8], [1, 1, 4], [1, 1, 8], [2, 3, 4]]) + assert "/World/envs/Unplanned" not in renderer._object_scales_by_path def test_prepare_stage_keeps_material_binding_inside_clone_source(monkeypatch: pytest.MonkeyPatch): @@ -432,15 +420,16 @@ def test_prepare_stage_writes_pre_ovrtx_stage_dump(tmp_path: Path, monkeypatch: stage = _make_multi_env_stage(2) renderer = _make_ovrtx_renderer_without_backend() - renderer.cfg.temp_usd_dir = str(tmp_path) + output_dir = tmp_path / "nested" / "usd" + renderer.cfg.temp_usd_dir = str(output_dir) expected_pre_export = stage.ExportToString() renderer.prepare_stage(stage, 2) - pre_stage_path = tmp_path / _PRE_OVRTX_STAGE_FILE + pre_stage_path = output_dir / _PRE_OVRTX_STAGE_FILE assert pre_stage_path.is_file() assert pre_stage_path.read_text(encoding="utf-8") == expected_pre_export - assert (tmp_path / _OVRTX_STAGE_FILE).exists() is False + assert (output_dir / _OVRTX_STAGE_FILE).exists() is False def test_prepare_stage_skips_temp_usd_write_when_temp_usd_dir_unset(monkeypatch: pytest.MonkeyPatch): @@ -574,8 +563,8 @@ def _write_array_attribute(prim_paths: list[str], attribute_name: str, tensors: ] -def test_prepare_stage_stores_clone_plan_and_exports(monkeypatch: pytest.MonkeyPatch): - """prepare_stage stores the clone plan and exports only its source-row content.""" +def test_prepare_stage_exports_only_clone_source_content(monkeypatch: pytest.MonkeyPatch): + """prepare_stage exports only its source-row content.""" num_envs = 4 published = ClonePlan( @@ -592,8 +581,6 @@ def test_prepare_stage_stores_clone_plan_and_exports(monkeypatch: pytest.MonkeyP renderer.prepare_stage(stage, 4) - assert renderer._clone_plan is published - # Only the env_0 source subtree keeps content. The rows clone the env roots themselves, so the # remaining roots are trimmed: OVRTX refuses to clone onto a prim that already exists. _assert_export_contains_env_roots_and_children(renderer._exported_usd_string, [0]) diff --git a/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py b/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py index de45923463ef..941767ebfae9 100644 --- a/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py +++ b/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py @@ -116,68 +116,8 @@ def _make_renderer_without_backend(device: str = "cpu") -> tuple[OVRTXRenderer, return renderer, renderer.backend.renderer -def test_points_array_binding_uses_write_not_map(): - """OVRTX array bindings accept ``List[DLTensor]`` via ``write()``, not mapped tensors.""" - binding = _FakePointsBinding("points") - with pytest.raises(RuntimeError, match="do not expose mapped point buffers"): - binding.map() - - -def test_setup_deformable_bindings_binds_surface_mesh_points(monkeypatch: pytest.MonkeyPatch): - """Surface deformable registry entries create OVRTX ``points`` array bindings.""" - renderer, backend = _make_renderer_without_backend() - entry = SimpleNamespace( - prim_path="/World/envs/env_[^/]+/Deformable", - vis_mesh_prim_path="/World/envs/env_[^/]+/Deformable/mesh", - deformable_type="surface", - particle_offsets=[7], - particles_per_body=3, - ) - - monkeypatch.setattr(NewtonManager, "_deformable_registry", [entry]) - - renderer._setup_deformable_bindings_legacy(num_envs=1) - - assert len(backend.calls) == 1 - assert backend.calls[0]["prim_paths"] == ["/World/envs/env_0/Deformable/mesh"] - assert backend.calls[0]["attribute_name"] == "points" - assert backend.calls[0]["dtype"] is np.float32 - assert backend.calls[0]["shape"] == (3,) - assert renderer._deformable_points_binding is backend.bindings["points"] - assert len(backend.writes) == 2 - assert backend.writes[0]["attribute_name"] == "omni:resetXformStack" - assert backend.writes[0]["prim_paths"] == ["/World/envs/env_0/Deformable/mesh"] - assert backend.writes[1]["attribute_name"] == "omni:xform" - assert backend.writes[1]["prim_paths"] == ["/World/envs/env_0/Deformable/mesh"] - assert len(renderer._deformable_particle_counts) == 1 - assert renderer._deformable_particle_counts[0] == 3 - assert renderer._deformable_particle_offsets == [7] - - -def test_setup_deformable_bindings_binds_volume_mesh_points(monkeypatch: pytest.MonkeyPatch): - """Volume deformable registry entries create OVRTX ``points`` bindings.""" - renderer, backend = _make_renderer_without_backend() - entry = SimpleNamespace( - prim_path="/World/envs/env_[^/]+/Deformable", - vis_mesh_prim_path="/World/envs/env_[^/]+/Deformable/mesh", - deformable_type="volume", - particle_offsets=[7], - particles_per_body=3, - ) - - monkeypatch.setattr(NewtonManager, "_deformable_registry", [entry]) - - renderer._setup_deformable_bindings_legacy(num_envs=1) - - assert len(backend.calls) == 1 - assert backend.calls[0]["prim_paths"] == ["/World/envs/env_0/Deformable/mesh"] - assert backend.calls[0]["attribute_name"] == "points" - assert renderer._deformable_points_binding is backend.bindings["points"] - assert renderer._deformable_particle_offsets == [7] - - def test_setup_deformable_bindings_binds_mixed_surface_and_volume_entries(monkeypatch: pytest.MonkeyPatch): - """Surface and volume deformable registry entries bind together with distinct offsets.""" + """Registry metadata binds every surface and volume instance without a USD stage.""" renderer, backend = _make_renderer_without_backend() surface_entry = SimpleNamespace( prim_path="/World/envs/env_[^/]+/DeformableSurface", @@ -194,99 +134,32 @@ def test_setup_deformable_bindings_binds_mixed_surface_and_volume_entries(monkey particles_per_body=3, ) + monkeypatch.setattr("isaaclab.sim.utils.stage.get_current_stage", lambda: None) monkeypatch.setattr(NewtonManager, "_deformable_registry", [surface_entry, volume_entry]) renderer._setup_deformable_bindings_legacy(num_envs=2) - assert backend.calls[0]["prim_paths"] == [ + paths = [ "/World/envs/env_0/DeformableSurface/mesh", "/World/envs/env_1/DeformableSurface/mesh", "/World/envs/env_0/DeformableVolume/mesh", "/World/envs/env_1/DeformableVolume/mesh", ] - assert renderer._deformable_particle_offsets == [0, 3, 6, 9] - assert renderer._deformable_particle_counts == [3, 3, 3, 3] - - -def test_setup_deformable_bindings_works_without_stage(monkeypatch: pytest.MonkeyPatch): - """Deformable bindings are created from registry metadata without a USD stage.""" - renderer, backend = _make_renderer_without_backend() - entry = SimpleNamespace( - prim_path="/World/envs/env_[^/]+/Deformable", - vis_mesh_prim_path="/World/envs/env_[^/]+/Deformable/mesh", - deformable_type="surface", - particle_offsets=[0], - particles_per_body=3, - ) - - monkeypatch.setattr("isaaclab.sim.utils.stage.get_current_stage", lambda: None) - monkeypatch.setattr(NewtonManager, "_deformable_registry", [entry]) - - renderer._setup_deformable_bindings_legacy(num_envs=1) - assert len(backend.calls) == 1 - assert backend.calls[0]["prim_paths"] == ["/World/envs/env_0/Deformable/mesh"] - assert renderer._deformable_points_binding is backend.bindings["points"] - - -def test_setup_deformable_bindings_binds_all_surface_mesh_instances(monkeypatch: pytest.MonkeyPatch): - """Surface deformable registry entries bind every cloned visual mesh instance.""" - renderer, backend = _make_renderer_without_backend() - entry = SimpleNamespace( - prim_path="/World/envs/env_[^/]+/Deformable", - vis_mesh_prim_path="/World/envs/env_[^/]+/Deformable/mesh", - deformable_type="surface", - particle_offsets=[0, 3, 6, 9], - particles_per_body=3, - ) - - monkeypatch.setattr(NewtonManager, "_deformable_registry", [entry]) - - renderer._setup_deformable_bindings_legacy(num_envs=4) - - expected_paths = [f"/World/envs/env_{i}/Deformable/mesh" for i in range(4)] - assert backend.calls[0]["prim_paths"] == expected_paths - assert renderer._deformable_particle_offsets == [0, 3, 6, 9] - assert renderer._deformable_particle_counts == [3, 3, 3, 3] - - -def test_update_deformable_points_writes_world_particle_positions(monkeypatch: pytest.MonkeyPatch): - """Newton ``particle_q`` slices are handed to OVRTX through :meth:`OVRTXRenderer.update_geometries`.""" - renderer, _backend = _make_renderer_without_backend() - renderer._deformable_points_binding = _FakePointsBinding("points") - renderer._deformable_particle_offsets = [1] - renderer._deformable_particle_counts = [3] - particle_q = wp.array( - [ - wp.vec3f(-1.0, -1.0, -1.0), - wp.vec3f(1.0, 2.0, 3.0), - wp.vec3f(4.0, 5.0, 6.0), - wp.vec3f(7.0, 8.0, 9.0), - ], - dtype=wp.vec3f, - device="cpu", - ) + assert backend.calls[0]["prim_paths"] == paths + assert backend.calls[0]["attribute_name"] == "points" + assert backend.calls[0]["dtype"] is np.float32 + assert backend.calls[0]["shape"] == (3,) + assert [write["attribute_name"] for write in backend.writes] == ["omni:resetXformStack", "omni:xform"] + assert all(write["prim_paths"] == paths for write in backend.writes) + particle_q = wp.array(np.arange(36, dtype=np.float32).reshape(12, 3), dtype=wp.vec3f, device="cpu") monkeypatch.setattr(NewtonManager, "get_state", classmethod(lambda cls: SimpleNamespace(particle_q=particle_q))) - - class _FakeStream: - cuda_stream = 42 - - renderer._warp_device = SimpleNamespace(stream=_FakeStream()) - + renderer._warp_device = SimpleNamespace(stream=SimpleNamespace(cuda_stream=42)) renderer.update_geometries() - - written = renderer._deformable_points_binding.written - assert written is not None - assert len(written) == 1 - assert written[0].ptr == particle_q[1:4].ptr - assert renderer._deformable_points_binding.write_kwargs is not None - assert renderer._deformable_points_binding.write_kwargs["data_access"] is DataAccess.ASYNC - assert renderer._deformable_points_binding.write_kwargs["cuda_stream"] == 42 - assert written[0].numpy().tolist() == [ - [1.0, 2.0, 3.0], - [4.0, 5.0, 6.0], - [7.0, 8.0, 9.0], - ] + written = backend.bindings["points"].written + assert len(written) == len(paths) + for points, offset in zip(written, surface_entry.particle_offsets + volume_entry.particle_offsets, strict=True): + np.testing.assert_array_equal(points.numpy(), particle_q.numpy()[offset : offset + 3]) def test_setup_deformable_bindings_rejects_offset_count_mismatch(monkeypatch: pytest.MonkeyPatch): @@ -334,33 +207,6 @@ def test_update_geometries_rejects_inconsistent_deformable_mapping(monkeypatch: renderer.update_geometries() -def test_setup_particle_points_bindings_binds_mpm_visual_prims(monkeypatch: pytest.MonkeyPatch): - """MPM particle visual prims create an OPTIMIZE ``points`` array binding.""" - renderer, backend = _make_renderer_without_backend() - particle_visual_prims = { - "/World/envs/env_0/Media/Particles": SimpleNamespace(offset=10, count=5), - "/World/envs/env_1/Media/Particles": SimpleNamespace(offset=15, count=5), - } - - monkeypatch.setattr(NewtonManager, "_particle_visual_prims", particle_visual_prims) - - renderer._setup_particle_bindings_legacy() - - assert len(backend.calls) == 1 - assert backend.calls[0]["prim_paths"] == [ - "/World/envs/env_0/Media/Particles", - "/World/envs/env_1/Media/Particles", - ] - assert backend.calls[0]["attribute_name"] == "points" - assert backend.calls[0]["flags"] is BindingFlag.OPTIMIZE - assert renderer._particle_points_binding is backend.bindings["points"] - assert renderer._particle_visual_offsets == [10, 15] - assert renderer._particle_visual_counts == [5, 5] - assert len(backend.writes) == 2 - assert backend.writes[0]["attribute_name"] == "omni:resetXformStack" - assert backend.writes[1]["attribute_name"] == "omni:xform" - - def test_setup_particle_points_bindings_binds_multiple_mpm_assets(monkeypatch: pytest.MonkeyPatch): """Multiple MPM assets bind as ``num_assets * num_envs`` points prims, like deformables.""" renderer, backend = _make_renderer_without_backend() @@ -375,6 +221,10 @@ def test_setup_particle_points_bindings_binds_multiple_mpm_assets(monkeypatch: p renderer._setup_particle_bindings_legacy() + assert len(backend.calls) == 1 + assert backend.calls[0]["attribute_name"] == "points" + assert backend.calls[0]["flags"] is BindingFlag.OPTIMIZE + assert [write["attribute_name"] for write in backend.writes] == ["omni:resetXformStack", "omni:xform"] # Binding order follows dict insertion order (no path sort). assert backend.calls[0]["prim_paths"] == [ "/World/envs/env_0/Media/Particles", @@ -382,55 +232,20 @@ def test_setup_particle_points_bindings_binds_multiple_mpm_assets(monkeypatch: p "/World/envs/env_0/Foam/Particles", "/World/envs/env_1/Foam/Particles", ] - assert renderer._particle_visual_offsets == [0, 5, 10, 13] - assert renderer._particle_visual_counts == [5, 5, 3, 3] - - -def test_update_particle_points_writes_world_particle_positions(monkeypatch: pytest.MonkeyPatch): - """The first MPM ``points`` update writes world-space positions through GPU ASYNC.""" - renderer, backend = _make_renderer_without_backend() - renderer._particle_points_binding = _FakePointsBinding("points") - renderer._particle_visual_offsets = [2] - renderer._particle_visual_counts = [2] - particle_q = wp.array( - [ - wp.vec3f(0.0, 0.0, 0.0), - wp.vec3f(1.0, 0.0, 0.0), - wp.vec3f(2.0, 3.0, 4.0), - wp.vec3f(5.0, 6.0, 7.0), - ], - dtype=wp.vec3f, - device="cpu", - ) + particle_q = wp.array(np.arange(48, dtype=np.float32).reshape(16, 3), dtype=wp.vec3f, device="cpu") monkeypatch.setattr(NewtonManager, "get_state", classmethod(lambda cls: SimpleNamespace(particle_q=particle_q))) - - class _FakeStream: - cuda_stream = 42 - - renderer._warp_device = SimpleNamespace(stream=_FakeStream()) - + renderer._warp_device = SimpleNamespace(stream=SimpleNamespace(cuda_stream=42)) renderer.update_geometries() - - assert len(backend.writes) == 0 - written = renderer._particle_points_binding.written - assert written is not None - assert len(written) == 1 - assert written[0].ptr == particle_q[2:4].ptr - assert written[0].numpy().tolist() == [ - [2.0, 3.0, 4.0], - [5.0, 6.0, 7.0], - ] - assert renderer._particle_points_binding.write_kwargs is not None - assert renderer._particle_points_binding.write_kwargs["data_access"] is DataAccess.ASYNC - assert renderer._particle_points_binding.write_kwargs["cuda_stream"] == 42 + for points, visual in zip(backend.bindings["points"].written, particle_visual_prims.values(), strict=True): + np.testing.assert_array_equal(points.numpy(), particle_q.numpy()[visual.offset : visual.offset + visual.count]) def test_update_geometries_writes_deformable_and_mpm_bindings(monkeypatch: pytest.MonkeyPatch): """Deformable and MPM points use GPU ASYNC writes from the first update.""" renderer, backend = _make_renderer_without_backend() renderer._deformable_points_binding = _FakePointsBinding("deformable_points") - renderer._deformable_particle_offsets = [0] - renderer._deformable_particle_counts = [2] + renderer._deformable_particle_offsets = [1] + renderer._deformable_particle_counts = [1] renderer._particle_points_binding = _FakePointsBinding("points") renderer._particle_visual_offsets = [2] renderer._particle_visual_counts = [2] @@ -456,7 +271,8 @@ class _FakeStream: deformable_written = renderer._deformable_points_binding.written assert deformable_written is not None assert len(deformable_written) == 1 - assert deformable_written[0].ptr == particle_q[0:2].ptr + assert deformable_written[0].ptr == particle_q[1:2].ptr + np.testing.assert_array_equal(deformable_written[0].numpy(), particle_q.numpy()[1:2]) assert renderer._deformable_points_binding.write_kwargs is not None assert renderer._deformable_points_binding.write_kwargs["data_access"] is DataAccess.ASYNC assert renderer._deformable_points_binding.write_kwargs["cuda_stream"] == 42 @@ -466,10 +282,10 @@ class _FakeStream: assert mpm_written is not None assert len(mpm_written) == 1 assert mpm_written[0].ptr == particle_q[2:4].ptr + np.testing.assert_array_equal(mpm_written[0].numpy(), particle_q.numpy()[2:4]) assert renderer._particle_points_binding.write_kwargs is not None assert renderer._particle_points_binding.write_kwargs["data_access"] is DataAccess.ASYNC assert renderer._particle_points_binding.write_kwargs["cuda_stream"] == 42 - assert len(backend.writes) == 0 def _install_cable_shapes(shapes: dict[str, list[int]], monkeypatch: pytest.MonkeyPatch) -> None: @@ -593,31 +409,72 @@ def _write(query, attribute, **kwargs): assert tensors[0].dtype.lanes == 3 -def test_update_transforms_writes_caller_owned_buffer(monkeypatch: pytest.MonkeyPatch): - """Object xforms fill a persistent GPU buffer and blocking ASYNC write, not map/unmap.""" - renderer, _ = _make_renderer_without_backend() - buffer = object() - renderer._object_xform_binding = _FakePointsBinding("omni:xform") - renderer._object_newton_indices = [0, 1] - renderer._object_scales = object() - renderer._object_transform_buffer = buffer - - monkeypatch.setattr(NewtonManager, "get_state", classmethod(lambda cls: SimpleNamespace(body_q=object()))) - launch_kwargs: dict = {} +@pytest.mark.parametrize("use_ovstage", [False, True]) +def test_update_transforms_consumes_sdp_matrices_once_per_publication(monkeypatch, use_ovstage): + """Both OVRTX paths bind published bodies and consume SDP's scaled, transposed matrices.""" + from isaaclab.scene_data import SceneDataFormat, SceneDataProvider - def _capture_launch(*args, **kwargs): - launch_kwargs.update(kwargs) + def reject_newton_access(*args, **kwargs): + raise AssertionError("Rigid transform transport must not read Newton state") - monkeypatch.setattr(ovrtx_renderer_module.wp, "launch", _capture_launch) + monkeypatch.setattr(NewtonManager, "get_model", reject_newton_access) + monkeypatch.setattr(NewtonManager, "get_state", reject_newton_access) + assert not hasattr(ovrtx_renderer_module, "sync_newton_transforms_kernel") + renderer, _ = _make_renderer_without_backend() + paths = ["/World/Shared", "/World/envs/env_1/Object"] + poses = np.array([[1, 2, 3, 0, 0, 0, 1], [4, 5, 6, 0, 0, 0, 1]], dtype=np.float32) + transforms = SceneDataFormat.Transform() + transforms.transforms = wp.array(poses, dtype=wp.transformf, device="cpu") + backend = SimpleNamespace(transforms=transforms, transforms_version=0, transform_count=2, transform_paths=paths) + backend.get_transforms = lambda _format: transforms + renderer._sdp = SceneDataProvider(backend) + renderer._transform_version = -1 + renderer._object_scales_by_path = {paths[0]: (2, 3, 4)} renderer._warp_device = SimpleNamespace(stream=SimpleNamespace(cuda_stream=99)) + renderer._use_ovstage = use_ovstage + renderer._current_ordinal = 5 + writes = [] + + if use_ovstage: + renderer.backend.paths = SimpleNamespace(create_path_list_from_strings=lambda actual: actual) + renderer.backend.stage = SimpleNamespace( + query_from_path_list=lambda actual: actual, + write_attribute=lambda query, attribute, **kwargs: ( + writes.append((query, attribute, kwargs)) or SimpleNamespace(wait=lambda: None) + ), + ) + monkeypatch.setattr(ovrtx_renderer_module, "xform_tensor_from_warp", lambda matrices: matrices) + renderer._setup_xform_bindings_ovstage() + assert renderer._object_xform_query == paths + writes.clear() + else: + renderer._setup_xform_bindings_legacy() + assert renderer.backend.renderer.calls[0]["prim_paths"] == paths + renderer._object_xform_binding.write = lambda matrices, **kwargs: writes.append((None, matrices, kwargs)) renderer.update_transforms() - - assert launch_kwargs["inputs"][0] is buffer - assert launch_kwargs["dim"] == 2 - assert renderer._object_xform_binding.written is buffer - assert renderer._object_xform_binding.write_kwargs["data_access"] is DataAccess.ASYNC - assert renderer._object_xform_binding.write_kwargs["cuda_stream"] == 99 + renderer.update_transforms() + assert len(writes) == 1 + matrices = writes[0][2]["tensors"] if use_ovstage else writes[0][1] + expected = np.tile(np.eye(4), (2, 1, 1)) + expected[0, :3, :3] = np.diag([2, 3, 4]) + expected[:, 3, :3] = poses[:, :3] + np.testing.assert_array_equal(matrices.numpy(), expected) + assert writes[0][2]["cuda_stream"] == 99 + if use_ovstage: + assert writes[0][2]["ordinal"] == 5 + else: + assert writes[0][2]["data_access"] is DataAccess.ASYNC + + poses[:, 0] += 10 + transforms.transforms.assign(poses) + backend.transforms_version += 1 + renderer.update_transforms() + assert len(writes) == 2 + updated = writes[1][2]["tensors"] if use_ovstage else writes[1][1] + assert updated is matrices + expected[:, 3, :3] = poses[:, :3] + np.testing.assert_array_equal(updated.numpy(), expected) def test_update_camera_writes_without_mapping(monkeypatch: pytest.MonkeyPatch): diff --git a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py index 166c78857870..e3dce5bdd66b 100644 --- a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py +++ b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py @@ -88,6 +88,7 @@ def _make_ovrtx_renderer_without_backend() -> OVRTXRenderer: @pytest.fixture(autouse=True) def _simulation_registry(monkeypatch): sim = types.SimpleNamespace(_backend_registry=[]) + sim.get_scene_data_provider = lambda: types.SimpleNamespace(backend=types.SimpleNamespace(transform_paths=[])) sim.get_or_create_backend = SimulationContext.get_or_create_backend.__get__(sim) sim.close_backend = SimulationContext.close_backend.__get__(sim) monkeypatch.setattr(SimulationContext, "_instance", sim) @@ -503,11 +504,6 @@ def fake_clone(src, *, device): assert clone_calls == [(source, "cuda:0")] -class _FakeArray: - def __init__(self, shape): - self.shape = shape - - def test_launch_extract_all_tiles_rejects_wider_output_channels(): """An output wider than the tiled input would read out of bounds, so it must raise before launching.""" renderer = _make_ovrtx_renderer_without_backend() @@ -515,55 +511,9 @@ def test_launch_extract_all_tiles_rejects_wider_output_channels(): render_data = _make_ovrtx_camera_render_data() with pytest.raises(ValueError, match="out of bounds"): - renderer._launch_extract_all_tiles(render_data, _FakeArray((8, 16, 3)), _FakeArray((2, 8, 16, 4))) - - -def test_launch_extract_all_tiles_launches_kernel_when_channels_are_compatible(monkeypatch): - """Equal or narrower output channel counts pass validation and reach the kernel launch.""" - renderer = _make_ovrtx_renderer_without_backend() - renderer._device = "cpu" - render_data = _make_ovrtx_camera_render_data() - render_data.num_cols = 2 - - launch_calls = [] - monkeypatch.setattr(wp, "launch", lambda **kwargs: launch_calls.append(kwargs)) - - tiled_buffer = _FakeArray((8, 16, 4)) - output_buffer = _FakeArray((2, 8, 16, 3)) - renderer._launch_extract_all_tiles(render_data, tiled_buffer, output_buffer) - - assert len(launch_calls) == 1 - assert launch_calls[0]["inputs"][:2] == [tiled_buffer, output_buffer] - - -def test_ovrtx_read_output_copies_no_pixel_data(): - """OVRTXRenderer.read_output copies no pixel data; with empty renderer_info it leaves info untouched.""" - renderer = _make_ovrtx_renderer_without_backend() - render_data = _make_ovrtx_camera_render_data() - camera_data = CameraData() - camera_data.info = {} - camera_data._output = {} - - result = renderer.read_output(render_data, camera_data) - assert result is None - assert render_data.warp_buffers == {} - assert camera_data.info == {} - assert camera_data.output == {} - - -def test_ovrtx_read_output_forwards_renderer_info(): - """OVRTXRenderer.read_output forwards render_data.renderer_info (e.g. semantic idToLabels) into info.""" - renderer = _make_ovrtx_renderer_without_backend() - render_data = _make_ovrtx_camera_render_data() - id_to_labels = {"2": {"class": "cartpole"}} - render_data.renderer_info = {"semantic_segmentation": {"idToLabels": id_to_labels}} - - camera_data = CameraData() - camera_data.info = {"semantic_segmentation": None} - camera_data._output = {} - - renderer.read_output(render_data, camera_data) - assert camera_data.info["semantic_segmentation"] == {"idToLabels": id_to_labels} + renderer._launch_extract_all_tiles( + render_data, types.SimpleNamespace(shape=(8, 16, 3)), types.SimpleNamespace(shape=(2, 8, 16, 4)) + ) def test_ovrtx_read_output_clears_stale_metadata_and_keeps_seeded_keys(): @@ -751,6 +701,7 @@ def test_ovrtx_cleanup_releases_only_the_given_render_data(cleanup_directly, use if cleanup_directly: render_data.cleanup() + renderer.cleanup(None) renderer.cleanup(render_data) renderer.cleanup(render_data) @@ -771,18 +722,6 @@ def test_ovrtx_cleanup_releases_only_the_given_render_data(cleanup_directly, use assert renderer._initialized_scene is True -def test_ovrtx_cleanup_without_render_data_keeps_renderer_state(): - """``cleanup(None)`` has nothing to release and must not disturb the renderer.""" - renderer = _make_ovrtx_renderer_without_backend() - renderer._render_product_paths = ["/RenderCamera_0/RenderProduct_camera"] - renderer._initialized_scene = True - - renderer.cleanup(None) - - assert renderer._render_product_paths == ["/RenderCamera_0/RenderProduct_camera"] - assert renderer._initialized_scene is True - - @pytest.mark.parametrize( "camera_path", [ @@ -996,7 +935,6 @@ def close(self) -> None: renderer._particle_paths_list = "particle" renderer._cable_points_query = "cable" renderer._cable_paths_list = "cable" - renderer._object_newton_indices = object() renderer._deformable_particle_offsets = [0] renderer._deformable_particle_counts = [1] renderer._particle_visual_offsets = [0] @@ -1014,8 +952,7 @@ def test_ovrtx_close_releases_legacy_renderer_state(): """Borrowers unbind their tensor bindings before the registry closes the native engine.""" events: list[str] = [] renderer = _make_legacy_renderer_with_backend(events) - render_data = renderer._camera_render_data[0] - + renderer.close() renderer.close() assert "destroy_renderer" not in events SimulationContext.instance().close_backend(renderer.backend) @@ -1028,28 +965,12 @@ def test_ovrtx_close_releases_legacy_renderer_state(): "unbind:cable", "destroy_renderer", ] - assert renderer._camera_xform_binding is None - assert renderer._camera_render_data == [] - assert render_data.camera_xform_binding is None - assert render_data.renderer_info == {} - assert renderer._object_xform_binding is None - assert renderer._object_transform_buffer is None - assert renderer._deformable_points_binding is None - assert renderer._particle_points_binding is None - assert renderer._cable_points_binding is None - assert renderer._particle_workaround_applied is False - assert renderer.backend.renderer is None - assert renderer._render_product_paths == [] - assert renderer._output_id_color_buffers == {} - assert renderer._initialized_scene is False def test_ovrtx_close_releases_ovstage_renderer_state(): """Queries release before the native engine, which must detach before stage resources close.""" events: list[str] = [] renderer = _make_ovstage_renderer_with_backend(events) - render_data = renderer._camera_render_data[0] - renderer.close() assert "destroy_renderer" not in events SimulationContext.instance().close_backend(renderer.backend) @@ -1069,21 +990,6 @@ def test_ovrtx_close_releases_ovstage_renderer_state(): "destroy_renderer", "exit_stack_close", ] - assert renderer._camera_xform_query is None - assert renderer._camera_render_data == [] - assert render_data.camera_xform_query is None - assert render_data.renderer_info == {} - assert renderer._particle_paths_list is None - assert renderer._cable_points_query is None - assert renderer._cable_paths_list is None - assert renderer._object_newton_indices is None - assert renderer.backend.renderer is None - assert renderer.backend.stage is None - assert renderer.backend.paths is None - assert renderer._render_product_paths == [] - assert renderer._output_id_color_buffers == {} - assert renderer._initialized_scene is False - assert renderer._current_ordinal == 0 events.clear() renderer.close() assert events == [] diff --git a/source/isaaclab_physx/changelog.d/sdp-transform-publication.rst b/source/isaaclab_physx/changelog.d/sdp-transform-publication.rst new file mode 100644 index 000000000000..9593f8b146cd --- /dev/null +++ b/source/isaaclab_physx/changelog.d/sdp-transform-publication.rst @@ -0,0 +1,8 @@ +Changed +^^^^^^^ + +* Published PhysX rigid transforms and their producer-owned version through SDP, and routed Isaac RTX + transform updates through one simulation-owned ``FabricBackend`` shared with Kit while preserving + native PhysX Fabric updates. Stage/device identified the resource; transforms remained binding state, + with SDP passed explicitly to updates. Fabric selection and hierarchy state moved out of core ``RenderContext``. + Kit app updates requested current SDP transforms without an additional physics ``forward()`` call. diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py index e1950a8ab516..228fd14618dd 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py @@ -560,6 +560,7 @@ def write_root_link_pose_to_sim_index( self.data._reset_pose() # set into simulation self.root_view.set_root_transforms(self.data._root_link_pose_w.data.view(wp.float32), indices=sim_env_ids) + SimulationManager.invalidate_transforms(kinematics=True) def write_root_link_pose_to_sim_mask( self, @@ -657,6 +658,7 @@ def write_root_com_pose_to_sim_index( self.data._reset_pose(from_link=False) # set into simulation self.root_view.set_root_transforms(self.data._root_link_pose_w.data.view(wp.float32), indices=sim_env_ids) + SimulationManager.invalidate_transforms(kinematics=True) def write_root_com_pose_to_sim_mask( self, @@ -1057,6 +1059,7 @@ def write_joint_state_to_sim_index( self.data._reset_velocity() # set into simulation self.root_view.set_dof_positions(joint_pos_backend, indices=sim_env_ids) + SimulationManager.invalidate_transforms(kinematics=True) self.root_view.set_dof_velocities(joint_vel_backend, indices=sim_env_ids) def write_joint_state_to_sim_mask( @@ -1161,6 +1164,7 @@ def write_joint_position_to_sim_index( self.data._reset_velocity() # set into simulation self.root_view.set_dof_positions(joint_pos_backend, indices=sim_env_ids) + SimulationManager.invalidate_transforms(kinematics=True) def write_joint_position_to_sim_mask( self, diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py index c8d776e756b4..57a9fb933294 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py @@ -153,6 +153,7 @@ def _ensure_fk_fresh(self) -> None: """ if self._fk_timestamp < self._sim_timestamp: self._physics_sim_view.update_articulations_kinematic() + SimulationManager._kinematics_dirty = False self._fk_timestamp = self._sim_timestamp def _reset_pose(self, from_link: bool = True) -> None: diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py index c190b516cac4..3529f307c8fa 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py @@ -371,6 +371,7 @@ def write_root_link_pose_to_sim_index( self.data._reset_pose() # set into simulation self.root_view.set_transforms(self._get_root_link_pose_w_f32(), indices=sim_env_ids) + SimulationManager.invalidate_transforms() def write_root_link_pose_to_sim_mask( self, @@ -463,6 +464,7 @@ def write_root_com_pose_to_sim_index( self.data._reset_pose(from_link=False) # set into simulation self.root_view.set_transforms(self._get_root_link_pose_w_f32(), indices=sim_env_ids) + SimulationManager.invalidate_transforms() def write_root_com_pose_to_sim_mask( self, diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py index 3f8afeb8aefa..05936dee34ea 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py @@ -475,6 +475,7 @@ def write_body_link_pose_to_sim_index( self.reshape_data_to_view_2d(self.data._body_link_pose_w.data, device=self.device).view(wp.float32), indices=view_ids, ) + SimulationManager.invalidate_transforms() def write_body_link_pose_to_sim_mask( self, @@ -585,6 +586,7 @@ def write_body_com_pose_to_sim_index( self.reshape_data_to_view_2d(self.data._body_link_pose_w.data, device=self.device).view(wp.float32), indices=view_ids, ) + SimulationManager.invalidate_transforms() def write_body_com_pose_to_sim_mask( self, diff --git a/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py b/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py index ea5ede85eda2..43b8188e30f7 100644 --- a/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py +++ b/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py @@ -30,6 +30,7 @@ import omni.physx import omni.timeline import omni.usd +import usdrt from pxr import Sdf, Usd, UsdPhysics, UsdUtils import isaaclab.sim as sim_utils @@ -187,7 +188,8 @@ class PhysxSceneDataBackend(SceneDataBackend): """Borrowed native resource; its lifetime belongs to the simulation registry.""" def __init__(self): - self._scene_data = SceneDataFormat.Transform() + self._transforms = SceneDataFormat.Transform() + self.transforms_version = 0 self._points_data = SceneDataFormat.Points() self.clear() @@ -197,7 +199,11 @@ def clear(self) -> None: self._rigid_body_view: omni.physics.tensors.RigidBodyView | None = None self._volume_deformable_view: omni.physics.tensors.DeformableBodyView | None = None self._surface_deformable_view: omni.physics.tensors.DeformableBodyView | None = None - self._scene_data.transforms = None + self._transforms.transforms = None + self.transforms_version += 1 + self._poses_version = self._fabric_version = -1 + self._fabric_transforms = SceneDataFormat.FabricMatrix44() + self._fabric_selection = None self._points_data.points = None self._geometry_paths: list[str] = [] self._geometry_counts: list[int] = [] @@ -358,12 +364,21 @@ def geometry_counts(self) -> list[int]: self._discover_deformable_geometry() return self._geometry_counts + @property + def native_transform_formats(self) -> tuple[Any, ...]: + """Native pose formats available without extracting or converting body state.""" + if PhysxManager._fabric is not None: + return SceneDataFormat.Transform, SceneDataFormat.FabricMatrix44 + return (SceneDataFormat.Transform,) + @property def transforms(self) -> SceneDataFormat.Transform: - """Return the current PhysX rigid body transforms as :class:`SceneDataFormat.Transform`.""" - if view := self.get_rigid_body_view(): - self._scene_data.transforms = view.get_transforms().view(wp.transformf) - return self._scene_data + """Publish native rigid-body poses [m, xyzw].""" + PhysxManager.pre_render() + if self._poses_version != self.transforms_version and (view := self.get_rigid_body_view()): + self._transforms.transforms = view.get_transforms().view(wp.transformf) + self._poses_version = self.transforms_version + return self._transforms @property def transform_count(self) -> int: @@ -379,6 +394,26 @@ def transform_paths(self) -> list[str]: return list(view.prim_paths) return [] + def get_transforms(self, output_format: Any) -> SceneDataFormat.Transform | SceneDataFormat.FabricMatrix44: + """Publish the requested native representation, refreshing only that representation.""" + if output_format is not SceneDataFormat.FabricMatrix44 or PhysxManager._fabric is None: + return self.transforms + PhysxManager.pre_render() + if self._fabric_version != self.transforms_version: + PhysxManager._fabric.force_update(0.0, 0.0) + if self._fabric_selection is None: + stage = usdrt.Usd.Stage.Attach(PhysxManager._stage_id) + self._fabric_selection = stage.SelectPrims( + require_applied_schemas=["PhysicsRigidBodyAPI"], + require_attrs=[(usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.Read)], + device=str(PhysicsManager._device), + ) + if self._fabric_selection.PrepareForReuse() or self._fabric_transforms.matrices is None: + self._fabric_transforms.matrices = wp.fabricarray(self._fabric_selection, "omni:fabric:worldMatrix") + self.transforms_version += 1 + self._fabric_version = self.transforms_version + return self._fabric_transforms + class PhysxManager(PhysicsManager): """Manages PhysX physics simulation lifecycle. @@ -395,6 +430,7 @@ class PhysxManager(PhysicsManager): _timeline: ClassVar[omni.timeline.ITimeline] = omni.timeline.get_timeline_interface() _event_bus: ClassVar[carb.eventdispatcher.IEventDispatcher] = carb.eventdispatcher.get_eventdispatcher() _scene_data_backend: ClassVar[PhysxSceneDataBackend | None] = None + _kinematics_dirty: ClassVar[bool] = False backend: ClassVar[PhysxBackend | None] = None """Borrowed native resource, available after physics warmup and released on stop.""" @@ -404,7 +440,6 @@ class PhysxManager(PhysicsManager): _stage_id: ClassVar[int] = -1 _subscriptions: ClassVar[dict[str, Any]] = {} _fabric: ClassVar[Any] = None - _update_fabric: ClassVar[Callable[[float, float], None] | None] = None _anim_recorder: ClassVar[AnimationRecorder | None] = None _callback_exception: ClassVar[Exception | None] = None @@ -447,6 +482,7 @@ def initialize(cls, sim_context: SimulationContext) -> None: cls._load_fabric() cls._anim_recorder = AnimationRecorder(sim_context) cls._scene_data_backend = PhysxSceneDataBackend() + cls._kinematics_dirty = False # force update cycle to apply dt sim = PhysicsManager._sim @@ -499,16 +535,32 @@ def reset(cls, soft: bool = False) -> None: if cls.backend is not None: cls.backend.simulation_view._backend.initialize_kinematic_bodies() + cls.invalidate_transforms(kinematics=True) cls.raise_callback_exception_if_any() @classmethod def forward(cls) -> None: """Update articulation kinematics and fabric for rendering.""" sim = PhysicsManager._sim - if cls._fabric is not None and cls._update_fabric is not None: - if cls.backend is not None and sim is not None and sim.is_playing(): - cls.backend.simulation_view.update_articulations_kinematic() - cls._update_fabric(0.0, 0.0) + if cls.backend is not None and sim is not None and sim.is_playing(): + cls.backend.simulation_view.update_articulations_kinematic() + cls._kinematics_dirty = False + cls.invalidate_transforms() + if cls._fabric is not None: + cls._scene_data_backend.get_transforms(SceneDataFormat.FabricMatrix44) + + @classmethod + def invalidate_transforms(cls, *, kinematics: bool = False) -> None: + """Invalidate both native pose representations after writes; defer FK when needed.""" + cls._kinematics_dirty |= kinematics + cls._scene_data_backend.transforms_version += 1 + + @classmethod + def pre_render(cls) -> None: + """Complete pending pose writes before SDP publishes articulation transforms.""" + if cls._kinematics_dirty and cls.backend is not None: + cls.backend.simulation_view.update_articulations_kinematic() + cls._kinematics_dirty = False @classmethod def get_scene_data_backend(cls) -> SceneDataBackend: @@ -535,6 +587,8 @@ def step(cls) -> None: physx_sim = omni.physx.get_physx_simulation_interface() physx_sim.simulate(sim.cfg.dt, 0.0) physx_sim.fetch_results() + cls._kinematics_dirty = False + cls.invalidate_transforms() device = PhysicsManager._device if "cuda" in device: torch.cuda.set_device(device) @@ -589,8 +643,9 @@ def _sync_fabric_after_resume(cls) -> None: cls._re_sync_fabric() if cls.backend is not None: cls.backend.simulation_view.update_articulations_kinematic() - if cls._update_fabric is not None: - cls._update_fabric(0.0, 0.0) + cls._kinematics_dirty = False + if cls._fabric is not None: + cls._fabric.force_update(0.0, 0.0) @classmethod def close(cls) -> None: @@ -610,11 +665,11 @@ def close(cls) -> None: cls._event_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": "/"}) cls._fabric = None - cls._update_fabric = None cls._anim_recorder = None cls._warmup_needed = True cls._assets_loaded = True cls._callback_exception = None + cls._kinematics_dirty = False super().close() @@ -879,12 +934,10 @@ def _load_fabric(cls) -> None: from omni.physxfabric import get_physx_fabric_interface cls._fabric = get_physx_fabric_interface() - cls._update_fabric = getattr(cls._fabric, "force_update", cls._fabric.update) else: if ext_mgr.is_extension_enabled("omni.physx.fabric"): ext_mgr.set_extension_enabled_immediate("omni.physx.fabric", False) cls._fabric = None - cls._update_fabric = None # disable usd sync when fabric is enabled (via SettingsManager) for key in [ diff --git a/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py b/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py new file mode 100644 index 000000000000..5d8f1e6b8231 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py @@ -0,0 +1,126 @@ +# 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-owned Fabric resource shared by Kit and Isaac RTX.""" + +from __future__ import annotations + +from dataclasses import field + +import warp as wp + +import usdrt +import usdrt.hierarchy +from pxr import Usd, UsdUtils + +from isaaclab.scene_data import SceneDataFormat, SceneDataProvider +from isaaclab.sim import BackendCfg +from isaaclab.utils import configclass + + +@wp.kernel(enable_backward=False) +def _capture_scales( + matrices: wp.fabricarray(dtype=wp.mat44d), + indices: wp.fabricarray(dtype=wp.int32), + scales: wp.array(dtype=wp.vec3f), +): + i = wp.tid() + matrix = wp.mat44f(matrices[i]) + scales[indices[i]] = wp.vec3f( + wp.length(wp.vec3f(matrix[0, 0], matrix[0, 1], matrix[0, 2])), + wp.length(wp.vec3f(matrix[1, 0], matrix[1, 1], matrix[1, 2])), + wp.length(wp.vec3f(matrix[2, 0], matrix[2, 1], matrix[2, 2])), + ) + + +class FabricBackend: + """Own one stage's native Fabric handles and shared transform bindings. + + Consumers supply the simulation's SDP explicitly. Its producer layout stays fixed for the + binding's lifetime; it is not part of the native stage/device identity. + """ + + def __init__(self, cfg: FabricBackendCfg): + self.device = cfg.device + self.stage = usdrt.Usd.Stage.Attach(UsdUtils.StageCache.Get().GetId(cfg.stage).ToLongInt()) + self.hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( + self.stage.GetFabricId(), self.stage.GetStageIdAsStageId() + ) + self.transforms: SceneDataFormat.FabricMatrix44 | None = None + self._selection = self._write_selection = None + self._mapping = self._scales = None + self._version = -1 + + def bind_transforms(self, provider: SceneDataProvider) -> None: + """Bind the initialized simulation's rigid destinations once; native Fabric needs no conversion binding.""" + if self.transforms is not None: + return + if SceneDataFormat.FabricMatrix44 in provider.backend.native_transform_formats: + self.transforms = SceneDataFormat.FabricMatrix44() + return + + stage = self.stage + stage.SynchronizeToFabric() + self.hierarchy.update_world_xforms() + for index, path in enumerate(provider.backend.transform_paths): + prim = stage.GetPrimAtPath(path) + if not prim or not prim.HasAPI("PhysicsRigidBodyAPI"): + continue + prim.CreateAttribute("isaaclab:transformIndex", usdrt.Sdf.ValueTypeNames.Int, custom=True).Set(index) + # Physics publishes absolute body poses; only visual descendants inherit them. + self.hierarchy.set_reset_xform_stack(prim.GetPath().fabricPath, True) + attrs = [ + (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.Read), + (usdrt.Sdf.ValueTypeNames.Int, "isaaclab:transformIndex", usdrt.Usd.Access.Read), + (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:localMatrix", usdrt.Usd.Access.Read), + ] + self._selection = stage.SelectPrims(require_attrs=attrs, device=self.device) + self._write_selection = stage.SelectPrims( + require_attrs=[*attrs[:-1], (*attrs[-1][:2], usdrt.Usd.Access.ReadWrite)], device=self.device + ) + self._scales = wp.empty(provider.transform_count, dtype=wp.vec3f, device=self.device) + self.transforms = SceneDataFormat.FabricMatrix44() + + def update_transforms(self, provider: SceneDataProvider) -> None: + """Request SDP transforms and propagate converted body matrices to visual descendants.""" + self.bind_transforms(provider) + changed = self._selection is not None and self._selection.PrepareForReuse() + if self._selection is not None and (changed or self.transforms.matrices is None): + self._write_selection.PrepareForReuse() + self._mapping = wp.fabricarray(self._selection, "isaaclab:transformIndex") + if self.transforms.matrices is None: + wp.launch( + _capture_scales, + dim=len(self._mapping), + inputs=[wp.fabricarray(self._selection, "omni:fabric:worldMatrix"), self._mapping], + outputs=[self._scales], + device=self._scales.device, + ) + self.transforms = SceneDataFormat.FabricMatrix44() + self.transforms.matrices = wp.fabricarray(self._write_selection, "omni:fabric:localMatrix") + provider.get_transforms(self.transforms, self._mapping, scales=self._scales) + version = provider.backend.transforms_version + if self._selection is not None and (changed or self._version != version): + self._write_selection.PrepareForReuse() + device = self._scales.device + wp.synchronize_stream(device) + if not self.hierarchy.update_world_xforms_gpu(not changed and self._version != -1): + raise RuntimeError("Fabric GPU transform hierarchy update failed.") + wp.synchronize_device(device) + self._version = version + + def close(self) -> None: + """Release stage-bound selections and borrowed SDP buffers.""" + self.transforms = self._selection = self._write_selection = None + self._mapping = self._scales = self.hierarchy = self.stage = None + + +@configclass +class FabricBackendCfg(BackendCfg): + """Native Fabric identity; the stage is borrowed from the active simulation.""" + + class_type: type = FabricBackend + stage: Usd.Stage = field(kw_only=True, metadata={"copy": False}) + device: str = field(kw_only=True) diff --git a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py index 1e4d8eee058f..011e0b55f093 100644 --- a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py +++ b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py @@ -23,6 +23,7 @@ from isaaclab.app.settings_manager import get_settings_manager from isaaclab.renderers import BaseRenderer, RenderBufferKind, RenderBufferSpec from isaaclab.renderers.camera_render_spec import CameraRenderSpec +from isaaclab.sim import SimulationContext from isaaclab.sim.utils import enable_extension from isaaclab.utils.version import get_isaac_sim_version from isaaclab.utils.warp.kernels import reshape_tiled_image @@ -195,6 +196,12 @@ def __init__(self, cfg: IsaacRtxRendererCfg): ensure_rtx_hydra_engine_attached() # ``/isaaclab/render/rtx_sensors`` is owned by ``Camera.__init__`` (must be set pre-``sim.reset()``). + def initialize(self) -> None: + """Bind shared Fabric destinations after scene creation.""" + sim = SimulationContext.instance() + self._fabric = sim.get_or_create_backend(sim.fabric_cfg) + self._fabric.bind_transforms(sim.get_scene_data_provider()) + @property def visual_material_writer(self): """Write material channels directly through Fabric.""" @@ -571,9 +578,8 @@ def set_outputs(self, render_data: IsaacRtxRenderData, output_data: dict[str, Pr ) def update_transforms(self) -> None: - """No-op for Isaac RTX - uses USD scene directly. - See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.update_transforms`.""" - pass + """Update shared Fabric transforms and propagate the visual hierarchy.""" + self._fabric.update_transforms(SimulationContext.instance().get_scene_data_provider()) def update_geometries(self) -> None: """No-op for Isaac RTX - uses USD scene directly. diff --git a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py index f0d893dddb0d..67d487e4ae56 100644 --- a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py +++ b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py @@ -219,32 +219,21 @@ def ensure_isaac_rtx_render_update(force: bool = False) -> None: if sim is None: return - render_generation = getattr(sim, "render_generation", getattr(sim, "_render_generation", 0)) - key = (id(sim), sim._physics_step_count, render_generation) + key = (id(sim), sim.get_physics_step_count(), sim.render_generation) if _last_render_update_key == key: return # Already pumped this step (by another camera or a visualizer) - # If a visualizer already pumps the Kit app loop, mark as done and skip. - # However, on the very first call for a new SimulationContext, the visualizer - # has not had a chance to pump yet (sim.render() was never called), so we - # must perform the initial app.update() ourselves to populate annotator buffers. + # Prime annotators once; afterward the Kit visualizer owns its app updates. first_call_for_sim = _last_render_update_key[0] != id(sim) if not first_call_for_sim and any(viz.pumps_app_update() for viz in sim.visualizers): _last_render_update_key = key return - # Pump when continuous rendering is active (GUI/RTX sensors/visualizers/XR). ``is_rendering`` - # excludes headless offscreen rendering so the per-step loop does not pump between frames. - # Offscreen frames are produced on demand: the ``--video`` / ``rgb_array`` path calls this with - # ``force=True`` (see :func:`pump_kit_app_for_headless_video_render_if_needed`) to pump exactly - # when a frame is requested, without making every step pump. + # Headless offscreen capture requests a frame explicitly with force=True. if not force and not sim.is_rendering: return - # Sync physics results → Fabric so RTX sees updated positions. - # physics_manager.step() only runs simulate()/fetch_results() and does NOT - # call _update_fabric(), so without this the render would lag one frame behind. - sim.physics_manager.forward() + sim.get_or_create_backend(sim.fabric_cfg).update_transforms(sim.get_scene_data_provider()) import omni.kit.app diff --git a/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py b/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py index 9703728b5121..34c05e227800 100644 --- a/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py +++ b/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py @@ -19,10 +19,13 @@ from packaging import version from isaaclab.renderers import RenderBufferKind, RenderBufferSpec +from isaaclab.sim import SimulationContext from isaaclab.utils.renderers import ISAAC_RTX_SHOW_ALL_PARTITIONS_BY_DEFAULT_SETTING def _install_omni_stubs(monkeypatch): + sim = SimpleNamespace(stage=object(), device="cpu", get_scene_data_provider=MagicMock()) + monkeypatch.setattr(SimulationContext, "instance", lambda: sim) omni_module = sys.modules.get("omni", types.ModuleType("omni")) replicator_module = types.ModuleType("omni.replicator") replicator_core_module = types.ModuleType("omni.replicator.core") @@ -264,24 +267,6 @@ def _create_attribute(name, value_type): assert global_setting_calls == [] -def test_render_product_uuid_name_format_is_sdf_safe(): - """``rp_{uuid4().hex}`` matches the create_render_data naming contract and is SDF-safe.""" - import uuid - - from pxr import Sdf - - names = [f"rp_{uuid.uuid4().hex}" for _ in range(64)] - assert len(set(names)) == len(names) - for name in names: - assert name.startswith("rp_") - hex_part = name.removeprefix("rp_") - assert len(hex_part) == 32 - int(hex_part, 16) # raises if not hex - assert "-" not in name - assert Sdf.Path.IsValidIdentifier(name) - assert Sdf.Path.IsValidPathString(f"/Render/{name}") - - @pytest.mark.parametrize( ("has_gui", "expected_disable_color_render"), [ diff --git a/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_utils.py b/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_utils.py index a36a888a4f35..4c7878231184 100644 --- a/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_utils.py +++ b/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_utils.py @@ -3,11 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Unit tests for RTX streaming wait helpers. - -Covers callback state updates, subscription behavior, and timeout-aware wait -logic in :mod:`isaaclab_physx.renderers.isaac_rtx_renderer_utils`. -""" +"""Unit tests for RTX streaming waits and render-update cadence.""" from __future__ import annotations @@ -32,7 +28,6 @@ # test-specific timeout overrides for _STREAMING_WAIT_TIMEOUT_S STREAMING_TIMEOUT_S = 0.1 -STREAMING_TIMEOUT_SHORT_S = 0.01 # simulated per-update sleep to advance wall-clock time inside the wait loop MOCK_UPDATE_SLEEP_S = 0.02 @@ -80,54 +75,29 @@ def mock_omni_kit_app(): yield mock_module -# --------------------------------------------------------------------------- -# _get_stage_streaming_busy -# --------------------------------------------------------------------------- - - -class TestGetStageStreamingBusy: - """Synchronous streaming status query delegates to UsdContext.""" - - def test_returns_true_when_busy(self, mock_omni_usd): - mock_ctx = MagicMock() - mock_ctx.get_stage_streaming_status.return_value = True - mock_omni_usd.get_context.return_value = mock_ctx - assert rtx_utils._get_stage_streaming_busy() is True - - def test_returns_false_when_idle(self, mock_omni_usd): - mock_ctx = MagicMock() - mock_ctx.get_stage_streaming_status.return_value = False - mock_omni_usd.get_context.return_value = mock_ctx - assert rtx_utils._get_stage_streaming_busy() is False - - def test_returns_false_when_no_context(self, mock_omni_usd): - mock_omni_usd.get_context.return_value = None - assert rtx_utils._get_stage_streaming_busy() is False - - # --------------------------------------------------------------------------- # _wait_for_streaming_complete # --------------------------------------------------------------------------- class TestWaitForStreamingComplete: - """Blocking wait pumps app.update() while busy and respects timeout. + """Blocking wait pumps app.update() while busy and respects timeout.""" - These tests patch ``_get_stage_streaming_busy`` at the module level so - they don't depend on ``omni.usd`` being importable. - """ - - def test_returns_immediately_when_not_busy(self, mock_omni_kit_app): - """Skips loop and issues only the final update when idle.""" + @pytest.mark.parametrize("has_context", [False, True]) + def test_returns_immediately_when_not_busy(self, mock_omni_usd, mock_omni_kit_app, has_context): + """Idle and absent stages need only the final update.""" mock_app = MagicMock() mock_omni_kit_app.get_app.return_value = mock_app + context = mock_omni_usd.get_context.return_value + context.get_stage_streaming_status.return_value = False + if not has_context: + mock_omni_usd.get_context.return_value = None - with patch.object(rtx_utils, "_get_stage_streaming_busy", return_value=False): - rtx_utils._wait_for_streaming_complete() + rtx_utils._wait_for_streaming_complete() mock_app.update.assert_called_once() - def test_pumps_updates_until_idle(self, mock_omni_kit_app): + def test_pumps_updates_until_idle(self, mock_omni_usd, mock_omni_kit_app): """Pumps updates until streaming reports idle.""" mock_app = MagicMock() mock_omni_kit_app.get_app.return_value = mock_app @@ -141,13 +111,13 @@ def _count_update(): loop_calls += 1 mock_app.update.side_effect = _count_update + mock_omni_usd.get_context.return_value.get_stage_streaming_status.side_effect = _streaming_status - with patch.object(rtx_utils, "_get_stage_streaming_busy", side_effect=_streaming_status): - rtx_utils._wait_for_streaming_complete() + rtx_utils._wait_for_streaming_complete() assert mock_app.update.call_count == MOCK_ITERATIONS_BEFORE_IDLE + 1 - def test_respects_timeout(self, monkeypatch, mock_omni_kit_app): + def test_respects_timeout(self, monkeypatch, mock_omni_kit_app, caplog): """Exits wait loop on timeout if busy never clears.""" monkeypatch.setattr(rtx_utils, "_STREAMING_WAIT_TIMEOUT_S", STREAMING_TIMEOUT_S) mock_app = MagicMock() @@ -158,48 +128,7 @@ def test_respects_timeout(self, monkeypatch, mock_omni_kit_app): rtx_utils._wait_for_streaming_complete() assert mock_app.update.call_count > 0 - - def test_timeout_logs_warning(self, monkeypatch, mock_omni_kit_app): - """Logs warning when timeout is reached while still busy.""" - monkeypatch.setattr(rtx_utils, "_STREAMING_WAIT_TIMEOUT_S", STREAMING_TIMEOUT_SHORT_S) - mock_app = MagicMock() - mock_omni_kit_app.get_app.return_value = mock_app - mock_logger = MagicMock() - - with ( - patch.object(rtx_utils, "_get_stage_streaming_busy", return_value=True), - patch.object(rtx_utils, "logger", mock_logger), - ): - rtx_utils._wait_for_streaming_complete() - - mock_logger.warning.assert_called_once() - assert "RTX streaming did not complete within" in mock_logger.warning.call_args[0][0] - - def test_logs_info_on_non_trivial_completion(self, mock_omni_kit_app): - """Logs completion info when streaming finishes after delay.""" - mock_app = MagicMock() - mock_omni_kit_app.get_app.return_value = mock_app - mock_logger = MagicMock() - call_count = 0 - - def _streaming_status(): - return call_count < 1 - - def _become_idle_after_delay(): - nonlocal call_count - time.sleep(MOCK_UPDATE_SLEEP_S) - call_count += 1 - - mock_app.update.side_effect = _become_idle_after_delay - - with ( - patch.object(rtx_utils, "_get_stage_streaming_busy", side_effect=_streaming_status), - patch.object(rtx_utils, "logger", mock_logger), - ): - rtx_utils._wait_for_streaming_complete() - - mock_logger.info.assert_called_once() - assert "RTX streaming completed in" in mock_logger.info.call_args[0][0] + assert "RTX streaming did not complete within" in caplog.text # --------------------------------------------------------------------------- @@ -219,6 +148,7 @@ def mock_sim(self): """A minimal mock of :class:`SimulationContext`.""" sim = MagicMock() sim._physics_step_count = 0 + sim.get_physics_step_count.side_effect = lambda: sim._physics_step_count sim._render_generation = 0 sim.render_generation = 0 sim.is_rendering = True @@ -239,41 +169,19 @@ def mock_sim_context(self, monkeypatch): monkeypatch.setattr(rtx_utils, "sim_utils", types.SimpleNamespace(SimulationContext=sim_context)) return sim_context - def test_first_call_with_visualizer_still_pumps( - self, mock_sim, mock_sim_context, pumping_visualizer, mock_omni_kit_app - ): - """Regression: first call for a new sim must pump even with a visualizer. - - Without the fix (commit 2e8ace7), a visualizer returning - ``pumps_app_update() == True`` caused the function to skip - ``app.update()`` on the very first call. The visualizer had not - pumped yet (``sim.render()`` was never called), so annotator - buffers were never populated and cameras hung waiting for data. - """ - mock_sim.visualizers = [pumping_visualizer] - mock_app = MagicMock() - mock_omni_kit_app.get_app.return_value = mock_app - mock_sim_context.instance.return_value = mock_sim - - with ( - patch.object(rtx_utils, "_get_stage_streaming_busy", return_value=False), - ): - rtx_utils.ensure_isaac_rtx_render_update() - - mock_app.update.assert_called_once() - - def test_second_call_with_visualizer_skips_pump( + def test_visualizer_pumps_only_after_initial_render_update( self, mock_sim, mock_sim_context, pumping_visualizer, mock_omni_kit_app ): - """After the first call, a visualizer that pumps causes the skip.""" + """Publish the first frame before yielding app updates to an active visualizer.""" mock_sim.visualizers = [pumping_visualizer] mock_app = MagicMock() mock_omni_kit_app.get_app.return_value = mock_app mock_sim_context.instance.return_value = mock_sim + provider = mock_sim.get_scene_data_provider.return_value + update_transforms = mock_sim.get_or_create_backend.return_value.update_transforms + mock_app.update.side_effect = lambda: update_transforms.assert_called_once_with(provider) - with ( - patch.object(rtx_utils, "_get_stage_streaming_busy", return_value=False), - ): + with patch.object(rtx_utils, "_get_stage_streaming_busy", return_value=False): rtx_utils.ensure_isaac_rtx_render_update() mock_app.update.assert_called_once() mock_app.update.reset_mock() @@ -282,6 +190,9 @@ def test_second_call_with_visualizer_skips_pump( rtx_utils.ensure_isaac_rtx_render_update() mock_app.update.assert_not_called() + mock_sim.get_or_create_backend.assert_called_once_with(mock_sim.fabric_cfg) + update_transforms.assert_called_once_with(provider) + mock_sim.physics_manager.forward.assert_not_called() def test_no_sim_is_noop(self, mock_sim_context, mock_omni_kit_app): """No-op when SimulationContext.instance() returns None.""" @@ -310,13 +221,17 @@ def test_dedup_same_step(self, mock_sim, mock_sim_context, mock_omni_kit_app): mock_app.update.assert_not_called() - def test_not_rendering_skips(self, mock_sim, mock_sim_context, mock_omni_kit_app): - """No ``app.update()`` when rendering is disabled.""" + @pytest.mark.parametrize("force", [False, True]) + def test_not_rendering_pumps_only_when_forced(self, mock_sim, mock_sim_context, mock_omni_kit_app, force): + """Offscreen capture publishes through SDP only when a frame is requested.""" mock_sim.is_rendering = False mock_app = MagicMock() mock_omni_kit_app.get_app.return_value = mock_app mock_sim_context.instance.return_value = mock_sim - rtx_utils.ensure_isaac_rtx_render_update() + with patch.object(rtx_utils, "_get_stage_streaming_busy", return_value=False): + rtx_utils.ensure_isaac_rtx_render_update(force=force) - mock_app.update.assert_not_called() + assert mock_app.update.call_count == int(force) + assert mock_sim.get_or_create_backend.return_value.update_transforms.call_count == int(force) + mock_sim.physics_manager.forward.assert_not_called() diff --git a/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py b/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py index 467f94461fad..6f0874866d47 100644 --- a/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py +++ b/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py @@ -4,13 +4,107 @@ # SPDX-License-Identifier: BSD-3-Clause from types import SimpleNamespace +from unittest.mock import Mock +import numpy as np import pytest +import warp as wp pytest.importorskip("pxr") pytest.importorskip("omni.physics.tensors") +@pytest.mark.parametrize("operation", ["step", "forward"]) +def test_pose_publication_refreshes_after_physics_but_reuses_clean_reads(monkeypatch, operation): + """SDP borrows native poses once per dirty generation and completes pending joint writes.""" + from isaaclab_physx.physics import physx_manager + + from isaaclab.physics import PhysicsManager + from isaaclab.scene_data import SceneDataFormat, SceneDataProvider + + manager = physx_manager.PhysxManager + fabric = Mock() + monkeypatch.setattr(manager, "_fabric", fabric) + backend = physx_manager.PhysxSceneDataBackend() + transforms = wp.zeros(1, dtype=wp.transformf, device="cpu") + view = Mock(count=1, get_transforms=Mock(return_value=transforms)) + backend._rigid_body_view = view + monkeypatch.setattr(backend, "get_rigid_body_view", Mock(wraps=backend.get_rigid_body_view)) + sim_view = Mock() + monkeypatch.setattr(manager, "backend", SimpleNamespace(simulation_view=sim_view)) + monkeypatch.setattr(manager, "_scene_data_backend", backend) + monkeypatch.setattr(manager, "_kinematics_dirty", False) + monkeypatch.setattr(manager, "_anim_recorder", None) + monkeypatch.setattr( + PhysicsManager, "_sim", SimpleNamespace(stage=object(), cfg=SimpleNamespace(dt=0.01), is_playing=lambda: True) + ) + monkeypatch.setattr(PhysicsManager, "_device", "cpu") + monkeypatch.setattr(physx_manager.omni.physx, "get_physx_simulation_interface", Mock(return_value=Mock())) + provider = SceneDataProvider(backend) + fabric_matrices = wp.zeros(1, dtype=wp.mat44d, device="cpu") + backend._fabric_selection = SimpleNamespace( + PrepareForReuse=Mock(return_value=False), + __fabric_arrays_interface__={ + "version": 1, + "device": "cpu", + "attribs": { + "omni:fabric:worldMatrix": { + "type": (True, "f8", 16, 0, "matrix"), + "access": 1, + "pointers": [fabric_matrices.ptr], + "counts": [1], + } + }, + }, + ) + assert provider.get_transforms(SceneDataFormat.FabricMatrix44()) + version = backend.transforms_version + assert provider.get_transforms(SceneDataFormat.FabricMatrix44()) + fabric.force_update.assert_called_once_with(0.0, 0.0) + backend.get_rigid_body_view.assert_not_called() + view.get_transforms.assert_not_called() + assert backend.transforms_version == version + native = SceneDataFormat.Transform() + assert provider.get_transforms(native) + assert native.transforms.ptr == transforms.ptr + output = SceneDataFormat.Matrix44() + assert provider.get_transforms(output) + matrices = output.matrices + view.get_transforms.assert_called_once_with() + + transforms.fill_(wp.transformf(wp.vec3f(1, 2, 3), wp.quat_identity())) + getattr(manager, operation)() + assert backend.transforms_version > version + version = backend.transforms_version + manager.pre_render() + manager.pre_render() + assert sim_view.update_articulations_kinematic.call_count == int(operation == "forward") + assert provider.get_transforms(output) + assert output.matrices is matrices + np.testing.assert_array_equal(matrices.numpy()[0, :3, 3], [1, 2, 3]) + assert view.get_transforms.call_count == 2 + provider.get_transforms(SceneDataFormat.FabricMatrix44()) + provider.get_transforms(SceneDataFormat.FabricMatrix44()) + assert fabric.force_update.call_count == 2 + + transforms.fill_(wp.transformf(wp.vec3f(4, 5, 6), wp.quat_identity())) + manager.invalidate_transforms(kinematics=True) + assert backend.transforms_version > version + version = backend.transforms_version + provider.get_transforms(SceneDataFormat.FabricMatrix44()) + provider.get_transforms(SceneDataFormat.FabricMatrix44()) + assert sim_view.update_articulations_kinematic.call_count == 1 + int(operation == "forward") + assert fabric.force_update.call_count == 3 + assert backend.transforms_version == version + provider.get_transforms(native) + assert provider.get_transforms(output) + np.testing.assert_array_equal(output.matrices.numpy()[0, :3, 3], [4, 5, 6]) + assert view.get_transforms.call_count == 3 + assert backend.transforms_version == version + backend.clear() + assert backend.transforms_version > version + + @pytest.mark.parametrize("joint_has_rigid_body_api", [False, True]) def test_rigid_body_view_uses_exact_path_for_joint_name_collision(monkeypatch, joint_has_rigid_body_api): """Joint names must keep same-named rigid bodies out of wildcard views.""" diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index e719dea8462f..2eb6583faac8 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -26,11 +26,13 @@ import warp as wp # noqa: E402 from frame_view_contract_utils import * # noqa: F401, F403, E402 from frame_view_contract_utils import CHILD_OFFSET, ViewBundle # noqa: E402, F401 +from isaaclab_physx.physics import PhysxCfg # noqa: E402 from isaaclab_physx.sim.views import FabricFrameView as FrameView # noqa: E402 -from pxr import Gf, UsdGeom # noqa: E402 +from pxr import Gf, UsdGeom, UsdPhysics # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 +from isaaclab.scene_data import SceneDataFormat, SceneDataProvider # noqa: E402 pytestmark = pytest.mark.isaacsim_ci PARENT_POS = (0.0, 0.0, 1.0) @@ -135,6 +137,50 @@ def factory(num_envs: int, device: str) -> ViewBundle: # ------------------------------------------------------------------ +@pytest.mark.parametrize("device", [device for device in test_devices() if device.startswith("cuda")]) +def test_sdp_native_gpu_fabric_binding_preserves_live_physx_pose(device, request): + """Native GPU binding preserves live poses and publishes same-step writes without forward().""" + _skip_if_unavailable(device) + prim = UsdGeom.Cube.Define(sim_utils.get_current_stage(), "/World/Cube").GetPrim() + UsdPhysics.RigidBodyAPI.Apply(prim) + UsdPhysics.CollisionAPI.Apply(prim) + sim = sim_utils.SimulationContext( + sim_utils.SimulationCfg(physics=PhysxCfg(), device=device, gravity=(0, 0, 0), use_fabric=True) + ) + sim.set_setting("/physics/fabricUpdateTransformations", True) + sim.reset() + frame_view = FrameView("/World/Cube", device=device) + request.addfinalizer(frame_view.close) + frame_view.get_world_poses() # Initialize its authored pose before the native pose write. + view = sim.physics_manager.get_physics_sim_view().create_rigid_body_view("/World/Cube") + view.set_transforms( + wp.array([[1, 2, 3, 0, 0, 0, 1]], dtype=wp.float32, device=device), + indices=wp.array([0], dtype=wp.int32, device=device), + ) + sim.step(render=False) + sim.forward() + before = tuple(value.torch.clone() for value in frame_view.get_world_poses()) + torch.testing.assert_close(before[0], torch.tensor([[1, 2, 3]], dtype=torch.float32, device=device)) + provider = SceneDataProvider(sim.get_scene_data_provider().backend) + output = SceneDataFormat.FabricMatrix44() + assert provider.get_transforms(output) + assert output.matrices.shape == (1,) + for value, expected in zip(frame_view.get_world_poses(), before, strict=True): + torch.testing.assert_close(value.torch, expected, rtol=0, atol=0) + + step_count = sim.get_physics_step_count() + view.set_transforms( + wp.array([[-2, 0.5, 4, 0, 0, 0, 1]], dtype=wp.float32, device=device), + indices=wp.array([0], dtype=wp.int32, device=device), + ) + sim.physics_manager.invalidate_transforms() + provider.get_transforms(output) + torch.testing.assert_close( + frame_view.get_world_poses()[0].torch, torch.tensor([[-2, 0.5, 4]], dtype=torch.float32, device=device) + ) + assert sim.get_physics_step_count() == step_count + + @pytest.mark.parametrize("device", test_devices()) def test_float_scale_initializes_fabric(device): """A legal float3 scale initializes Fabric without changing the FP32 view contract.""" @@ -317,23 +363,6 @@ def test_writer_scope_exception_recovers_state(device, view_factory): assert torch.allclose(follow_up_t, torch.tensor([[10.0, 11.0, 12.0]] * 2, device=device), atol=1e-5) -@pytest.mark.parametrize("device", ["cuda:0"]) -def test_prepare_for_reuse_detects_topology_change(device, view_factory): - """Each persistent ``PrimSelection`` exposes ``PrepareForReuse`` and returns a - bool. When the underlying Fabric topology is unchanged it returns False. - """ - bundle = view_factory(1, device) - view = bundle.view - view.get_world_poses() # trigger Fabric init - - assert view._fabric_sel.sel_ro is not None, "RO selection not initialized" - assert view._fabric_sel.sel_rw is not None, "RW selection not initialized" - for selection in (view._fabric_sel.sel_ro, view._fabric_sel.sel_rw): - result = selection.PrepareForReuse() - assert isinstance(result, bool), f"PrepareForReuse should return bool, got {type(result)}" - assert not result, "PrepareForReuse should return False when no topology change" - - @pytest.mark.parametrize("device", test_devices()) def test_selections_match_only_the_view_prims(device, view_factory): """Selections contain only the managed child prims and their unique parents. diff --git a/source/isaaclab_visualizers/changelog.d/sdp-transform-publication.rst b/source/isaaclab_visualizers/changelog.d/sdp-transform-publication.rst new file mode 100644 index 000000000000..45b28bb33218 --- /dev/null +++ b/source/isaaclab_visualizers/changelog.d/sdp-transform-publication.rst @@ -0,0 +1,6 @@ +Changed +^^^^^^^ + +* Routed Kit viewport transform updates through SDP, sharing a registry-owned Fabric binding with camera + renderers and preserving native PhysX Fabric updates. No visualizer configuration changes were required. + Headless viewport transforms and asset tracking refreshed only when a frame was requested. diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py index fc9c4e6019bb..2b2602d14590 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py @@ -199,6 +199,9 @@ def initialize(self, scene_data_provider: SceneDataProvider) -> None: ) self._setup_streaming_view(num_envs) + sim = SimulationContext.instance() + self._fabric = sim.get_or_create_backend(sim.fabric_cfg) + self._fabric.bind_transforms(scene_data_provider) self._is_initialized = True self._setup_initial_camera_view() @@ -213,13 +216,13 @@ def step(self, dt: float) -> None: self._app_pumped_this_step = False self._sim_time += dt self._step_counter += 1 - # Update dynamic asset tracking before the frame renders. - if self.cfg.origin_type == "asset": - self._update_asset_tracking_camera() # Headless mode: skip the app update and camera panel refresh; rendering is # triggered on demand by render_rgb_array() / render_tiled_rgb_array(). if self._runtime_headless: return + self._fabric.update_transforms(self._scene_data_provider) + if self.cfg.origin_type == "asset": + self._update_asset_tracking_camera() _externally_paused = self.is_training_paused() if not _externally_paused: try: @@ -289,6 +292,9 @@ def render_rgb_array(self) -> np.ndarray: import omni.kit.app import omni.replicator.core as rep + self._fabric.update_transforms(self._scene_data_provider) + if self._runtime_headless and self.cfg.origin_type == "asset": + self._update_asset_tracking_camera() camera_path = self._controlled_camera_path or "/OmniverseKit_Persp" w, h = self.cfg.window_width, self.cfg.window_height @@ -418,10 +424,6 @@ def add_live_plots( if isinstance(source, DirectScalarLivePlots): self.kit_manager_visualizers[source.manager_name] = DirectScalarLiveVisualizer(source) - def requires_forward_before_step(self) -> bool: - """OV viewport relies on refreshed kinematic state before render.""" - return True - def pumps_app_update(self) -> bool: """KitVisualizer calls app.update() in step(), so render() should not do it again.""" return True @@ -1238,7 +1240,7 @@ def _setup_initial_camera_view(self) -> None: def _update_asset_tracking_camera(self) -> None: """Update the viewport camera to track an asset root or body. - Called every :meth:`step` when :attr:`KitVisualizerCfg.origin_type` is ``"asset"``. + Called before viewport frames when :attr:`KitVisualizerCfg.origin_type` is ``"asset"``. Parses :attr:`~KitVisualizerCfg.origin_track_path`: ``"asset_name"`` tracks the root, ``"asset_name/body_name"`` tracks a specific body. """ diff --git a/source/isaaclab_visualizers/test/test_kit_visualizer_scene_partitioning.py b/source/isaaclab_visualizers/test/test_kit_visualizer_scene_partitioning.py index 92c7186dfb1b..51a5f40a9106 100644 --- a/source/isaaclab_visualizers/test/test_kit_visualizer_scene_partitioning.py +++ b/source/isaaclab_visualizers/test/test_kit_visualizer_scene_partitioning.py @@ -19,6 +19,28 @@ from isaaclab.utils.renderers import ISAAC_RTX_SHOW_ALL_PARTITIONS_BY_DEFAULT_SETTING +@pytest.mark.parametrize("headless", [False, True]) +def test_viewport_pose_publication_is_deferred_for_headless_capture(monkeypatch, headless): + visualizer = KitVisualizer(KitVisualizerCfg(headless=headless, origin_type="asset")) + visualizer._is_initialized = True + visualizer._fabric = MagicMock() + visualizer._scene_data_provider = MagicMock() + monkeypatch.setattr(visualizer, "is_training_paused", lambda: True) + tracking = MagicMock() + monkeypatch.setattr(visualizer, "_update_asset_tracking_camera", tracking) + monkeypatch.setattr(visualizer, "_update_camera_image_panel", MagicMock()) + monkeypatch.setattr(visualizer, "_refresh_partial_viz_point_instancers_if_needed", MagicMock()) + + visualizer.step(0.1) + + assert tracking.call_count == int(not headless) + request = visualizer._fabric.update_transforms + if headless: + request.assert_not_called() + else: + request.assert_called_once_with(visualizer._scene_data_provider) + + @pytest.mark.parametrize("generated", [False, True]) def test_streaming_renderer_registers_before_visualizer_initialization(monkeypatch, generated): sim = MagicMock() diff --git a/source/isaaclab_visualizers/test/visualizer_golden_utils.py b/source/isaaclab_visualizers/test/visualizer_golden_utils.py index fdfb330ab905..0cf9a35b1277 100644 --- a/source/isaaclab_visualizers/test/visualizer_golden_utils.py +++ b/source/isaaclab_visualizers/test/visualizer_golden_utils.py @@ -388,7 +388,7 @@ def _capture_frame(env, viz_type: str, capture_mode: str, backend: str, actions: return _viz_utils._capture_visualizer_tiled_camera_rgb(_get_active_visualizer(env, viz_type)) if viz_type == "kit": return _viz_utils._capture_kit_viewport_with_pose_reapply( - env, _get_active_visualizer(env, "kit"), physics_backend=backend, prior_physics_steps=buffer_steps + env, _get_active_visualizer(env, "kit"), physics_backend=backend ) newton_viz = _get_active_visualizer(env, "newton") viewer = getattr(newton_viz, "_viewer", None) @@ -463,7 +463,6 @@ def _capture_frame(env, viz_type: str, capture_mode: str, backend: str, actions: _get_active_visualizer(env, "kit"), resolution=_viz_utils._SHADOW_HAND_KIT_INTEGRATION_RENDER_RESOLUTION, physics_backend=backend, - prior_physics_steps=0, ) newton_viz = _get_active_visualizer(env, "newton") viewer = getattr(newton_viz, "_viewer", None) @@ -539,7 +538,6 @@ def _capture_frame(env, viz_type: str, capture_mode: str, backend: str, actions: _get_active_visualizer(env, "kit"), resolution=_viz_utils._ANYMAL_D_KIT_INTEGRATION_RENDER_RESOLUTION, physics_backend=backend, - prior_physics_steps=_viz_utils._START_BUFFER_STEPS, ) newton_viz = _get_active_visualizer(env, "newton") viewer = getattr(newton_viz, "_viewer", None) @@ -602,13 +600,7 @@ def _capture_frame(env, viz_type: str, capture_mode: str, actions: torch.Tensor) if capture_mode == "tiled": return _viz_utils._capture_visualizer_tiled_camera_rgb(_get_active_visualizer(env, viz_type)) if viz_type == "kit": - # Do NOT call env.sim.render() here: the VBD cloth solver never sets - # NewtonManager._newton_fabric_ready, so env.sim.render() blocks in - # the Fabric sync path indefinitely on some GPU/driver combinations - # (observed 48+ min hang on RTX PRO 4500 Blackwell). Instead use - # app_updates_only=True which drives RTX TAA via lightweight app.update() - # ticks without triggering Newton Fabric sync. The 12%/SSIM-0.85 - # thresholds are loose enough to accept the resulting frame quality. + # Warm up RTX TAA without advancing the cloth simulation. return _viz_utils._capture_kit_viewport_with_pose_reapply( env, _get_active_visualizer(env, "kit"), diff --git a/source/isaaclab_visualizers/test/visualizer_integration_utils.py b/source/isaaclab_visualizers/test/visualizer_integration_utils.py index a7e95474820a..dc99ff79b925 100644 --- a/source/isaaclab_visualizers/test/visualizer_integration_utils.py +++ b/source/isaaclab_visualizers/test/visualizer_integration_utils.py @@ -142,13 +142,7 @@ """Hard cap on render frames pumped during convergence-based warmup.""" _FRANKA_CLOTH_KIT_VIEWPORT_WARMUP_FRAMES = 20 -"""Franka cloth kit-viewport warmup uses lightweight ``app.update()`` ticks -(not ``env.sim.render()``). Each ``env.sim.render()`` call for the VBD cloth -scene blocks in the Newton Fabric sync path (the VBD cloth solver never sets -``NewtonManager._newton_fabric_ready``), causing hangs on some GPU/driver -combinations. 20 ``app.update()`` ticks drive RTX TAA accumulation without -triggering the Fabric sync, producing an acceptable frame within the -loose 12% / SSIM-0.85 thresholds.""" +"""Bounded RTX TAA warmup for the Franka cloth viewport capture.""" _WARMUP_STABLE_DIFF_PCT = 0.5 """Fraction of pixels (%) with inter-frame L2 > 1.0 below which two consecutive frames are @@ -1060,78 +1054,29 @@ def _reapply_kit_camera_pose(env, kit_visualizer: KitVisualizer) -> None: _update_active_simulation_app() -def _force_newton_transforms_resync() -> None: - """Force-mark Newton body transforms and particles dirty and re-sync to USD Fabric. - - Needed when the Fabric SelectPrims check fails on a prior pre_render() call (GPU - attribute propagation delay), leaving dirty flags cleared without writing positions. - """ - with contextlib.suppress(Exception): - from isaaclab_newton.physics import NewtonManager # noqa: PLC0415 - - if NewtonManager._usdrt_stage is not None and NewtonManager.backend is not None: - NewtonManager._transforms_dirty = True - NewtonManager.sync_transforms_to_fabric() - NewtonManager._particles_dirty = True - NewtonManager.sync_particles_to_usd() - - -def _drain_until_newton_fabric_ready(max_updates: int = 200, updates_per_iter: int = 2) -> None: - """Pump Kit updates until Newton has written body positions to Fabric. - - Polls ``NewtonManager._newton_fabric_ready`` (set after the first successful - SelectPrims call) with real-time sleeps so the GPU can process pending Fabric work. - Returns immediately if already ready (common case after a normal physics warmup). - - The tiled-camera path uses ``max_updates=600`` safely (tiled cameras are not rendered - until ``camera_sensor.update()``); the viewport path keeps a lower ceiling to limit - contaminated TAA frames accumulating during the drain. - """ - with contextlib.suppress(Exception): - from isaaclab_newton.physics import NewtonManager # noqa: PLC0415 - - for _ in range(max(0, int(max_updates))): - if NewtonManager._newton_fabric_ready: - return - with contextlib.suppress(Exception): - import torch # noqa: PLC0415 - - if torch.cuda.is_available(): - torch.cuda.synchronize() - _force_newton_transforms_resync() - _drain_kit_app_updates(updates_per_iter) - - def _capture_kit_viewport_with_pose_reapply( env, kit_visualizer: KitVisualizer, resolution: tuple[int, int] | None = None, physics_backend: str = "", - prior_physics_steps: int = 0, max_warmup_frames: int | None = None, app_updates_only: bool = False, ) -> np.ndarray: """Set the configured eye/lookat, warm RTX, then capture. - Re-applies the camera between the two ``app.update()`` calls in the warmup loop so - that Newton stage init (which resets the viewport camera) does not affect the final - frame. When ``prior_physics_steps > 0``, also re-syncs Newton body transforms - between the two calls so the correct pose is rendered. + Re-applies the camera after rendering so Newton viewport initialization does not + change the captured viewpoint. The normal render path refreshes body transforms. Args: env: The simulation environment. kit_visualizer: The active :class:`KitVisualizer` instance. resolution: Optional ``(width, height)`` override for the render product. - physics_backend: ``"newton"`` to enable per-render camera reapply and body- - transform re-sync. - prior_physics_steps: When > 0, injects a Newton body-transform re-sync - between the two ``app.update()`` calls. + physics_backend: ``"newton"`` to enable per-render camera reapply. max_warmup_frames: When set, overrides the default convergence cap. Use a small value when per-frame render cost is very high and test thresholds are loose enough that convergence is not required (e.g. franka cloth RTX). - app_updates_only: When True, uses lightweight ``app.update()`` ticks instead - of ``env.sim.render()``. Required for VBD cloth scenes where - ``env.sim.render()`` blocks in the Newton Fabric sync path. + app_updates_only: When True, warms RTX with ``app.update()`` ticks instead + of ``env.sim.render()``. """ kit_visualizer.set_camera_view(kit_visualizer.cfg.eye, kit_visualizer.cfg.lookat) camera_path = getattr(kit_visualizer, "_controlled_camera_path", None) @@ -1139,14 +1084,11 @@ def _capture_kit_viewport_with_pose_reapply( annotator, render_product = _build_rgb_annotator_for_camera(camera_path, resolution=resolution) try: if physics_backend == "newton": - _drain_until_newton_fabric_ready() prev: np.ndarray | None = None for i in range(_WARMUP_MAX_FRAMES): kit_visualizer.set_camera_view(kit_visualizer.cfg.eye, kit_visualizer.cfg.lookat) env.sim.render() kit_visualizer.set_camera_view(kit_visualizer.cfg.eye, kit_visualizer.cfg.lookat) - if prior_physics_steps > 0: - _force_newton_transforms_resync() _update_active_simulation_app() with contextlib.suppress(Exception): annotator.get_data() @@ -1187,9 +1129,8 @@ def _warm_kit_rtx_render_product( satisfy :func:`_frames_converged` or :data:`_WARMUP_MAX_FRAMES` is reached. When ``max_frames_override`` is set, it replaces both caps above — useful when the per-frame render cost is high and loose thresholds make convergence unnecessary. - When ``app_updates_only`` is True, replaces ``env.sim.render()`` with lightweight - ``app.update()`` calls. Use this for VBD cloth scenes where ``env.sim.render()`` - blocks in the Newton Fabric sync path (VBD cloth particles never set the ready flag). + When ``app_updates_only`` is True, warms RTX with ``app.update()`` calls without + advancing the visualizers. """ if max_frames_override is not None: max_frames = max_frames_override @@ -1369,23 +1310,7 @@ def _capture_visualizer_tiled_camera_rgb( if force_recompute and getattr(visualizer, "_camera_is_owned", False): visualizer._update_owned_camera_poses() if isinstance(visualizer, KitVisualizer): - # Probe with a short drain to detect backend: on Newton, _newton_fabric_ready is set - # after the first iteration; on PhysX it is never set so we skip the full drain and - # let _pump_tiled_until_stable handle convergence instead. - _drain_until_newton_fabric_ready(max_updates=20, updates_per_iter=4) - try: - from isaaclab_newton.physics import NewtonManager # noqa: PLC0415 - - if NewtonManager._newton_fabric_ready: - if not paused: - _drain_until_newton_fabric_ready(max_updates=600, updates_per_iter=4) - _update_active_simulation_app() - if not paused: - _force_newton_transforms_resync() - else: - _update_active_simulation_app() - except Exception: - _update_active_simulation_app() + _update_active_simulation_app() return _pump_tiled_until_stable(camera_sensor, camera_indices) rgb_batch = camera_rgb_batch(camera_sensor, camera_indices) frame = compose_rgb_grid_tensor(rgb_batch).detach().cpu().numpy() @@ -1727,10 +1652,7 @@ def _make_anymal_d_env(visualizer_kind: str | tuple[str, ...], backend_kind: str _FRANKA_CLOTH_WARMUP_STEPS = 1 """Steps after reset before capturing the franka cloth scene. -One step lets Newton propagate articulation FK so all robot arm links are visible at the -correct positions. At 0 steps, Newton has not yet synced body positions to USD Fabric, -leaving the arm links at the origin and invisible in the Kit viewport. One step also lets -the cloth begin falling under gravity while remaining in a nearly-deterministic pose — the +One step lets the cloth begin falling under gravity while remaining in a nearly-deterministic pose — the VBD solver's non-deterministic parallel reductions accumulate over many steps, so capturing at 1 step keeps inter-run pixel variance much lower than at 20 steps. This mirrors the approach used in the kitless rendering tests in From c6ea3678405a27fdf82f21979630e6aa77127bca Mon Sep 17 00:00:00 2001 From: ooctipus Date: Wed, 23 Sep 2026 20:18:38 -0700 Subject: [PATCH 08/11] Add observation delay using the shared DelayBuffer (#7976) ## Description Observation terms lacked a built-in delay tied to recorded samples. Added `delay_min_lag`, `delay_max_lag`, and `delay_hold_prob` directly to `ObservationTermCfg`, using the same `DelayBuffer` as `DelayedPDActuator`: ```python joint_pos = ObservationTermCfg(func=mdp.joint_pos_rel, delay_min_lag=1, delay_max_lag=3, delay_hold_prob=0.8) ``` Each environment samples its lag uniformly at initialization/reset. On each recorded sample, `delay_hold_prob=0.8` retains the current lag with probability 0.8 and otherwise draws a new lag, which may equal the previous one. The default of 1.0 holds the lag for the episode, preserving Isaac Lab's delayed actuator policy; 0.0 redraws every recorded sample. Holding a lag keeps latency constant as frames advance. Equal bounds give a fixed delay; both zero disable it. The field names, lag-retention semantics, and placement follow mjlab: delay runs after modifiers, noise, clipping, and scaling, before history. `compute(update_history=True)` records a sample; extra `compute()` or `compute_group()` reads leave delay/history and lag sampling unchanged. Before the first recorded sample after reset, delay returns the current input; during warm-up it returns the oldest available sample. Observation lag counts recorded samples, while actuator lag retains its physics-step clock. `DelayBuffer` owns ring storage and optional lag sampling. Its default `hold_prob=None` preserves externally selected lags and `compute(data)` for existing actuator callers. Actuator configuration, execution, and reset behavior are preserved. Tests extend existing suites; the diff adds no runtime module or test file. Related: #3471. ## Validation - 145 core tests passed; one CUDA-graph case skipped on CPU. Covered delay/history recording, extra reads without RNG advancement, processing order, config validation, partial resets, output isolation, hold probabilities 0/0.5/1, and CUDA graph replay with lag sampling. - Four actuator reset integration tests passed, running Newton and PhysX separately. - Confirmed regression tests fail when extra reads advance delay or the lag-retention mask is removed. - Eight-file architecture audit and `uv run --no-sync isaaclab -f` passed. - CPU/CUDA buffer benchmarks at batch 1024, width 48, and history lengths 4/32/128 showed lower runtime than the base implementation's history shifting. Adding optional sampling kept the existing manually selected-lag path within timing variation and introduced no RNG draws. These are local microbenchmarks, not end-to-end simulation measurements. ## Type of change - New feature - Bug fix - Documentation update ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` ## Checklist - [x] Ran pre-commit checks. - [x] Updated public documentation and changelog. - [x] Extended existing tests. - [ ] Passed PR CI. --- docs/source/api/lab/isaaclab.managers.rst | 40 +++++ .../changelog.d/shared-delay-buffer.minor.rst | 17 ++ .../isaaclab/managers/manager_term_cfg.py | 36 +++++ .../isaaclab/managers/observation_manager.py | 58 +++++-- .../test/utils/actuator_equivalence.py | 43 +++-- .../isaaclab/utils/buffers/delay_buffer.py | 153 ++++++++++++------ .../managers/test_observation_manager_unit.py | 80 ++++++--- .../isaaclab/test/utils/test_delay_buffer.py | 150 ++++++++++------- 8 files changed, 412 insertions(+), 165 deletions(-) create mode 100644 source/isaaclab/changelog.d/shared-delay-buffer.minor.rst diff --git a/docs/source/api/lab/isaaclab.managers.rst b/docs/source/api/lab/isaaclab.managers.rst index 77af12ad7bcc..9663fb766324 100644 --- a/docs/source/api/lab/isaaclab.managers.rst +++ b/docs/source/api/lab/isaaclab.managers.rst @@ -54,6 +54,46 @@ Manager Base Observation Manager ------------------- +Observation delay +~~~~~~~~~~~~~~~~~ + +Configure observation delay directly on the term: + +.. code-block:: python + + from isaaclab.envs import mdp + from isaaclab.managers import ObservationTermCfg + + joint_pos = ObservationTermCfg(func=mdp.joint_pos_rel, delay_min_lag=1, delay_max_lag=3) + +Each environment samples its lag uniformly from the inclusive bounds at initialization and reset. +By default, ``delay_hold_prob=1.0`` keeps that lag for the episode, matching the reset-based lag policy of +:class:`~isaaclab.actuators.DelayedPDActuator`. Equal bounds give a constant delay; both zero disable delay. +Both use :class:`~isaaclab.utils.buffers.DelayBuffer`; observation lag counts recorded observation +samples, while actuator lag counts physics steps. + +Set ``delay_hold_prob`` below 1.0 to vary latency within an episode: + +.. code-block:: python + + joint_pos = ObservationTermCfg(func=mdp.joint_pos_rel, delay_min_lag=1, delay_max_lag=3, delay_hold_prob=0.8) + +On each recorded sample, each environment retains its lag with probability 0.8 and otherwise draws a new +one. Zero redraws on every sample. Holding the lag keeps the latency constant while frames continue to +advance; it does not freeze the returned frame. Reset always redraws the selected environments' lags. + +The processing order is observation function, modifiers, noise, clipping, scaling, delay, then history. +The ``delay_min_lag`` and ``delay_max_lag`` field names and delay placement follow +`mjlab's observation configuration `_. +``delay_hold_prob`` follows mjlab's lag-retention semantics, with a default of 1.0 to preserve Isaac Lab's +reset-based delay policy. + +``compute(update_history=True)`` records a sample in both delay and observation history buffers. +``compute()`` and ``compute_group()`` read without advancing either buffer or resampling lag. +For a delay buffer with no recorded sample after initialization or reset, the current input is returned +without recording it. Once recording starts, delays exceeding the available history return the oldest +sample. Partial resets invalidate only the selected environments' histories. + .. autoclass:: ObservationManager :members: :inherited-members: diff --git a/source/isaaclab/changelog.d/shared-delay-buffer.minor.rst b/source/isaaclab/changelog.d/shared-delay-buffer.minor.rst new file mode 100644 index 000000000000..c370539ea8af --- /dev/null +++ b/source/isaaclab/changelog.d/shared-delay-buffer.minor.rst @@ -0,0 +1,17 @@ +Added +^^^^^ + +* Added ``delay_min_lag`` and ``delay_max_lag`` to ``ObservationTermCfg``. Lags were sampled per environment + at initialization and reset, then held for the episode by default, matching the delayed PD actuator's lag policy. +* Added ``delay_hold_prob`` for optional per-sample lag resampling. ``DelayBuffer`` owned the sampling; + retaining a lag kept latency constant as frames advanced. The default of 1.0 preserved the existing policy. +* Applied delay after observation modifiers, noise, clipping, and scaling, before history. Recorded samples + advanced only with ``update_history=True``; extra reads left delay history unchanged. +* Shared ``DelayBuffer`` between observations and existing delayed actuators, retaining the actuator API and + physics-step behavior. Added optional non-recording reads and replaced history shifting with device-indexed + ring storage, including CUDA graph replay and isolated partial resets. + +Fixed +^^^^^ + +* Fixed ``DelayBuffer.set_time_lag`` subset updates to accept both supported integer dtypes. diff --git a/source/isaaclab/isaaclab/managers/manager_term_cfg.py b/source/isaaclab/isaaclab/managers/manager_term_cfg.py index c5f672ff887c..3a4f3d7ef297 100644 --- a/source/isaaclab/isaaclab/managers/manager_term_cfg.py +++ b/source/isaaclab/isaaclab/managers/manager_term_cfg.py @@ -185,6 +185,33 @@ class ObservationTermCfg(ManagerTermBaseCfg): please make sure the length of the tuple matches the dimensions of the tensor outputted from the term. """ + delay_min_lag: int = 0 + """Minimum observation delay, counted in recorded samples. Defaults to zero. + + Each environment samples an integer lag uniformly from ``[delay_min_lag, delay_max_lag]`` at + initialization and reset. With the default :attr:`delay_hold_prob` of 1.0, it keeps that lag until + its next reset, as with :class:`~isaaclab.actuators.DelayedPDActuator`. + Observation samples advance with ``ObservationManager.compute(update_history=True)``; actuator delays + instead count physics steps. Extra observation reads do not advance the delay. + """ + + delay_max_lag: int = 0 + """Maximum observation delay, counted in recorded samples. Zero disables delay. + + Set both lag bounds equal for a constant delay. Delay is applied after modifiers, noise, clipping, and + scaling, before observation history. Until enough samples have been recorded, the oldest available + sample is returned. After reset, no data from the previous episode is returned. + """ + + delay_hold_prob: float = 1.0 + """Probability of retaining the current lag on each recorded observation sample. + + Defaults to 1.0, keeping the reset-sampled lag for the episode. Zero redraws the lag on every recorded + sample. Intermediate values retain the lag independently per environment with this probability, + otherwise sampling uniformly from the configured bounds. Holding the lag keeps the latency constant + while observation frames continue to advance. Extra reads do not resample the lag. + """ + history_length: int = 0 """Number of past observations to store in the observation buffers. Defaults to 0, meaning no history. @@ -198,6 +225,15 @@ class ObservationTermCfg(ManagerTermBaseCfg): """Whether or not the observation manager should flatten history-based observation terms to a 2-D (N, D) tensor. Defaults to True.""" + def validate_config(self): + """Validate observation delay bounds.""" + if type(self.delay_min_lag) is not int or type(self.delay_max_lag) is not int: + raise TypeError("Observation delay bounds must be integers.") + if not 0 <= self.delay_min_lag <= self.delay_max_lag: + raise ValueError("Observation delay requires 0 <= delay_min_lag <= delay_max_lag.") + if not 0.0 <= self.delay_hold_prob <= 1.0: + raise ValueError("delay_hold_prob must be in [0, 1].") + @configclass class ObservationGroupCfg: diff --git a/source/isaaclab/isaaclab/managers/observation_manager.py b/source/isaaclab/isaaclab/managers/observation_manager.py index daea47a4ee7e..519a6f38bc66 100644 --- a/source/isaaclab/isaaclab/managers/observation_manager.py +++ b/source/isaaclab/isaaclab/managers/observation_manager.py @@ -17,7 +17,7 @@ from ..envs.utils.io_descriptors import _warn_io_descriptors_deprecated from ..utils import class_to_dict, modifiers, noise -from ..utils.buffers import CircularBuffer +from ..utils.buffers import CircularBuffer, DelayBuffer from .manager_base import ManagerBase, ManagerTermBase from .manager_term_cfg import ObservationGroupCfg, ObservationTermCfg @@ -270,7 +270,16 @@ def _collect_io_descriptors(self, group_names_to_export: list[str] = ["policy"]) # Iterate over the term's own parameters and add them to the overloads dictionary for k, v in term_cfg.__dict__.items(): # For now we do not add the noise modifier - if k in ["modifiers", "clip", "scale", "history_length", "flatten_history_dim"]: + if k in [ + "modifiers", + "clip", + "scale", + "delay_min_lag", + "delay_max_lag", + "delay_hold_prob", + "history_length", + "flatten_history_dim", + ]: overloads[k] = v desc.update(overloads) group_data[group_name].append(desc) @@ -290,7 +299,15 @@ def _collect_io_descriptors(self, group_names_to_export: list[str] = ["policy"]) # Check if v is a tensor and convert to list if isinstance(v, torch.Tensor): v = v.detach().cpu().numpy().tolist() - if k in ["scale", "clip", "history_length", "flatten_history_dim"]: + if k in [ + "scale", + "clip", + "delay_min_lag", + "delay_max_lag", + "delay_hold_prob", + "history_length", + "flatten_history_dim", + ]: formatted_item["overloads"][k] = v elif k in ["modifiers", "description", "units"]: formatted_item["extras"][k] = v @@ -309,8 +326,10 @@ def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: for group_name, group_cfg in self._group_obs_class_term_cfgs.items(): for term_cfg in group_cfg: term_cfg.func.reset(env_ids=env_ids) - # reset terms with history + # reset delay and observation histories for the selected environments for term_name in self._group_obs_term_names[group_name]: + if term_name in self._group_obs_term_delay_buffer[group_name]: + self._group_obs_term_delay_buffer[group_name][term_name].reset(env_ids) if term_name in self._group_obs_term_history_buffer[group_name]: self._group_obs_term_history_buffer[group_name][term_name].reset(batch_ids=env_ids) # call all modifiers that are classes @@ -327,9 +346,9 @@ def compute(self, update_history: bool = False) -> dict[str, torch.Tensor | dict Please check the :meth:`compute_group` on the processing of observations per group. Args: - update_history: The boolean indicator without return obs should be appended to observation history. - Default to False, in which case calling compute_group does not modify history. This input is no-ops - if the group's history_length == 0. + update_history: Whether to record a new sample in delay and observation history buffers. + Defaults to False, which reads recorded data without advancing either buffer. For a delay + buffer with no sample since initialization or reset, the current observation is returned. Returns: A dictionary with keys as the group names and values as the computed observations. @@ -361,6 +380,8 @@ def compute_group(self, group_name: str, update_history: bool = False) -> torch. 3. Apply corruption/noise model based on :attr:`ObservationTermCfg.noise` 4. Apply clipping based on :attr:`ObservationTermCfg.clip` 5. Apply scaling based on :attr:`ObservationTermCfg.scale` + 6. Apply delay based on :attr:`ObservationTermCfg.delay_min_lag` and :attr:`ObservationTermCfg.delay_max_lag` + 7. Append the delayed observation to history if requested We apply noise to the computed term first to maintain the integrity of how noise affects the data as it truly exists in the real world. If the noise is applied after clipping or scaling, the noise @@ -370,9 +391,9 @@ def compute_group(self, group_name: str, update_history: bool = False) -> torch. Args: group_name: The name of the group for which to compute the observations. Defaults to None, in which case observations for all the groups are computed and returned. - update_history: The boolean indicator without return obs should be appended to observation group's history. - Default to False, in which case calling compute_group does not modify history. This input is no-ops - if the group's history_length == 0. + update_history: Whether to record a new sample in delay and observation history buffers. + Defaults to False, which reads recorded data without advancing either buffer. For a delay + buffer with no sample since initialization or reset, the current observation is returned. Returns: Depending on the group's configuration, the tensors for individual observation terms are @@ -409,6 +430,10 @@ def compute_group(self, group_name: str, update_history: bool = False) -> torch. obs = obs.clip_(min=term_cfg.clip[0], max=term_cfg.clip[1]) if term_cfg.scale is not None: obs = obs.mul_(term_cfg.scale) + if term_name in self._group_obs_term_delay_buffer[group_name]: + obs = self._group_obs_term_delay_buffer[group_name][term_name].compute( + obs, update_history=update_history + ) # Update the history buffer if observation term has history enabled if term_cfg.history_length > 0: circular_buffer = self._group_obs_term_history_buffer[group_name][term_name] @@ -476,6 +501,7 @@ def _prepare_terms(self): self._group_obs_concatenate: dict[str, bool] = {} self._group_obs_concatenate_dim: dict[str, int] = {} + self._group_obs_term_delay_buffer: dict[str, dict[str, DelayBuffer]] = {} self._group_obs_term_history_buffer: dict[str, dict] = {} # create a list to store classes instances, e.g., for modifiers and noise models # we store it as a separate list to only call reset on them and prevent unnecessary calls @@ -512,6 +538,7 @@ def _prepare_terms(self): self._group_obs_class_term_cfgs[group_name] = [] # history buffers + group_entry_delay_buffer: dict[str, DelayBuffer] = {} group_entry_history_buffer: dict[str, CircularBuffer] = {} # read common config for the group @@ -545,6 +572,7 @@ def _prepare_terms(self): f"Configuration for the term '{term_name}' is not of type ObservationTermCfg." f" Received: '{type(term_cfg)}'." ) + term_cfg.validate_config() # resolve common terms in the config self._resolve_common_term_cfg(f"{group_name}/{term_name}", term_cfg, min_argc=1) @@ -638,6 +666,15 @@ def _prepare_terms(self): ) self._group_obs_class_instances.append(term_cfg.noise.func) + if term_cfg.delay_max_lag > 0: + group_entry_delay_buffer[term_name] = DelayBuffer( + term_cfg.delay_max_lag, + self.num_envs, + self.device, + min_lag=term_cfg.delay_min_lag, + hold_prob=term_cfg.delay_hold_prob, + ) + # create history buffers and calculate history term dimensions if term_cfg.history_length > 0: group_entry_history_buffer[term_name] = CircularBuffer( @@ -659,4 +696,5 @@ def _prepare_terms(self): # call reset (in-case above call to get obs dims changed the state) term_cfg.func.reset() # add history buffers for each group + self._group_obs_term_delay_buffer[group_name] = group_entry_delay_buffer self._group_obs_term_history_buffer[group_name] = group_entry_history_buffer diff --git a/source/isaaclab/isaaclab/test/utils/actuator_equivalence.py b/source/isaaclab/isaaclab/test/utils/actuator_equivalence.py index 44cf00d1d9ad..6d19d5648313 100644 --- a/source/isaaclab/isaaclab/test/utils/actuator_equivalence.py +++ b/source/isaaclab/isaaclab/test/utils/actuator_equivalence.py @@ -423,31 +423,28 @@ def test_newton_state_reset_isolated_to_reset_env(self): ctx.__exit__(None, None, None) def test_lab_state_reset_isolated_to_reset_env(self): - """Lab: DelayedPDActuator circular buffer zeroed for env 0 only.""" + """Reset environments accept fresh commands while the remaining environments retain their history.""" ctx, sim, articulation = self._build_and_warm(use_newton_actuators=False) try: - from ...actuators import DelayedPDActuator # noqa: PLC0415 - - delayed = [a for a in articulation.actuators.values() if isinstance(a, DelayedPDActuator)] - self.assertGreater(len(delayed), 0, "expected at least one Lab DelayedPDActuator") - actuator = delayed[0] - buf = actuator.positions_delay_buffer._circular_buffer._buffer - # ``_buffer`` shape: (max_length, batch_size, num_joints). - self.assertIsNotNone(buf, "delay buffer should be populated after warmup") - self.assertTrue( - (buf[:, self.UNCHANGED_ENV] != 0).any().item(), - "expected non-zero buffer entries for env 1 after warmup", - ) - - articulation.reset(env_ids=torch.tensor([self.RESET_ENV], device=articulation.device, dtype=torch.long)) + commands = articulation.actuators.target_command + old_target = commands.position.torch.clone() + # Episode reset samples the configured lag; construction alone leaves actuator lag at zero. + articulation.reset() + commands.set_position_index(value=old_target) + articulation.write_data_to_sim() + new_target = old_target + 0.02 + articulation.reset(env_ids=torch.tensor([self.RESET_ENV], device=articulation.device)) + commands.set_position_index(value=new_target) + articulation.write_data_to_sim() + expected_target = old_target.clone() + expected_target[self.RESET_ENV] = new_target[self.RESET_ENV] + for actuator in articulation.actuators.values(): + joints = actuator.joint_indices + demand = ( + actuator.stiffness * (expected_target[:, joints] - articulation.data.joint_pos.torch[:, joints]) + - actuator.damping * articulation.data.joint_vel.torch[:, joints] + ) + torch.testing.assert_close(actuator.computed_effort, demand) - self.assertTrue( - torch.all(buf[:, self.RESET_ENV] == 0).item(), - f"Lab: env {self.RESET_ENV} buffer not zeroed after reset.", - ) - self.assertTrue( - (buf[:, self.UNCHANGED_ENV] != 0).any().item(), - f"Lab: env {self.UNCHANGED_ENV} buffer was zeroed — reset leaked into an unselected env.", - ) finally: ctx.__exit__(None, None, None) diff --git a/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py b/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py index c74bab9fb079..eaf2b3255458 100644 --- a/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py +++ b/source/isaaclab/isaaclab/utils/buffers/delay_buffer.py @@ -10,28 +10,23 @@ import torch -from .circular_buffer import CircularBuffer - class DelayBuffer: - """Delay buffer that allows retrieving stored data with delays. - - This class uses a batched circular buffer to store input data. Different to a standard circular buffer, - which uses the LIFO (last-in-first-out) principle to retrieve the data, the delay buffer class allows - retrieving data based on the lag set by the user. For instance, if the delay set inside the buffer - is 1, then the second last entry from the stream is retrieved. If it is 2, then the third last entry - and so on. + """Ring storage for delayed batched tensors, independent of actions or observations. - The class supports storing a batched tensor data. This means that the shape of the appended data - is expected to be (batch_size, ...), where the first dimension is the batch dimension. Correspondingly, - the delay can be set separately for each batch index. If the requested delay is larger than the current - length of the underlying buffer, the most recent entry is returned. + Each updating call writes one frame and retrieves a per-batch delayed frame. Storage is allocated + on the first call and never shifted. The write index and per-batch history lengths stay on the device, + including during CUDA graph replay. Callers select lags explicitly, or enable buffer-owned sampling + with ``hold_prob``. The caller determines when to record a sample. - .. note:: - By default, the delay buffer has no delay, meaning that the data is returned as is. + When recording, a lag of zero returns the current input. Until enough samples exist after initialization or reset, + the oldest available sample is returned. Reset only invalidates the selected batches' history; + no previous-episode data can be read, and the remaining batches continue uninterrupted. """ - def __init__(self, history_length: int, batch_size: int, device: str): + def __init__( + self, history_length: int, batch_size: int, device: str, *, min_lag: int = 0, hold_prob: float | None = None + ): """Initialize the delay buffer. Args: @@ -40,18 +35,29 @@ def __init__(self, history_length: int, batch_size: int, device: str): is expected. The minimum acceptable value is zero, which means only the latest data is stored. batch_size: The batch dimension of the data. device: The device used for processing. + min_lag: Minimum lag to sample, with :attr:`history_length` as the inclusive maximum. + Defaults to zero. Used when ``hold_prob`` enables automatic sampling. + hold_prob: Probability of retaining the current lag on each recorded sample. Defaults to None, + preserving lags selected externally through :meth:`set_time_lag`. Setting a probability enables + independent per-batch sampling at initialization and reset: 1.0 keeps that lag until reset, + and 0.0 resamples on every recorded sample. Holding a lag keeps latency constant as frames advance. """ - # set the parameters self._history_length = max(0, history_length) - - # the buffer size: current data plus the history length - self._circular_buffer = CircularBuffer(self._history_length + 1, batch_size, device) - - # the minimum and maximum lags across all batch indices. - self._min_time_lag = 0 - self._max_time_lag = 0 - # the lags for each batch index. + if type(min_lag) is not int or not 0 <= min_lag <= self._history_length: + raise ValueError("min_lag must be an integer in [0, history_length].") + if hold_prob is not None and not 0.0 <= hold_prob <= 1.0: + raise ValueError("hold_prob must be in [0, 1].") + self._min_lag = min_lag + self._hold_prob = hold_prob + self._batch_size = batch_size + self._device = device + self._buffer: torch.Tensor | None = None + self._write_index = torch.zeros(1, dtype=torch.long, device=device) + self._num_pushes = torch.zeros(batch_size, dtype=torch.long, device=device) + self._ALL_INDICES = torch.arange(batch_size, device=device) self._time_lags = torch.zeros(batch_size, dtype=torch.int, device=device) + if hold_prob is not None: + self.reset() """ Properties. @@ -60,12 +66,12 @@ def __init__(self, history_length: int, batch_size: int, device: str): @property def batch_size(self) -> int: """The batch size of the ring buffer.""" - return self._circular_buffer.batch_size + return self._batch_size @property def device(self) -> str: """The device used for processing.""" - return self._circular_buffer.device + return self._device @property def history_length(self) -> int: @@ -75,13 +81,21 @@ def history_length(self) -> int: """ return self._history_length + @property + def num_pushes(self) -> torch.Tensor: + """Number of frames written since each batch's last reset. Shape is (batch_size,). + + Callers may read this device tensor to schedule updates; they must not modify it. + """ + return self._num_pushes + @property def min_time_lag(self) -> int: """Minimum amount of time steps that can be delayed. This value cannot be negative or larger than :attr:`max_time_lag`. """ - return self._min_time_lag + return int(self._time_lags.min().item()) @property def max_time_lag(self) -> int: @@ -89,14 +103,16 @@ def max_time_lag(self) -> int: This value cannot be greater than :attr:`history_length`. """ - return self._max_time_lag + return int(self._time_lags.max().item()) @property def time_lags(self) -> torch.Tensor: """The time lag across each batch index. The shape of the tensor is (batch_size, ). The value at each index represents the delay for that index. - This value is used to retrieve the data from the buffer. + This value is used to retrieve the data from the buffer. Call :meth:`set_time_lag` to validate + external inputs. Callers generating bounded lags on the device may update this tensor in place, + keeping every value in ``[0, history_length]`` without a device-to-host validation round trip. """ return self._time_lags @@ -142,33 +158,78 @@ def set_time_lag(self, time_lag: int | torch.Tensor, batch_ids: Sequence[int] | if max_time_lag > self._history_length: raise ValueError(f"The maximum time lag cannot be larger than the history length. Received: {max_time_lag}") - self._time_lags[batch_ids] = time_lag.to(device=self.device) if isinstance(time_lag, torch.Tensor) else time_lag - self._min_time_lag = int(self._time_lags.min().item()) - self._max_time_lag = int(self._time_lags.max().item()) + if isinstance(time_lag, torch.Tensor): + time_lag = time_lag.to(device=self.device, dtype=self._time_lags.dtype) + self._time_lags[batch_ids] = time_lag def reset(self, batch_ids: Sequence[int] | None = None): """Reset the data in the delay buffer at the specified batch indices. + Automatically sampled lags are redrawn for those batches regardless of ``hold_prob``. + Externally selected lags are preserved. + Args: batch_ids: Elements to reset in the batch dimension. Default is None, which resets all the batch indices. """ - self._circular_buffer.reset(batch_ids) - - def compute(self, data: torch.Tensor) -> torch.Tensor: - """Append the input data to the buffer and returns a stale version of the data based on time lag delay. - - If the requested delay is larger than the number of buffered data points since the last reset, - the function returns the latest data. For instance, if the delay is set to 2 and only one data point - is stored in the buffer, the function will return the latest data. If the delay is set to 2 and three - data points are stored, the function will return the first data point. + indices = slice(None) if batch_ids is None else batch_ids + self._num_pushes[indices] = 0 + if self._hold_prob is not None: + self._time_lags[indices] = torch.randint( + self._min_lag, + self.history_length + 1, + self._time_lags[indices].shape, + dtype=self._time_lags.dtype, + device=self.device, + ) + + def compute(self, data: torch.Tensor, *, update_history: bool = True) -> torch.Tensor: + """Return delayed data, optionally recording the input as a new sample. + + If the requested delay exceeds the available history since reset, returns the oldest available + sample. The result is independent of the internal storage and may be modified by the caller. Args: data: The input data. Shape is (batch_size, ...). + update_history: Whether to record the input as a new sample. Defaults to True. If False, + return the delayed sample relative to the latest recorded frame without modifying the buffer. + Batches with no recorded sample since initialization or reset return the input instead. Returns: The delayed version of the data from the stored buffer. Shape is (batch_size, ...). """ - # add the new data to the last layer - self._circular_buffer.append(data) - # return output - return self._circular_buffer[self._time_lags] + if data.shape[0] != self.batch_size: + raise ValueError(f"The input data has '{data.shape[0]}' batch size while expecting '{self.batch_size}'") + if self._buffer is None: + if not update_history: + return data.to(device=self.device).clone() + self._buffer = torch.empty((self.history_length + 1, *data.shape), dtype=data.dtype, device=self.device) + elif data.shape != self._buffer.shape[1:]: + raise ValueError(f"Expected data shape {self._buffer.shape[1:]}, received {data.shape}.") + + data = data.to(device=self.device, dtype=self._buffer.dtype) + if not update_history: + lag = torch.minimum(self._time_lags, (self._num_pushes - 1).clamp_min(0)) + read_index = (self._write_index - 1 - lag) % (self.history_length + 1) + has_history = (self._num_pushes > 0).view(self.batch_size, *([1] * (data.ndim - 1))) + return torch.where(has_history, self._buffer[read_index, self._ALL_INDICES], data) + + if self._hold_prob is not None and self._hold_prob < 1.0: + lags = torch.randint( + self._min_lag, + self.history_length + 1, + (self.batch_size,), + dtype=self._time_lags.dtype, + device=self.device, + ) + if self._hold_prob > 0.0: + resample = torch.rand(self.batch_size, device=self.device) >= self._hold_prob + lags = torch.where(resample, lags, self._time_lags) + self._time_lags.copy_(lags) + + self._buffer.index_copy_(0, self._write_index, data.unsqueeze(0)) + lag = torch.minimum(self._time_lags, self._num_pushes) + read_index = (self._write_index - lag) % (self.history_length + 1) + result = self._buffer[read_index, self._ALL_INDICES] + self._num_pushes.add_(1) + self._write_index.add_(1).remainder_(self.history_length + 1) + return result diff --git a/source/isaaclab/test/managers/test_observation_manager_unit.py b/source/isaaclab/test/managers/test_observation_manager_unit.py index 81db0fa1f7c5..9bcdd528fd6a 100644 --- a/source/isaaclab/test/managers/test_observation_manager_unit.py +++ b/source/isaaclab/test/managers/test_observation_manager_unit.py @@ -16,7 +16,7 @@ import torch from isaaclab.managers import ObservationGroupCfg, ObservationManager, ObservationTermCfg -from isaaclab.utils import configclass, modifiers +from isaaclab.utils import DelayBuffer, configclass, modifiers, noise pytestmark = pytest.mark.unit @@ -145,31 +145,61 @@ def test_modifier_resolution_stays_out_of_observation_manager(): def test_modifier_base_cfg_marker_does_not_exist(): """Stateful modifiers must not require a marker configuration subtype.""" assert not hasattr(modifiers, "ModifierBaseCfg") + assert not hasattr(modifiers, "DelayCfg"), "Observation delay uses the shared buffer directly." -def test_compute_updates_history_only_when_requested(): - """Observation history changes only when ``update_history`` is enabled.""" +@pytest.mark.parametrize(("lag", "history_length"), [(0, 2), (1, 0), (1, 2)]) +def test_compute_updates_history_only_when_requested(lag, history_length): + """History alone, delay alone, and their combination advance only on recorded samples.""" + cfg = HistoryObservationsCfg() + cfg.policy.history_length = history_length + cfg.policy.enable_corruption = True + cfg.policy.dummy.delay_min_lag = cfg.policy.dummy.delay_max_lag = lag + cfg.policy.dummy.delay_hold_prob = 0.5 if history_length == 0 else 1.0 + cfg.policy.dummy.noise = noise.ConstantNoiseCfg(bias=0.0) + cfg.policy.dummy.scale = 2.0 env = DummyEnv() - manager = ObservationManager(HistoryObservationsCfg(), cast("ManagerBasedEnv", env)) - history = manager._group_obs_term_history_buffer["policy"]["dummy"] - - torch.testing.assert_close(history.current_length, torch.zeros(env.num_envs, dtype=torch.int64)) - + manager = ObservationManager(cfg, cast("ManagerBasedEnv", env)) + delay = manager._group_obs_term_delay_buffer["policy"].get("dummy") + history = manager._group_obs_term_history_buffer["policy"].get("dummy") manager.compute() - torch.testing.assert_close(history.current_length, torch.zeros(env.num_envs, dtype=torch.int64)) - - manager.compute(update_history=True) - torch.testing.assert_close(history.current_length, torch.ones(env.num_envs, dtype=torch.int64)) - history_after_update = history.buffer.clone() - - env.observation.add_(10.0) - observations = manager.compute() - policy_observation = observations["policy"] - assert isinstance(policy_observation, torch.Tensor) - torch.testing.assert_close(history.current_length, torch.ones(env.num_envs, dtype=torch.int64)) - torch.testing.assert_close(history.buffer, history_after_update) - torch.testing.assert_close(policy_observation, history_after_update.reshape(env.num_envs, -1)) - - manager.compute(update_history=True) - torch.testing.assert_close(history.current_length, torch.full((env.num_envs,), 2, dtype=torch.int64)) - torch.testing.assert_close(history.buffer[:, -1], env.observation) + if delay: + assert isinstance(delay, DelayBuffer) + assert torch.all(delay.num_pushes == 0) + if history: + assert torch.all(history.current_length == 0) + + for step in range(6): + if step == 3: + manager.reset([1]) + env.observation.fill_(step) + # Delay retains each sample's noise; history stacks the delayed, scaled outputs. + manager.cfg.policy.dummy.noise.bias = float(step) + output = manager.compute(update_history=True)["policy"] + sample_steps = torch.arange(step - max(1, history_length) + 1, step + 1) + expected = 4.0 * (sample_steps - lag).clamp_min(0).expand(env.num_envs, -1).clone() + if step >= 3: + expected[1].clamp_(min=12.0) + torch.testing.assert_close(output, expected) + env.observation.fill_(-100.0) + rng_state = torch.get_rng_state() + torch.testing.assert_close(manager.compute()["policy"], expected) + torch.testing.assert_close(manager.compute_group("policy"), expected) + assert torch.equal(torch.get_rng_state(), rng_state) + + +@pytest.mark.parametrize( + ("params", "error"), + [ + ({"delay_min_lag": -1}, ValueError), + ({"delay_min_lag": 2, "delay_max_lag": 1}, ValueError), + ({"delay_min_lag": 0.5}, TypeError), + ({"delay_hold_prob": -0.1}, ValueError), + ({"delay_hold_prob": 1.1}, ValueError), + ], +) +def test_observation_delay_config_validation(params, error): + """Delay requires ordered nonnegative integer bounds and a probability in [0, 1].""" + cfg = ObservationTermCfg(func=dummy_observation, **params) + with pytest.raises(error, match="delay"): + cfg.validate() diff --git a/source/isaaclab/test/utils/test_delay_buffer.py b/source/isaaclab/test/utils/test_delay_buffer.py index 203bf4fb15a8..a2f2c8b31254 100644 --- a/source/isaaclab/test/utils/test_delay_buffer.py +++ b/source/isaaclab/test/utils/test_delay_buffer.py @@ -8,15 +8,16 @@ import pytest import torch +from isaaclab.test.utils import test_devices from isaaclab.utils import DelayBuffer pytestmark = pytest.mark.unit -@pytest.fixture -def delay_buffer(): +@pytest.fixture(params=test_devices()) +def delay_buffer(request): """Create a delay buffer for testing.""" - device: str = "cpu" + device: str = request.param batch_size: int = 10 history_length: int = 4 return DelayBuffer(history_length, batch_size=batch_size, device=device) @@ -37,60 +38,67 @@ def test_constant_time_lags(delay_buffer): all_data = [] for i, data in enumerate(_generate_data(batch_size, 20, delay_buffer.device)): + # Reads before the first recorded sample use the input without allocating history. + if i == 0: + torch.testing.assert_close(delay_buffer.compute(data, update_history=False), data) + assert torch.all(delay_buffer.num_pushes == 0) all_data.append(data) # apply delay delayed_data = delay_buffer.compute(data) error = delayed_data - all_data[max(0, i - const_lag)] assert torch.all(error == 0) - - -def test_reset(delay_buffer): - """Test resetting the last two batch indices after iteration `reset_itr`.""" - const_lag: int = 2 - reset_itr = 10 - batch_size: int = 10 - - delay_buffer.set_time_lag(const_lag) - - all_data = [] - for i, data in enumerate(_generate_data(batch_size, 20, delay_buffer.device)): - all_data.append(data) - # from 'reset_itr' iteration reset the last and second-to-last environments - if i == reset_itr: - delay_buffer.reset([-2, -1]) - # apply delay - delayed_data = delay_buffer.compute(data) - # before 'reset_itr' is is similar to test_constant_time_lags - # after that indices [-2, -1] should be treated separately - if i < reset_itr: - error = delayed_data - all_data[max(0, i - const_lag)] - assert torch.all(error == 0) - else: - # error_regular = delayed_data[:-2] - all_data[max(0, i - const_lag)][:-2] - error2_reset = delayed_data[-2, -1] - all_data[max(reset_itr, i - const_lag)][-2, -1] - # assert torch.all(error_regular == 0) - assert torch.all(error2_reset == 0) - - -def test_random_time_lags(delay_buffer): - """Test random delays.""" - max_lag: int = 3 - time_lags = torch.randint( - low=0, high=max_lag + 1, size=(delay_buffer.batch_size,), dtype=torch.int, device=delay_buffer.device - ) - - delay_buffer.set_time_lag(time_lags) - - all_data = [] - for i, data in enumerate(_generate_data(delay_buffer.batch_size, 20, delay_buffer.device)): - all_data.append(data) - # apply delay - delayed_data = delay_buffer.compute(data) - true_delayed_index = torch.maximum(i - delay_buffer.time_lags, torch.zeros_like(delay_buffer.time_lags)) - true_delayed_index = true_delayed_index.tolist() - for i in range(delay_buffer.batch_size): - error = delayed_data[i] - all_data[true_delayed_index[i]][i] - assert torch.all(error == 0) + torch.testing.assert_close(delay_buffer.compute(data + 100, update_history=False), delayed_data) + assert torch.all(delay_buffer.num_pushes == i + 1) + + +@pytest.mark.parametrize("feature_shape", [(), (2, 3)]) +def test_reset(delay_buffer, feature_shape): + """Partial and full resets return fresh samples without affecting other histories.""" + import isaaclab.utils.buffers.delay_buffer as delay_module + + assert not hasattr(delay_module, "CircularBuffer"), "Delay storage must not depend on frame stacking." + delay_buffer.set_time_lag(2) + first_step = torch.zeros(delay_buffer.batch_size, dtype=torch.long, device=delay_buffer.device) + shape = (delay_buffer.batch_size, *([1] * len(feature_shape))) + for step in range(20): + data = torch.full((delay_buffer.batch_size, *feature_shape), step, device=delay_buffer.device) + if step in (7, 12): + ids = [1] if step == 7 else None + delay_buffer.reset(ids) + first_step[ids if ids is not None else slice(None)] = step + expected = torch.maximum(first_step, torch.full_like(first_step, step - 3)) + torch.testing.assert_close( + delay_buffer.compute(data, update_history=False), expected.view(shape).expand_as(data) + ) + expected = torch.maximum(first_step, torch.full_like(first_step, step - 2)) + torch.testing.assert_close(delay_buffer.compute(data), expected.view(shape).expand_as(data)) + + +@pytest.mark.parametrize("hold_prob", [0.0, 0.5, 1.0]) +def test_random_time_lags(delay_buffer, hold_prob, monkeypatch): + """Each batch retains its lag or resamples; reset always starts a new episode.""" + delay_buffer = DelayBuffer(4, delay_buffer.batch_size, delay_buffer.device, min_lag=1, hold_prob=hold_prob) + time_lags = torch.randint(1, 5, (delay_buffer.batch_size,), device=delay_buffer.device) + # Indexed assignment must accept int64 lags as well as the buffer's int32 dtype. + delay_buffer.set_time_lag(time_lags, list(range(delay_buffer.batch_size))) + expected_lags = time_lags.int() + first_step = torch.zeros_like(expected_lags) + draws = torch.tensor([0.25, 0.75] * (delay_buffer.batch_size // 2), device=delay_buffer.device) + monkeypatch.setattr(torch, "rand", lambda *args, **kwargs: draws) + monkeypatch.setattr(torch, "randint", lambda low, high, size, **kwargs: torch.full(size, sampled_lag, **kwargs)) + + for step, data in enumerate(_generate_data(delay_buffer.batch_size, 12, delay_buffer.device)): + sampled_lag = 1 + step % 4 + if step == 7: + delay_buffer.reset([0]) + expected_lags[0] = sampled_lag + first_step[0] = step + expected_lags[draws >= hold_prob] = sampled_lag + result = delay_buffer.compute(data) + torch.testing.assert_close(delay_buffer.time_lags, expected_lags) + expected = torch.maximum(step - expected_lags, first_step).unsqueeze(-1) + torch.testing.assert_close(result, expected) + torch.testing.assert_close(delay_buffer.compute(data + 100, update_history=False), expected) @pytest.mark.parametrize( @@ -119,14 +127,34 @@ def test_invalid_time_lag_does_not_mutate_state(delay_buffer, time_lag, batch_id def test_compute_result_independent_of_internal_buffer(delay_buffer): - """``compute()``'s returned tensor must not alias the internal circular buffer storage. - - Regression: ``DelayBuffer.compute`` previously called ``.clone()`` defensively. After - dropping the clone (advanced indexing returns a copy), this asserts the contract is - preserved — mutating the result in place must not affect the next ``compute()`` output. - """ - delay_buffer.set_time_lag(0) + """Mutating a delayed output must not corrupt retained samples.""" + delay_buffer.set_time_lag(1) first = delay_buffer.compute(torch.full((delay_buffer.batch_size, 1), 1, dtype=torch.int)) - first.fill_(999) # mutate the returned tensor + first.fill_(999) second = delay_buffer.compute(torch.full((delay_buffer.batch_size, 1), 2, dtype=torch.int)) - assert torch.all(second == 2), "Mutation of a prior compute() result leaked into the next call" + assert torch.all(second == 1) + + +def test_delay_buffer_cuda_graph(delay_buffer): + """Sampling and ring writes work during graph replay, including across partial resets.""" + if not delay_buffer.device.startswith("cuda"): + pytest.skip("CUDA graph replay requires CUDA.") + with torch.cuda.device(delay_buffer.device): + delay_buffer = DelayBuffer(4, delay_buffer.batch_size, delay_buffer.device, min_lag=1, hold_prob=0.5) + data = torch.zeros(delay_buffer.batch_size, 1, device=delay_buffer.device) + delay_buffer.compute(data) + graph = torch.cuda.CUDAGraph() + with torch.cuda.graph(graph): + result = delay_buffer.compute(data) + read = delay_buffer.compute(data, update_history=False) + delay_buffer.reset() + for step in range(9): + if step == 4: + delay_buffer.reset([1]) + data.fill_(step) + graph.replay() + expected = (step - delay_buffer.time_lags).clamp_min(0).unsqueeze(-1).to(data.dtype) + if step >= 4: + expected[1].clamp_(min=4) + torch.testing.assert_close(result, expected) + torch.testing.assert_close(read, expected) From e53ef4ad9b36f8bc4d5441a72245f92742ef7de2 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 24 Sep 2026 05:20:22 +0200 Subject: [PATCH 09/11] Gate checkout-only CLI tests with `source_checkout_root` (#7945) # Description Some CLI unit tests still assume that they run from an Isaac Lab source checkout. They read the root `pyproject.toml`, copy the repository launcher, or execute scripts that are not shipped in the installed package. This makes the tests fail when the suite is run against an installed wheel or conda package. Use the existing `source_checkout_root` fixture for these remaining tests, following the pattern introduced in #7389. The launcher and teleoperation tests also resolve checkout-only files from the path returned by the fixture. ## Type of change - Bug fix (non-breaking change which fixes an issue) ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` ## Checklist - [x] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added a changelog fragment under `source//changelog.d/` for every touched package - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there Local validation: ```text uvx pre-commit run --files : passed 56 passed in 9.22s ``` --- .../gate-checkout-only-cli-tests.skip | 1 + source/isaaclab/test/cli/test_env_commands.py | 21 ++++++++++--------- source/isaaclab/test/cli/test_install.py | 2 +- .../test/cli/test_install_command_parsing.py | 1 + .../test/cli/test_install_commands.py | 5 +++-- .../test/cli/test_teleop_entrypoints.py | 6 +++--- 6 files changed, 20 insertions(+), 16 deletions(-) create mode 100644 source/isaaclab/changelog.d/gate-checkout-only-cli-tests.skip diff --git a/source/isaaclab/changelog.d/gate-checkout-only-cli-tests.skip b/source/isaaclab/changelog.d/gate-checkout-only-cli-tests.skip new file mode 100644 index 000000000000..677b2d1bac01 --- /dev/null +++ b/source/isaaclab/changelog.d/gate-checkout-only-cli-tests.skip @@ -0,0 +1 @@ +# Test-only change; no changelog entry or version bump. diff --git a/source/isaaclab/test/cli/test_env_commands.py b/source/isaaclab/test/cli/test_env_commands.py index d29d7d0f668d..43aea874ad47 100644 --- a/source/isaaclab/test/cli/test_env_commands.py +++ b/source/isaaclab/test/cli/test_env_commands.py @@ -8,6 +8,7 @@ import os import shutil import subprocess +from pathlib import Path from unittest import mock import pytest @@ -59,11 +60,11 @@ def test_environment_setup_accepts_marked_source_build(tmp_path): envs._reject_downloaded_isaac_sim("uv") -def test_launcher_rejects_downloaded_isaac_sim_with_active_environment(tmp_path): +def test_launcher_rejects_downloaded_isaac_sim_with_active_environment(source_checkout_root: Path, tmp_path): """Platform launchers must reject an active environment before selecting its Python.""" launcher_name = "isaaclab.bat" if envs.is_windows() else "isaaclab.sh" launcher = tmp_path / launcher_name - shutil.copy2(envs.ISAACLAB_ROOT / launcher_name, launcher) + shutil.copy2(source_checkout_root / launcher_name, launcher) (tmp_path / "_isaac_sim").mkdir() environment = os.environ.copy() @@ -78,10 +79,10 @@ def test_launcher_rejects_downloaded_isaac_sim_with_active_environment(tmp_path) @pytest.mark.skipif(envs.is_windows(), reason="Linux launcher behavior") -def test_launcher_uses_bundled_python_with_inactive_default_environment(tmp_path): +def test_launcher_uses_bundled_python_with_inactive_default_environment(source_checkout_root: Path, tmp_path): """An inactive default environment must not override the bundled Python.""" launcher = tmp_path / "isaaclab.sh" - shutil.copy2(envs.ISAACLAB_ROOT / "isaaclab.sh", launcher) + shutil.copy2(source_checkout_root / "isaaclab.sh", launcher) bundled_python = tmp_path / "_isaac_sim" / "python.sh" bundled_python.parent.mkdir() bundled_python.write_text("#!/usr/bin/env bash\necho bundled-python\n") @@ -102,10 +103,10 @@ def test_launcher_uses_bundled_python_with_inactive_default_environment(tmp_path @pytest.mark.skipif(envs.is_windows(), reason="Linux launcher behavior") -def test_launcher_accepts_virtual_environment_on_bundled_python(tmp_path): +def test_launcher_accepts_virtual_environment_on_bundled_python(source_checkout_root: Path, tmp_path): """A virtual environment created on the package's own Python runs that interpreter, so it is allowed.""" launcher = tmp_path / "isaaclab.sh" - shutil.copy2(envs.ISAACLAB_ROOT / "isaaclab.sh", launcher) + shutil.copy2(source_checkout_root / "isaaclab.sh", launcher) bundled_python = tmp_path / "_isaac_sim" / "python.sh" bundled_python.parent.mkdir() bundled_python.write_text("#!/usr/bin/env bash\necho bundled-python\n") @@ -126,10 +127,10 @@ def test_launcher_accepts_virtual_environment_on_bundled_python(tmp_path): @pytest.mark.skipif(envs.is_windows(), reason="Linux launcher behavior") -def test_launcher_rejects_virtual_environment_on_foreign_python(tmp_path): +def test_launcher_rejects_virtual_environment_on_foreign_python(source_checkout_root: Path, tmp_path): """A virtual environment built on another interpreter stays rejected.""" launcher = tmp_path / "isaaclab.sh" - shutil.copy2(envs.ISAACLAB_ROOT / "isaaclab.sh", launcher) + shutil.copy2(source_checkout_root / "isaaclab.sh", launcher) (tmp_path / "_isaac_sim").mkdir() venv = tmp_path / "venv" (venv / "bin").mkdir(parents=True) @@ -145,10 +146,10 @@ def test_launcher_rejects_virtual_environment_on_foreign_python(tmp_path): @pytest.mark.skipif(envs.is_windows(), reason="Linux launcher behavior") -def test_launcher_allows_relinking_unmarked_source_build(tmp_path): +def test_launcher_allows_relinking_unmarked_source_build(source_checkout_root: Path, tmp_path): """The source-build command must bypass downloaded-package environment rejection.""" launcher = tmp_path / "isaaclab.sh" - shutil.copy2(envs.ISAACLAB_ROOT / "isaaclab.sh", launcher) + shutil.copy2(source_checkout_root / "isaaclab.sh", launcher) (tmp_path / "_isaac_sim").mkdir() active_python = tmp_path / "virtual-env" / "bin" / "python" active_python.parent.mkdir(parents=True) diff --git a/source/isaaclab/test/cli/test_install.py b/source/isaaclab/test/cli/test_install.py index 89d762f4661a..08f64e7ebc55 100644 --- a/source/isaaclab/test/cli/test_install.py +++ b/source/isaaclab/test/cli/test_install.py @@ -371,7 +371,7 @@ class TestEnsureNewton: def _completed(stdout: str = "", returncode: int = 0) -> subprocess.CompletedProcess: return subprocess.CompletedProcess(args=[], returncode=returncode, stdout=stdout, stderr="") - def test_installs_pinned_release_when_absent(self): + def test_installs_pinned_release_when_absent(self, source_checkout_root: Path): """When the pinned release is not installed, uninstall Newton then install it.""" from isaaclab.cli.commands import install diff --git a/source/isaaclab/test/cli/test_install_command_parsing.py b/source/isaaclab/test/cli/test_install_command_parsing.py index a24f6df1cd1d..aef4d22766e5 100644 --- a/source/isaaclab/test/cli/test_install_command_parsing.py +++ b/source/isaaclab/test/cli/test_install_command_parsing.py @@ -200,6 +200,7 @@ def _make_mock_env(**extra_env): return env +@pytest.mark.usefixtures("source_checkout_root") class TestCommandInstallDispatch: """Test that command_install() calls the right functions with the right args.""" diff --git a/source/isaaclab/test/cli/test_install_commands.py b/source/isaaclab/test/cli/test_install_commands.py index 132d3d433dbd..d39249244d34 100644 --- a/source/isaaclab/test/cli/test_install_commands.py +++ b/source/isaaclab/test/cli/test_install_commands.py @@ -420,6 +420,7 @@ def test_probe_receives_original_pythonpath(self, tmp_path): # --------------------------------------------------------------------------- +@pytest.mark.usefixtures("source_checkout_root") class TestEnsureCudaTorch: """Tests for :func:`_ensure_cuda_torch` across architectures and environment types. @@ -973,14 +974,14 @@ def test_all_non_nvidia_packages_are_repointed(self, tmp_path, pkg_name): class TestInstallRootExtraExcludesIsaacSim: """The ``teleop`` extra lists Isaac Sim for uv, but pip must never resolve it inline.""" - def test_root_extra_dependencies_exclude_isaacsim(self): + def test_root_extra_dependencies_exclude_isaacsim(self, source_checkout_root: Path): """pip has no override mechanism, so isaacsim + isaacteleop in one pass cannot resolve.""" dependencies = install_cmd._root_extra_dependencies("teleop") assert not any(d.startswith("isaacsim") for d in dependencies) assert any(d.startswith("isaacteleop") for d in dependencies) - def test_install_root_extra_omits_isaacsim_from_the_pip_command(self, tmp_path): + def test_install_root_extra_omits_isaacsim_from_the_pip_command(self, source_checkout_root: Path, tmp_path): """``./isaaclab.sh -i teleop`` must not hand Isaac Sim to pip alongside Isaac Teleop.""" python_exe = str(tmp_path / "python") pip_cmd = [python_exe, "-m", "pip"] diff --git a/source/isaaclab/test/cli/test_teleop_entrypoints.py b/source/isaaclab/test/cli/test_teleop_entrypoints.py index a459e5f5bb69..8c56e014f220 100644 --- a/source/isaaclab/test/cli/test_teleop_entrypoints.py +++ b/source/isaaclab/test/cli/test_teleop_entrypoints.py @@ -27,13 +27,13 @@ @pytest.mark.parametrize(("command", "script_parts"), TELEOP_WORKFLOWS.items()) -def test_teleop_workflow_help_exposes_task_preset_selectors(command, script_parts): +def test_teleop_workflow_help_exposes_task_preset_selectors(source_checkout_root: Path, command, script_parts): """Every teleop workflow accepts the task preset selectors documented for teleoperation.""" - script = cli.ISAACLAB_ROOT.joinpath(*script_parts) + script = source_checkout_root.joinpath(*script_parts) result = subprocess.run( [sys.executable, str(script), "--help"], - cwd=cli.ISAACLAB_ROOT, + cwd=source_checkout_root, capture_output=True, text=True, check=False, From 6bb09fc264246e1a99a5afa4c87ce5a3af76ac29 Mon Sep 17 00:00:00 2001 From: ooctipus Date: Wed, 23 Sep 2026 21:17:32 -0700 Subject: [PATCH 10/11] [Newton] Report joint reaction wrenches for fixed connections (#7978) ## Description Newton's joint wrench sensor omitted welded tool and wrist-sensor bodies because it inherited the articulation's control-joint filter. A fixed connection still transmits force and torque. Select reportable tree joints from the Newton model while reusing the articulation's cached body bindings. This includes fixed joints between bodies and continues to exclude free joints, world-fixed roots, and loop-closing constraints. The articulation's control-joint selection and the wrench conversion kernel remain unchanged. Callers should select sensor entries by body name. Extended the existing cartpole gravity-wrench test with a fixed pole joint. Three selection cases cover distinct root types, loop exclusion, reordered bodies, and per-environment anchors. The PR adds no USD fixture or test file, and only the existing Newton sensor test file changes. Fixes #7969. ## Validation - All 13 Newton sensor cases passed after merging current develop, using Newton 1.6.0 and Warp 1.17.0. - The fixed-pole physics case and all three selection cases failed with the original implementation; the movable-pole case still passed. - Verified cached body bindings are reused, no second sensing view is created, and control-joint selection stays unchanged. - Six-file architecture audit and `uv run --no-sync isaaclab -f` passed. ## Type of change - Bug fix - Documentation update ## Release backport - [ ] Backport this pull request to the active release branch after it merges into `develop` ## Checklist - [x] I have read and understood the contribution guidelines - [x] I have run the pre-commit checks - [x] I have made corresponding changes to the documentation - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective - [x] I have added a changelog fragment for every touched package - [x] My name already exists in `CONTRIBUTORS.md` --- AGENTS.md | 2 + .../concepts/sensors/joint_wrench_sensor.rst | 6 +- .../changelog.d/fixed-joint-wrench.rst | 7 + .../joint_wrench/joint_wrench_sensor.py | 37 ++++-- .../joint_wrench/joint_wrench_sensor_data.py | 2 +- .../test/sensors/test_joint_wrench_sensor.py | 120 ++++++++++++++---- 6 files changed, 130 insertions(+), 44 deletions(-) create mode 100644 source/isaaclab_newton/changelog.d/fixed-joint-wrench.rst diff --git a/AGENTS.md b/AGENTS.md index 3400069a1f88..207b1c2dcd76 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -10,6 +10,8 @@ - Use modern Python type hints, including `X | None` instead of `Optional[X]`. - Use `snake_case` for methods, functions, and CLI arguments. - Keep related public symbols discoverable through consistent prefixes. +- Keep joint-wrench sensor coverage separate from articulation control-joint selection. Reuse cached + body bindings without changing the shared view's joint filters or creating a second view for sensing. - For external wrenches, follow the asset API's `is_global` boolean and `_b`/`_w` buffer naming. Keep frame conversion decisions in `WrenchComposer` and track pending contributions with plain booleans; do not introduce frame enums, content bitmasks, or a classification layer. diff --git a/docs/source/concepts/sensors/joint_wrench_sensor.rst b/docs/source/concepts/sensors/joint_wrench_sensor.rst index 57a20c185568..049b673392ee 100644 --- a/docs/source/concepts/sensors/joint_wrench_sensor.rst +++ b/docs/source/concepts/sensors/joint_wrench_sensor.rst @@ -24,8 +24,10 @@ Set :attr:`~sensors.JointWrenchSensorCfg.prim_path` to the articulation root. Re depends on the physics backend: * PhysX and OVPhysX report every articulation link, including the root link. -* Newton reports the child link of each non-free, non-fixed joint. It therefore excludes the root - link and any links connected through free or fixed joints. +* Newton reports the child link of each non-free joint in the articulation tree, including fixed + connections between bodies such as a welded wrist sensor or tool flange. Free joints, fixed joints + to the world, and loop-closing constraints are excluded. The reported wrench at a weld includes + the loads transmitted by its child subtree, even though the joint has no degrees of freedom. Use :attr:`~sensors.JointWrenchSensor.body_names` or :meth:`~sensors.JointWrenchSensor.find_bodies` instead of assuming that different backends expose diff --git a/source/isaaclab_newton/changelog.d/fixed-joint-wrench.rst b/source/isaaclab_newton/changelog.d/fixed-joint-wrench.rst new file mode 100644 index 000000000000..297d7cb0ef6b --- /dev/null +++ b/source/isaaclab_newton/changelog.d/fixed-joint-wrench.rst @@ -0,0 +1,7 @@ +Fixed +^^^^^ + +* Fixed Newton joint wrench sensors to report fixed connections within an articulation, including + welded wrist sensors and tool flanges. Free joints, world-fixed roots, and loop-closing constraints + remained excluded. Select entries by ``body_names`` or ``find_bodies`` because the reported body count + and ordering may change. diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index b3226e2ca1de..5aa1f5c0b9f7 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -10,6 +10,7 @@ from collections.abc import Sequence from typing import TYPE_CHECKING +import numpy as np import warp as wp from newton import JointType from newton.selection import ArticulationView @@ -40,9 +41,9 @@ class JointWrenchSensor(BaseJointWrenchSensor): :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` may point at either an articulation root expression or an env-scoped parent prefix. Newton label - matching selects the articulations owned by that prefix. ``FREE`` and - ``FIXED`` joints are excluded because neither has a meaningful joint - anchor. + matching selects the articulations owned by that prefix. Reports tree joints, + including fixed connections between bodies. Free joints, fixed joints to the + world, and loop-closing constraints are excluded. """ cfg: JointWrenchSensorCfg @@ -141,10 +142,20 @@ def has_articulation_root_api(prim) -> bool: verbose=False, exclude_joint_types=[JointType.FREE, JointType.FIXED], ) - self._num_joints = self._root_view.joint_count + # Share body bindings with the articulation, but select sensor joints independently + # of its control joints. articulation_end excludes loop-closing constraints. + articulation_ids = self._root_view.articulation_ids.numpy()[:, 0] + joint_starts = model.articulation_start.numpy()[articulation_ids] + joint_end = model.articulation_end.numpy()[articulation_ids[0]] + tree_joint_ids = np.arange(joint_starts[0], joint_end) + joint_types = model.joint_type.numpy()[tree_joint_ids] + joint_parents = model.joint_parent.numpy()[tree_joint_ids] + report_joint = (joint_types != JointType.FREE) & ((joint_types != JointType.FIXED) | (joint_parents != -1)) + joint_ids = joint_starts[:, None] + (tree_joint_ids[report_joint] - joint_starts[0]) + self._num_joints = joint_ids.shape[1] if self._num_joints == 0: raise RuntimeError( - "Joint wrench sensor matched zero reportable joints (all joints are FREE or FIXED)." + "Joint wrench sensor matched zero reportable tree joints (only free joints or world-fixed roots)." f" Check the articulation at '{root_prim_path_expr}'." ) @@ -160,14 +171,14 @@ def has_articulation_root_api(prim) -> bool: self._sim_bind_body_parent_f = body_parent_f[:, 0] self._sim_bind_body_q = self._root_view.get_link_transforms(state_0)[:, 0] self._sim_bind_body_com = self._root_view.get_attribute("body_com", model)[:, 0] - self._sim_bind_joint_X_c = self._root_view.get_attribute("joint_X_c", model)[:, 0] - - # joint_child is per-articulation; topology is identical across envs, - # so we take the first-env mapping as the 1-D kernel input. - joint_child_full = self._root_view.get_attribute("joint_child", model)[:, 0] - joint_child_np = joint_child_full.numpy()[0] - if not all(0 <= b < self._sim_bind_body_parent_f.shape[1] for b in joint_child_np): - raise RuntimeError(f"joint_child contains out-of-range body indices for '{self.cfg.prim_path}'") + self._sim_bind_joint_X_c = wp.array( + model.joint_X_c.numpy()[joint_ids], dtype=wp.transformf, device=self._device + ) + + # The shared view orders all tree bodies by model index. Its topology is identical across envs. + joint_children = model.joint_child.numpy() + body_ids = np.unique(joint_children[tree_joint_ids]) + joint_child_np = np.searchsorted(body_ids, joint_children[joint_ids[0]]) self._joint_child = wp.array(joint_child_np, dtype=wp.int32, device=self._device) link_names = list(self._root_view.link_names) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py index 76c5a565bdfb..9522d2f67afa 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -58,7 +58,7 @@ def create_buffers(self, num_envs: int, num_joints: int, device: str) -> None: Args: num_envs: Number of environments. - num_joints: Number of reported joints (excludes FREE and FIXED joint types). + num_joints: Number of reported tree joints, including fixed joints between bodies. device: Device for array storage. """ self._force = wp.zeros((num_envs, num_joints), dtype=wp.vec3f, device=device) diff --git a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py index cbc423c533d7..d9274b38ed39 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -7,15 +7,20 @@ import sys from pathlib import Path +from unittest.mock import Mock sys.path.insert(0, str(Path(__file__).resolve().parents[1])) +import newton +import numpy as np import pytest import torch import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_physx.sim.schemas import PhysxJointCfg +from pxr import Usd, UsdPhysics + import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg @@ -25,7 +30,7 @@ from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass from isaaclab.utils import math as math_utils -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab_assets.robots.ant import ANT_CFG @@ -388,43 +393,39 @@ def test_wrench_with_external_force_and_torque(sim): torch.testing.assert_close(torque, expected_torque, atol=0.15, rtol=1e-2) -def test_interior_joint_wrench_at_rest(sim): - """Interior joint wrench accounts for the weight of all descendant bodies. - - The cartpole has two joints: ``slider_to_cart`` (interior, supports cart - and pole) and ``cart_to_pole`` (terminal, supports pole only). At steady - state with gravity as the only load, the reaction wrench at the interior - joint must equal the combined weight of cart and pole, with torque - computed from each body's moment about the joint anchor. - """ - scene = InteractiveScene(_CartpoleDampedSceneCfg(num_envs=1)) +@pytest.mark.parametrize("fixed_pole", [False, True]) +def test_interior_joint_wrench_at_rest(sim, tmp_path, fixed_pole): + """Cart supports both masses; the pole joint supports the pole even when welded.""" + scene_cfg = _CartpoleDampedSceneCfg(num_envs=1) + if fixed_pole: + scene_cfg.robot.actuators.pop("pole_actuator") + scene_cfg.robot.init_state.joint_pos.pop("cart_to_pole") + stage = Usd.Stage.Open(retrieve_file_path(scene_cfg.robot.spawn.usd_path)) + stage.SetEditTarget(stage.GetSessionLayer()) + pole_joint = next(prim for prim in stage.Traverse() if prim.GetName() == "cart_to_pole") + UsdPhysics.FixedJoint.Define(stage, pole_joint.GetPath()) + scene_cfg.robot.spawn.usd_path = str(tmp_path / "fixed_cartpole.usda") + stage.Export(scene_cfg.robot.spawn.usd_path) + scene = InteractiveScene(scene_cfg) sim.reset() sensor: JointWrenchSensor = scene["wrench"] robot: Articulation = scene["robot"] + assert sensor.body_names == ["cart", "pole"] + assert sensor._root_view is robot.root_view + assert robot.root_view.joint_names == (["slider_to_cart"] if fixed_pole else robot.joint_names) for _ in range(800): sim.step() scene.update(sim.get_physics_dt()) gravity = torch.tensor(sim.cfg.gravity, device=sim.device) - - # Interior joint (index 0, slider_to_cart): reaction wrench supports - # all bodies in the subtree — both cart and pole. - expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env=0, - joint=0, - gravity=gravity, - descendant_body_names=list(sensor.body_names), - ) - - force = sensor.data.force.torch[0, 0] - torque = sensor.data.torque.torch[0, 0] - - torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) - torch.testing.assert_close(torque, expected_torque, atol=1e-2, rtol=1e-3) + for joint, descendants in enumerate((["cart", "pole"], ["pole"])): + expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( + sensor, robot, env=0, joint=joint, gravity=gravity, descendant_body_names=descendants + ) + torch.testing.assert_close(sensor.data.force.torch[0, joint], expected_force, atol=1e-2, rtol=1e-3) + torch.testing.assert_close(sensor.data.torque.torch[0, joint], expected_torque, atol=1e-2, rtol=1e-3) # --------------------------------------------------------------------------- @@ -486,3 +487,66 @@ def test_no_stale_data_after_scene_reset(sim): post_reset_torque = sensor.data.torque.torch torch.testing.assert_close(post_reset_force, torch.zeros_like(post_reset_force)) torch.testing.assert_close(post_reset_torque, torch.zeros_like(post_reset_torque)) + + +@pytest.mark.parametrize("root_type", [newton.JointType.FREE, newton.JointType.FIXED, newton.JointType.REVOLUTE]) +def test_fixed_joint_selection(sim, monkeypatch, root_type): + """Select tree welds independently of control joints, with world roots and loops excluded.""" + from isaaclab_newton.sensors.joint_wrench import joint_wrench_sensor as sensor_module + from newton.selection import ArticulationView + + # Another articulation precedes the sensor's target in each world. The target has no + # movable internal joints; base, mount, and tool form a tree with an additional loop weld. + world = newton.ModelBuilder() + unrelated = world.add_link(label="unrelated") + world.add_articulation([world.add_joint_revolute(-1, unrelated)], label="Other") + base = world.add_link(label="base") + tool = world.add_link(label="tool") + mount = world.add_link(label="mount") + if root_type == newton.JointType.FREE: + root_joint = world.add_joint_free(base) + elif root_type == newton.JointType.FIXED: + root_joint = world.add_joint_fixed(-1, base) + else: + root_joint = world.add_joint_revolute(-1, base) + mount_joint = world.add_joint_fixed(base, mount, label="mount_joint") + wrist_joint = world.add_joint_fixed(mount, tool, label="wrist") + world.add_articulation([root_joint, mount_joint, wrist_joint], label="Robot") + world.add_joint_fixed(tool, base, label="loop_weld") + + builder = newton.ModelBuilder() + builder.request_state_attributes("body_parent_f") + for env in range(2): + # Distinct anchors catch accidental broadcasting of the first world's joint frames. + world.joint_X_c[wrist_joint] = wp.transform(wp.vec3(0.1 * (env + 1), 0.0, 0.0), wp.quat_identity()) + builder.add_world(world, label_prefix=f"/World/envs/env_{env}") + model = builder.finalize(device="cpu") + state = model.state() + root_expr = "/World/envs/env_.*/Robot" + views = {} + view_factory = Mock(wraps=ArticulationView) + monkeypatch.setattr(sensor_module, "ArticulationView", view_factory) + monkeypatch.setattr(sensor_module.NewtonManager, "views", views) + monkeypatch.setattr(sensor_module.NewtonManager, "get_model", lambda: model) + monkeypatch.setattr(sensor_module.NewtonManager, "get_state_0", lambda: state) + monkeypatch.setattr(sensor_module.BaseJointWrenchSensor, "_initialize_impl", lambda self: None) + monkeypatch.setattr(sensor_module, "resolve_matching_prims_from_source", lambda *a, **kw: [(None, root_expr)]) + sensor = sensor_module.JointWrenchSensor(JointWrenchSensorCfg(prim_path=root_expr)) + sensor._device, sensor._num_envs = "cpu", 2 + sensor._initialize_impl() + + expected_names = ["base", "mount", "tool"] if root_type == newton.JointType.REVOLUTE else ["mount", "tool"] + assert sensor.body_names == expected_names + assert sensor._data._force.shape == (2, len(expected_names)) + tool_index = sensor.find_bodies("tool")[0][0] + np.testing.assert_allclose(sensor._sim_bind_joint_X_c.numpy()[:, tool_index, 0], [0.1, 0.2]) + np.testing.assert_array_equal( + sensor._joint_child.numpy(), [0, 2, 1] if root_type == newton.JointType.REVOLUTE else [2, 1] + ) + # Sensing must not create a second view or include welds in the control-joint selection. + view = views[sensor_module.NewtonManager, root_expr] + assert sensor._root_view is view + assert view_factory.call_count == 1 + assert view.joint_count == int(root_type == newton.JointType.REVOLUTE) + assert len(views) == 1 + assert sensor._sim_bind_body_parent_f.ptr == view.get_attribute("body_parent_f", state).ptr From 37cfd637f6521dd36865f25918c303f120980acd Mon Sep 17 00:00:00 2001 From: Lynn Date: Thu, 24 Sep 2026 00:19:10 -0400 Subject: [PATCH 11/11] Fix FORGE wrench transforms and PhysX joint-wrench frames (#7967) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## Description Fixes #3513 and the PhysX joint-wrench frame discrepancy discussed in #7978. `forge_utils.change_FT_frame` used the inverse rotation and the wrong lever-arm sign when expressing a wrench in another frame. It now computes the source pose in the target frame and applies: - `f_t = R_ts f_s` - `tau_t = R_ts tau_s + p_ts × f_t` The existing independent point-force test covers translated frames with identity and arbitrary rotations. FORGE's current force-only observation remains unchanged because its call uses identity rotations. The PhysX sensor had a separate error: `get_link_incoming_joint_force()` already returns forces and torques in the child-side joint frame, about its anchor. The sensor applied `localPos1` and `localRot1` again. It now exposes the native components directly, removing the redundant USD traversal, frame buffers, and GPU transformations. This follows the [PhysX tensor API contract](https://docs.omniverse.nvidia.com/kit/docs/omni_physics/108.0/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.ArticulationView.get_link_incoming_joint_force). Replaced the circular PhysX frame test with one shared integration fixture used by both Newton and PhysX. It uses the same timestep, geometry, loads, expected values, and tolerances: a passive vertical hinge supports a 2 kg body through an offset child frame rotated 90 degrees relative to the body. The articulation is also rotated about world Z. Both backends must report `F=(0, 0, 19.62) N` and `tau=(-1.962, 4.905, 0) N·m`, calculated from gravity and the known lever arm. The test checks that the body stays at rest. No raw wrench or production conversion is used to calculate the reference. The fixture reuses the existing simple articulation asset with temporary USD overrides; no asset is added to the repository. The existing backend test files invoke the shared check. Removed the old circular reference helper. ## Validation - Newton sensor suite and FORGE tests: 12 passed. - PhysX sensor suite: 16 passed, including eager/recorded updates and reset behavior. - The shared physical test failed with the original PhysX implementation, with a 19.62 N force-component error, and passed after removing the duplicate transform. Newton passes the same analytic contract. - Architecture audit and `uv run --no-sync isaaclab -f` passed. - Recorded GPU update microbenchmark at 1024 environments × 8 links: approximately 3 microseconds per launch before and after. Initialization no longer scans USD joint frames or allocates their buffers. ## Type of change - Bug fix - Documentation update ## Checklist - [x] I have read and understood the contribution guidelines - [x] I have run the pre-commit checks - [x] I have made corresponding changes to the documentation - [x] I have added tests that prove my fix is effective - [x] I have added a changelog fragment for every touched package - [x] My name already exists in `CONTRIBUTORS.md` ## Follow-up FORGE names its raw incoming-joint wrench `force_sensor_world`, although the API reports the child joint frame. Moving FORGE's observation to an explicitly selected frame is separate work because it can change policy inputs. This PR preserves its current force observation. --------- Signed-off-by: Lynn Co-authored-by: Octi Zhang --- AGENTS.md | 2 + .../concepts/sensors/joint_wrench_sensor.rst | 4 + .../joint-wrench-frame-contract.skip | 1 + .../isaaclab/test/utils/joint_wrench.py | 76 ++++++++++++++++++ .../joint-wrench-frame-contract.skip | 1 + .../test/sensors/test_joint_wrench_sensor.py | 6 ++ .../changelog.d/fix-joint-wrench-frame.rst | 8 ++ .../joint_wrench/joint_wrench_sensor.py | 49 +----------- .../sensors/joint_wrench/kernels.py | 15 +--- .../test/sensors/test_joint_wrench_sensor.py | 80 +++---------------- .../fix-forge-ft-frame-transform.rst | 6 ++ .../contrib/forge/forge_utils.py | 30 ++++--- .../test/contrib/test_forge_utils.py | 72 +++++++++++++++++ 13 files changed, 203 insertions(+), 147 deletions(-) create mode 100644 source/isaaclab/changelog.d/joint-wrench-frame-contract.skip create mode 100644 source/isaaclab/isaaclab/test/utils/joint_wrench.py create mode 100644 source/isaaclab_newton/changelog.d/joint-wrench-frame-contract.skip create mode 100644 source/isaaclab_physx/changelog.d/fix-joint-wrench-frame.rst create mode 100644 source/isaaclab_tasks/changelog.d/fix-forge-ft-frame-transform.rst create mode 100644 source/isaaclab_tasks/test/contrib/test_forge_utils.py diff --git a/AGENTS.md b/AGENTS.md index 207b1c2dcd76..68dfe8bac59b 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -50,6 +50,8 @@ - Find and extend the closest existing test before creating a new test file or test case. - Add a test only when it covers a distinct behavior, regression, boundary, or failure mode that existing tests do not cover clearly. - Test observable behavior and public contracts, not implementation details. +- Validate joint-wrench frames with the same physical fixture and analytic load expectations across backends. + Do not derive the expected wrench by repeating the production transformation on the backend's raw output. - Use hard-coded values only when they are the intended contract or a small, independently verified example; otherwise derive the expected result from a separate, simple reference calculation. - Keep tests focused and remove or consolidate redundant coverage instead of growing overlapping test suites. - Do not add debug output to production Warp kernels. Use temporary standalone reproductions and remove debug output before committing. diff --git a/docs/source/concepts/sensors/joint_wrench_sensor.rst b/docs/source/concepts/sensors/joint_wrench_sensor.rst index 049b673392ee..5e6dd938af89 100644 --- a/docs/source/concepts/sensors/joint_wrench_sensor.rst +++ b/docs/source/concepts/sensors/joint_wrench_sensor.rst @@ -17,6 +17,10 @@ The ``incoming_joint_frame`` convention expresses the wrench in the child-side j child-side joint anchor. This matches the placement of a six-axis force/torque sensor mounted at the joint. Backend implementations convert their native solver output to this common convention. +PhysX's ``get_link_incoming_joint_force()`` already returns the wrench in the child-side joint frame, +referenced at its anchor, so the PhysX sensor exposes those components directly. Applying the USD +``localPos1`` and ``localRot1`` again would shift and rotate the wrench twice. + Configure the sensor -------------------- diff --git a/source/isaaclab/changelog.d/joint-wrench-frame-contract.skip b/source/isaaclab/changelog.d/joint-wrench-frame-contract.skip new file mode 100644 index 000000000000..8c9505780d0f --- /dev/null +++ b/source/isaaclab/changelog.d/joint-wrench-frame-contract.skip @@ -0,0 +1 @@ +Added a shared Newton/PhysX integration check with independently calculated joint-frame forces and torques. diff --git a/source/isaaclab/isaaclab/test/utils/joint_wrench.py b/source/isaaclab/isaaclab/test/utils/joint_wrench.py new file mode 100644 index 000000000000..7dc7c5526c4f --- /dev/null +++ b/source/isaaclab/isaaclab/test/utils/joint_wrench.py @@ -0,0 +1,76 @@ +# 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 + +"""Shared physical contract for joint-wrench sensor backends.""" + +from pathlib import Path + +import torch + +from pxr import Gf, Usd, UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.physics import PhysicsCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import JointWrenchSensorCfg +from isaaclab.sim import SimulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, retrieve_file_path + + +def check_joint_wrench_frame(physics: PhysicsCfg, tmp_path: Path) -> None: + """Compare a loaded joint's wrench with gravity equilibrium using either physics backend.""" + source = retrieve_file_path(f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd") + usd_path = str(tmp_path / "joint_wrench.usda") + stage = Usd.Stage.CreateNew(usd_path) + root = stage.DefinePrim("/Articulation", "Xform") + stage.SetDefaultPrim(root) + root.GetReferences().AddReference(source) + joint = next(UsdPhysics.Joint(prim) for prim in stage.Traverse() if prim.IsA(UsdPhysics.RevoluteJoint)) + UsdPhysics.RevoluteJoint(joint).GetAxisAttr().Set("Z") + joint.GetLocalPos1Attr().Set(Gf.Vec3f(0.25, -0.15, 0.1)) + joint.GetLocalRot1Attr().Set(Gf.Quatf(2.0**-0.5, Gf.Vec3f(2.0**-0.5, 0.0, 0.0))) + # Unit scale makes the authored joint offset a metric offset. Align the joint frames initially. + arm_prim = stage.GetPrimAtPath(joint.GetBody1Rel().GetTargets()[0]) + pose = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(1.0, 0.0, 0.0), -90.0)) + pose.SetTranslateOnly(Gf.Vec3d(-0.25, -0.1, -0.15)) + UsdGeom.Xformable(arm_prim).MakeMatrixXform().Set(pose) + mass = UsdPhysics.MassAPI.Apply(arm_prim) + mass.CreateMassAttr(2.0) + mass.CreateCenterOfMassAttr(Gf.Vec3f(0.0)) + for prim in stage.Traverse(): + if prim.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Set(False) + stage.GetRootLayer().Save() + + with sim_utils.build_simulation_context(sim_cfg=SimulationCfg(dt=1.0 / 200.0, physics=physics)) as sim: + sim._app_control_on_stop_handle = None + cfg = InteractiveSceneCfg(num_envs=1, env_spacing=2.0) + cfg.robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg(usd_path=usd_path), + actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0)}, + init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0), rot=(0.0, 0.0, 2.0**-0.5, 2.0**-0.5)), + ) + cfg.wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + scene = InteractiveScene(cfg) + sim.reset() + for _ in range(20): + sim.step() + scene.update(sim.get_physics_dt()) + + robot, sensor = scene["robot"], scene["wrench"] + arm = robot.body_names.index("Arm") + sensor_arm = sensor.find_bodies("Arm")[0][0] + assert robot.data.body_com_vel_w.torch[:, arm].norm() < 1e-3 + # The joint frame is rotated 90 degrees about world Z, so gravity still points along its -Z. + # Its 2 kg load is offset by (-0.25, -0.1, -0.15) m in joint coordinates. + # Reaction force is (0, 0, mg), and r x F gives torque (-0.1 mg, 0.25 mg, 0). + weight = -2.0 * sim.cfg.gravity[2] + expected_force = torch.tensor([[0.0, 0.0, weight]], device=sim.device) + expected_torque = torch.tensor([[-0.1 * weight, 0.25 * weight, 0.0]], device=sim.device) + torch.testing.assert_close(sensor.data.force.torch[:, sensor_arm], expected_force, atol=1e-2, rtol=1e-3) + torch.testing.assert_close(sensor.data.torque.torch[:, sensor_arm], expected_torque, atol=1e-2, rtol=1e-3) diff --git a/source/isaaclab_newton/changelog.d/joint-wrench-frame-contract.skip b/source/isaaclab_newton/changelog.d/joint-wrench-frame-contract.skip new file mode 100644 index 000000000000..d6df16959500 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/joint-wrench-frame-contract.skip @@ -0,0 +1 @@ +Validated the joint-wrench sensor against the same physical fixture and analytic expectations as PhysX. diff --git a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py index d9274b38ed39..9353b1f0831d 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -28,6 +28,7 @@ from isaaclab.sensors.joint_wrench import JointWrenchSensor, JointWrenchSensorCfg from isaaclab.sim import SimulationCfg from isaaclab.terrains import TerrainImporterCfg +from isaaclab.test.utils.joint_wrench import check_joint_wrench_frame from isaaclab.utils import configclass from isaaclab.utils import math as math_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path @@ -347,6 +348,11 @@ def test_force_and_torque_components_at_rest(sim): torch.testing.assert_close(torque, expected_torque, atol=1e-2, rtol=1e-3) +def test_non_identity_joint_frame_transform(tmp_path): + """Newton must satisfy the same physical joint-frame contract as PhysX.""" + check_joint_wrench_frame(NewtonCfg(solver_cfg=MJWarpSolverCfg(), num_substeps=1), tmp_path) + + def test_wrench_with_external_force_and_torque(sim): """Full analytical wrench validation with external force and torque applied. diff --git a/source/isaaclab_physx/changelog.d/fix-joint-wrench-frame.rst b/source/isaaclab_physx/changelog.d/fix-joint-wrench-frame.rst new file mode 100644 index 000000000000..9f8d6eb12de2 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/fix-joint-wrench-frame.rst @@ -0,0 +1,8 @@ +Fixed +^^^^^ + +* Fixed joint-wrench sensors applying an extra frame transformation to PhysX readings, which already used + the child-side joint frame and anchor. Removed the redundant USD frame buffers. Force and torque values + changed for joints with non-identity child frames; the sensor's documented frame convention was preserved. +* Replaced circular frame-conversion checks with a shared Newton/PhysX integration test using a known mass, + gravity, and lever arm to calculate the expected nonzero wrench independently. diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py index e6597e85d107..8428a2d15963 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py @@ -11,10 +11,9 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -import numpy as np import warp as wp -from pxr import Usd, UsdPhysics +from pxr import UsdPhysics from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor from isaaclab.sim.utils.queries import path_expr_to_glob, resolve_matching_prims_from_source @@ -61,8 +60,6 @@ def __init__(self, cfg: JointWrenchSensorCfg): self._data = JointWrenchSensorData() self._root_view: physx.ArticulationView | None = None - self._joint_pos_b: wp.array | None = None - self._joint_quat_b: wp.array | None = None self._num_bodies: int = 0 self._raw_incoming_joint_wrench: wp.array | None = None self._update_cmd: wp.Launch | None = None @@ -143,49 +140,11 @@ def has_articulation_root_api(prim) -> bool: raise RuntimeError(f"Joint wrench sensor matched zero bodies at '{self.cfg.prim_path}'.") self._data._body_names = list(self._root_view.shared_metatype.link_names) - self._create_joint_frame_buffers() self._data.create_buffers(num_envs=self._num_envs, num_bodies=self._num_bodies, device=self._device) self._use_recorded_launch = wp.get_device(self._device).is_cuda logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_bodies} bodies") - def _create_joint_frame_buffers(self) -> None: - """Create child-side joint frame transforms indexed by PhysX link order.""" - joint_pos_b = np.zeros((self._num_bodies, 3), dtype=np.float32) - joint_quat_b = np.zeros((self._num_bodies, 4), dtype=np.float32) - joint_quat_b[:, 3] = 1.0 - - first_env_matching_prim = resolve_matching_prims_from_source(self.cfg.prim_path)[0][0] - link_name_to_index = {name: index for index, name in enumerate(self._data._body_names)} - - for prim in Usd.PrimRange(first_env_matching_prim): - joint = UsdPhysics.Joint(prim) - if not joint or joint.GetJointEnabledAttr().Get() is False: - continue - body1_targets = joint.GetBody1Rel().GetTargets() - if len(body1_targets) == 0: - continue - body_index = link_name_to_index.get(body1_targets[0].name) - if body_index is None: - continue - - local_pos1 = joint.GetLocalPos1Attr().Get() - if local_pos1 is not None: - joint_pos_b[body_index] = (float(local_pos1[0]), float(local_pos1[1]), float(local_pos1[2])) - - local_rot1 = joint.GetLocalRot1Attr().Get() - if local_rot1 is not None: - local_rot1_imag = local_rot1.GetImaginary() - joint_quat_b[body_index] = ( - float(local_rot1_imag[0]), - float(local_rot1_imag[1]), - float(local_rot1_imag[2]), - float(local_rot1.GetReal()), - ) - - self._joint_pos_b = wp.array(joint_pos_b, dtype=wp.vec3f, device=self._device) - self._joint_quat_b = wp.array(joint_quat_b, dtype=wp.quatf, device=self._device) - def _update_buffers_impl(self, env_mask: wp.array) -> None: """Read PhysX incoming joint wrenches and split them into force / torque buffers. @@ -197,8 +156,6 @@ def _update_buffers_impl(self, env_mask: wp.array) -> None: f"Joint wrench sensor '{self.cfg.prim_path}': not initialized." " Access sensor data only after sim.reset() has been called." ) - if self._joint_pos_b is None or self._joint_quat_b is None: - raise RuntimeError(f"Joint wrench sensor '{self.cfg.prim_path}': joint frame buffers are not initialized.") # Refresh the PhysX buffer every update, but create its typed Warp view only once: # the getter lazily allocates its output buffer and refreshes the same memory in place @@ -239,8 +196,6 @@ def _launch_update(self, env_mask: wp.array, record_cmd: bool = False) -> wp.Lau inputs=[ env_mask, self._raw_incoming_joint_wrench, - self._joint_pos_b, - self._joint_quat_b, self._timestamp, self._data._force, self._data._torque, @@ -257,8 +212,6 @@ def _invalidate_initialize_callback(self, event) -> None: """ super()._invalidate_initialize_callback(event) self._root_view = None - self._joint_pos_b = None - self._joint_quat_b = None self._num_bodies = 0 self._raw_incoming_joint_wrench = None self._update_cmd = None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py index 7a580ca836cc..d07918d9baa3 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py @@ -10,13 +10,11 @@ def joint_wrench_split_kernel( env_mask: wp.array(dtype=wp.bool), incoming_joint_wrench: wp.array(dtype=wp.spatial_vectorf, ndim=2), - joint_pos_b: wp.array(dtype=wp.vec3f), - joint_quat_b: wp.array(dtype=wp.quatf), timestamp: wp.array(dtype=wp.float32), out_force: wp.array(dtype=wp.vec3f, ndim=2), out_torque: wp.array(dtype=wp.vec3f, ndim=2), ): - """Convert PhysX incoming joint spatial wrenches into the child-side joint frame.""" + """Split PhysX wrenches, already expressed at the child-side joint anchor in its frame.""" env, body = wp.tid() if not env_mask[env]: return @@ -27,15 +25,8 @@ def joint_wrench_split_kernel( return wrench = incoming_joint_wrench[env, body] - force_b = wp.spatial_top(wrench) - torque_b = wp.spatial_bottom(wrench) - - # PhysX reports the wrench in body1's frame, referenced at body1's origin. - # Shift torque to the child-side joint anchor and rotate both components - # into the child-side joint frame. - torque_joint_anchor_b = torque_b - wp.cross(joint_pos_b[body], force_b) - out_force[env, body] = wp.quat_rotate_inv(joint_quat_b[body], force_b) - out_torque[env, body] = wp.quat_rotate_inv(joint_quat_b[body], torque_joint_anchor_b) + out_force[env, body] = wp.spatial_top(wrench) + out_torque[env, body] = wp.spatial_bottom(wrench) @wp.kernel diff --git a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py index 9e5f15cb8712..85f2fa918e67 100644 --- a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py @@ -12,7 +12,6 @@ """Rest everything follows.""" -import math from types import SimpleNamespace import pytest @@ -24,8 +23,6 @@ from isaaclab_physx.sensors.joint_wrench.joint_wrench_sensor_data import JointWrenchSensorData from isaaclab_physx.sim.schemas import PhysxJointCfg -from pxr import Gf, UsdPhysics - import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg @@ -34,8 +31,8 @@ from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor from isaaclab.sim import SimulationCfg from isaaclab.terrains import TerrainImporterCfg +from isaaclab.test.utils.joint_wrench import check_joint_wrench_frame from isaaclab.utils import configclass -from isaaclab.utils import math as math_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab_assets.robots.ant import ANT_CFG @@ -149,49 +146,10 @@ def _physx_incoming_joint_wrench(sensor: JointWrenchSensor) -> torch.Tensor: def _assert_sensor_matches_physx_tensor(sensor: JointWrenchSensor) -> None: - """Compare sensor buffers to the raw PhysX tensor transformed into joint frames.""" + """The sensor exposes the PhysX tensor's existing child-joint-frame components.""" raw_wrench = _physx_incoming_joint_wrench(sensor) - sensor_data = sensor.data - - expected_force, expected_torque = _physx_incoming_joint_wrench_in_joint_frame(sensor, raw_wrench) - torch.testing.assert_close(sensor_data.force.torch, expected_force) - torch.testing.assert_close(sensor_data.torque.torch, expected_torque) - - -def _physx_incoming_joint_wrench_in_joint_frame( - sensor: JointWrenchSensor, raw_wrench: torch.Tensor -) -> tuple[torch.Tensor, torch.Tensor]: - """Transform raw PhysX body-frame incoming joint wrenches into the configured convention.""" - force_b = raw_wrench[..., :3] - torque_b = raw_wrench[..., 3:] - joint_pos_b = wp.to_torch(sensor._joint_pos_b).unsqueeze(0) - joint_quat_b = wp.to_torch(sensor._joint_quat_b).unsqueeze(0) - torque_joint_anchor_b = torque_b - torch.cross(joint_pos_b.expand_as(force_b), force_b, dim=-1) - - flat_joint_quat_b = joint_quat_b.expand_as(raw_wrench[..., :4]).reshape(-1, 4) - expected_force = math_utils.quat_apply_inverse(flat_joint_quat_b, force_b.reshape(-1, 3)).reshape(force_b.shape) - expected_torque = math_utils.quat_apply_inverse(flat_joint_quat_b, torque_joint_anchor_b.reshape(-1, 3)).reshape( - torque_b.shape - ) - return expected_force, expected_torque - - -def _set_child_joint_frame(scene: InteractiveScene, child_body_name: str) -> None: - """Set a non-identity child-side joint frame for the requested body in env 0.""" - for prim in scene.stage.Traverse(): - if not prim.GetPath().pathString.startswith("/World/envs/env_0/Robot"): - continue - joint = UsdPhysics.Joint(prim) - if joint and any(target.name == child_body_name for target in joint.GetBody1Rel().GetTargets()): - joint.GetLocalPos1Attr().Set(Gf.Vec3f(0.25, -0.15, 0.1)) - joint.GetLocalRot1Attr().Set( - Gf.Quatf( - math.cos(math.pi / 4.0), - Gf.Vec3f(math.sin(math.pi / 4.0), 0.0, 0.0), - ) - ) - return - raise RuntimeError(f"Failed to find a USD joint with child body '{child_body_name}'.") + torch.testing.assert_close(sensor.data.force.torch, raw_wrench[..., :3]) + torch.testing.assert_close(sensor.data.torque.torch, raw_wrench[..., 3:]) # --------------------------------------------------------------------------- @@ -292,29 +250,9 @@ def test_force_and_torque_components_at_rest(sim): assert torch.any(raw_wrench[:, arm_idx, :] != 0.0) -def test_non_identity_joint_frame_transform(sim): - """PhysX raw body-frame wrench is converted to the child-side joint frame.""" - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) - _set_child_joint_frame(scene, "Arm") - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - robot: Articulation = scene["robot"] - arm_idx = robot.body_names.index("Arm") - - for _ in range(400): - sim.step() - scene.update(sim.get_physics_dt()) - - raw_wrench = _physx_incoming_joint_wrench(sensor) - expected_force, expected_torque = _physx_incoming_joint_wrench_in_joint_frame(sensor, raw_wrench) - torch.testing.assert_close(sensor.data.force.torch, expected_force) - torch.testing.assert_close(sensor.data.torque.torch, expected_torque) - - raw_force = raw_wrench[:, arm_idx, :3] - raw_torque = raw_wrench[:, arm_idx, 3:] - assert not torch.allclose(sensor.data.force.torch[:, arm_idx], raw_force) - assert not torch.allclose(sensor.data.torque.torch[:, arm_idx], raw_torque) +def test_non_identity_joint_frame_transform(tmp_path): + """PhysX must satisfy the same physical joint-frame contract as Newton.""" + check_joint_wrench_frame(PhysxCfg(), tmp_path) def test_wrench_with_external_force_and_torque(sim): @@ -499,8 +437,6 @@ def _make_joint_wrench_sensor(use_recorded_launch: bool = True, num_envs: int = sensor._num_envs = num_envs sensor._num_bodies = 1 sensor._root_view = root_view - sensor._joint_pos_b = wp.zeros(1, dtype=wp.vec3f, device=device) - sensor._joint_quat_b = wp.array([wp.quatf(0.0, 0.0, 0.0, 1.0)], dtype=wp.quatf, device=device) sensor._timestamp = wp.ones(num_envs, dtype=wp.float32, device=device) sensor._data = JointWrenchSensorData() sensor._data.create_buffers(num_envs=num_envs, num_bodies=1, device=device) @@ -520,6 +456,8 @@ def _make_joint_wrench_sensor(use_recorded_launch: bool = True, num_envs: int = @pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") def test_joint_wrench_caches_physx_wrench_view(): """Repeated eager updates should reuse one typed view over the refreshed PhysX buffer.""" + # The native tensor already owns the frame; do not reconstruct it from USD. + assert not hasattr(PhysxJointWrenchSensor, "_create_joint_frame_buffers") sensor, root_view, _, env_mask = _make_joint_wrench_sensor(use_recorded_launch=False) sensor._update_buffers_impl(env_mask) diff --git a/source/isaaclab_tasks/changelog.d/fix-forge-ft-frame-transform.rst b/source/isaaclab_tasks/changelog.d/fix-forge-ft-frame-transform.rst new file mode 100644 index 000000000000..f66a54100585 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/fix-forge-ft-frame-transform.rst @@ -0,0 +1,6 @@ +Fixed +^^^^^ + +* Fixed :func:`~isaaclab_tasks.contrib.forge.forge_utils.change_FT_frame` applying the inverse rotation and the + wrong lever-arm sign when re-expressing a force/torque reading in another frame. The FORGE force observation + is unchanged because the environment uses identity rotations and only consumes the force components. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/forge/forge_utils.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/forge/forge_utils.py index 0e575b14c88c..a791074f5968 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/forge/forge_utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/forge/forge_utils.py @@ -5,7 +5,7 @@ import torch -from isaaclab.utils.math import combine_frame_transforms, quat_apply, quat_inv +from isaaclab.utils.math import quat_apply, subtract_frame_transforms def get_random_prop_gains(default_values, noise_levels, num_envs, device): @@ -24,25 +24,23 @@ def get_random_prop_gains(default_values, noise_levels, num_envs, device): def change_FT_frame(source_F, source_T, source_frame, target_frame): """Convert force/torque reading from source to target frame. + The wrench is re-expressed in the target frame's axes and the torque is taken about the + target frame's origin. + Args: - source_F: Force in source frame. - source_T: Torque in source frame. - source_frame: Tuple of (quat_xyzw, pos) for source frame. - target_frame: Tuple of (quat_xyzw, pos) for target frame. + source_F: Force in source frame [N]. Shape is (N, 3). + source_T: Torque in source frame, about the source frame's origin [N·m]. Shape is (N, 3). + source_frame: Tuple of (quat_xyzw, pos) for source frame, both expressed in a common frame. + target_frame: Tuple of (quat_xyzw, pos) for target frame, both expressed in the same common frame. Returns: - Tuple of (target_F, target_T) - force and torque in target frame. + Tuple of (target_F, target_T) - force [N] and torque [N·m] in target frame. """ - # Modern Robotics eq. 3.95 - # Compute inverse of source frame - source_quat_inv = quat_inv(source_frame[0]) - source_pos_inv = -quat_apply(source_quat_inv, source_frame[1]) - - # Combine: source_inv * target = target_T_source - target_T_source_pos, target_T_source_quat = combine_frame_transforms( - source_pos_inv, source_quat_inv, target_frame[1], target_frame[0] + # Modern Robotics eq. 3.95: F_t = Ad_{T_st}^T F_s, written in terms of T_ts (source pose in the target frame). + source_pos_in_target, source_quat_in_target = subtract_frame_transforms( + target_frame[1], target_frame[0], source_frame[1], source_frame[0] ) - target_F = quat_apply(target_T_source_quat, source_F) - target_T = quat_apply(target_T_source_quat, (source_T + torch.cross(target_T_source_pos, source_F, dim=-1))) + target_F = quat_apply(source_quat_in_target, source_F) + target_T = quat_apply(source_quat_in_target, source_T) + torch.cross(source_pos_in_target, target_F, dim=-1) return target_F, target_T diff --git a/source/isaaclab_tasks/test/contrib/test_forge_utils.py b/source/isaaclab_tasks/test/contrib/test_forge_utils.py new file mode 100644 index 000000000000..0380c2f732bc --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_forge_utils.py @@ -0,0 +1,72 @@ +# 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 + +import importlib.util +from pathlib import Path + +import pytest +import torch + +from isaaclab.utils.math import quat_from_matrix + +pytestmark = pytest.mark.unit + + +def _load_forge_utils_module(): + module_path = Path(__file__).parents[2] / "isaaclab_tasks" / "contrib" / "forge" / "forge_utils.py" + spec = importlib.util.spec_from_file_location("forge_utils_under_test", module_path) + module = importlib.util.module_from_spec(spec) + assert spec.loader is not None + spec.loader.exec_module(module) + return module + + +def _random_rotation_matrices(num: int, generator: torch.Generator) -> torch.Tensor: + q, r = torch.linalg.qr(torch.randn((num, 3, 3), generator=generator, dtype=torch.float64)) + q = q * torch.sign(torch.diagonal(r, dim1=-2, dim2=-1)).unsqueeze(-2) + # Flip one column where needed so every matrix is a proper rotation. + q[torch.linalg.det(q) < 0, :, 0] *= -1.0 + return q + + +def _wrench_in_frame(points, forces, rot, pos): + """Reference wrench of point forces, expressed in the frame (rot, pos) and taken about its origin.""" + force = forces.sum(dim=1) + torque = torch.cross(points - pos.unsqueeze(1), forces, dim=-1).sum(dim=1) + rot_t = rot.transpose(-2, -1) + return (rot_t @ force.unsqueeze(-1)).squeeze(-1), (rot_t @ torque.unsqueeze(-1)).squeeze(-1) + + +@pytest.mark.parametrize("rotate_frames", [False, True]) +def test_change_FT_frame_matches_point_force_reference(rotate_frames): + forge_utils = _load_forge_utils_module() + generator = torch.Generator().manual_seed(0) + num = 64 + + # Two point forces per sample so the wrench has a non-trivial torque about any origin. + points = torch.randn((num, 2, 3), generator=generator, dtype=torch.float64) + forces = torch.randn((num, 2, 3), generator=generator, dtype=torch.float64) + + source_pos = torch.randn((num, 3), generator=generator, dtype=torch.float64) + target_pos = torch.randn((num, 3), generator=generator, dtype=torch.float64) + if rotate_frames: + source_rot = _random_rotation_matrices(num, generator) + target_rot = _random_rotation_matrices(num, generator) + else: + source_rot = torch.eye(3, dtype=torch.float64).expand(num, 3, 3) + target_rot = source_rot + + source_F, source_T = _wrench_in_frame(points, forces, source_rot, source_pos) + expected_F, expected_T = _wrench_in_frame(points, forces, target_rot, target_pos) + + target_F, target_T = forge_utils.change_FT_frame( + source_F, + source_T, + (quat_from_matrix(source_rot), source_pos), + (quat_from_matrix(target_rot), target_pos), + ) + + torch.testing.assert_close(target_F, expected_F) + torch.testing.assert_close(target_T, expected_T)