From 8ae33e523f977aed1c54f38c2ada743823759eaa Mon Sep 17 00:00:00 2001 From: ooctipus Date: Fri, 25 Sep 2026 04:15:59 -0700 Subject: [PATCH 1/7] Fix flat-ground visual coverage for large environment batches (#8025) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## Description Keep 50 m of visible flat ground beyond each edge of the environment grid. Previously, large batches had only one environment spacing of margin, so locomotion robots could walk off the visible mesh while still standing on the infinite collision plane. | Environments | Spacing | Visual width before → after | Margin per side before → after | |---|---|---|---| | 1 | 1 m | 100 → 100 m | 50 → 50 m | | 4,096 | 4 m | 260 → 352 m | 4 → 50 m | | 16,384 | 2.5 m | 322.5 → 417.5 m | 2.5 → 50 m | The existing UV rescaling preserves **1 m checker cells / 2 m texture repeats** at every size. The infinite collision plane and explicit `import_ground_plane(size=...)` overrides are unchanged. **No rough-terrain, heightfield, solver, or task-parameter changes.** ## Validation - `uv run isaaclab -f`: passed. - Default-ground-plane asset/material tests: 3 passed. - CPU USD authoring checks at 1 / 4,096 / 16,384 origins verified the actual transformed mesh bounds and metric UV repeats. The margin assertion failed on the base and passed with this fix. - Headless GL before/after images at 20 m beyond a 16,384-environment grid corner: the old mesh ended behind the marker; the enlarged mesh covered it with unchanged checker size. This used a tiny static scene, not 16,384 simulated robots. - A 16-environment Go2-flat checkpoint replay passed 300 policy steps in the active training checkout with this same ground-size fix applied; robot and floor remained visible. - Fresh `develop` with Newton 1.6.0 produced a blank viewport with **both** the base and patched ground-size formulas (pixel-identical control frames). That playback was not counted as a successful visual check. - Extended the existing terrain-importer integration test across those batch sizes. The full Kit integration suite was not run locally. ## Type of change Bug fix (visualization only). ## Release backport - [ ] Backport this pull request to the active release branch after it merges into `develop` ## Checklist - [x] Formatting checks passed. - [x] Updated the API docstring and existing regression test. - [x] Added an `isaaclab` changelog fragment. Co-authored-by: ooctipus --- .../ground-plane-walking-margin.rst | 6 +++ .../isaaclab/terrains/terrain_importer.py | 8 ++-- .../test/terrains/test_terrain_importer.py | 41 +++++++++---------- 3 files changed, 29 insertions(+), 26 deletions(-) create mode 100644 source/isaaclab/changelog.d/ground-plane-walking-margin.rst diff --git a/source/isaaclab/changelog.d/ground-plane-walking-margin.rst b/source/isaaclab/changelog.d/ground-plane-walking-margin.rst new file mode 100644 index 00000000000..c878f766937 --- /dev/null +++ b/source/isaaclab/changelog.d/ground-plane-walking-margin.rst @@ -0,0 +1,6 @@ +Fixed +^^^^^ + +* Extended the default visual ground plane 50 m beyond the outermost environment + origins so locomotion robots did not immediately leave the visible floor in + large batches. Preserved metric texture tiling and the infinite collision plane. diff --git a/source/isaaclab/isaaclab/terrains/terrain_importer.py b/source/isaaclab/isaaclab/terrains/terrain_importer.py index 9629c9c4882..cb13afdefa3 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_importer.py +++ b/source/isaaclab/isaaclab/terrains/terrain_importer.py @@ -193,8 +193,8 @@ def import_ground_plane(self, name: str, size: tuple[float, float] | None = None Args: name: The name of the imported terrain. This name is used to create the USD prim corresponding to the terrain. - size: The visual size of the plane [m]. If None, the visual mesh covers the configured - environment grid with a 100 m minimum. The collision plane remains infinite. + size: The visual size of the plane [m]. If None, the visual mesh extends 50 m beyond + each side of the configured environment grid. The collision plane remains infinite. Raises: ValueError: If a terrain with the same name already exists. @@ -256,11 +256,11 @@ def import_mesh(self, name: str, mesh: trimesh.Trimesh): ) def _compute_ground_plane_size(self) -> tuple[float, float]: - """Compute a bounded visual plane size that covers the environment grid [m].""" + """Cover the environment grid with 50 m of visual walking room on each side [m].""" num_rows = int(np.ceil(self.cfg.num_envs / np.sqrt(self.cfg.num_envs))) num_cols = int(np.ceil(self.cfg.num_envs / num_rows)) spacing = self.cfg.env_spacing or 0.0 - return (max(100.0, (num_rows + 1) * spacing), max(100.0, (num_cols + 1) * spacing)) + return ((num_rows - 1) * spacing + 100.0, (num_cols - 1) * spacing + 100.0) def _is_heightfield_collider_requested(self, cfg: TerrainGeneratorCfg) -> bool: """Check whether the generated terrain should be collided against as a heightfield. diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index 1ef078c5fd2..e8cd3f847fb 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -81,7 +81,8 @@ def test_visual_material_defaults(): assert unmaterialized_generator_cfg.visual_material is None -def test_plane(): +@pytest.mark.parametrize("num_envs,env_spacing", [(1, 1.0), (4096, 4.0), (16384, 2.5)]) +def test_plane(num_envs, env_spacing): """Generates a plane and tests that the resulting mesh has the correct size.""" with build_simulation_context(device="cuda:0", auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -90,47 +91,43 @@ def test_plane(): terrain_importer_cfg = terrain_gen.TerrainImporterCfg( prim_path="/World/ground", terrain_type="plane", - num_envs=4096, - env_spacing=4.0, + num_envs=num_envs, + env_spacing=env_spacing, visual_material=PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), ) terrain_importer = TerrainImporter(terrain_importer_cfg) - # The importer lays the environments out on a 64 x 64 grid with the configured spacing. + # These square grids are centered on the world origin. origins = terrain_importer.env_origins - assert origins.shape == (4096, 3) + half_span = (int(np.sqrt(num_envs)) - 1) * env_spacing / 2.0 + assert origins.shape == (num_envs, 3) assert origins.device == torch.device("cuda:0") - torch.testing.assert_close(origins[0].cpu(), torch.tensor([126.0, -126.0, 0.0])) - torch.testing.assert_close(origins[-1].cpu(), torch.tensor([-126.0, 126.0, 0.0])) + torch.testing.assert_close(origins[0].cpu(), torch.tensor([half_span, -half_span, 0.0])) + torch.testing.assert_close(origins[-1].cpu(), torch.tensor([-half_span, half_span, 0.0])) # check if mesh prim path exists mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" assert mesh_prim_path in terrain_importer.terrain_prim_paths - # The visual mesh is bounded to the environment grid while the collision Plane stays infinite. + # Leave walking room beyond the outermost origins; collision remains infinite. + origins = terrain_importer.env_origins.cpu().numpy() + half_size = np.max(np.abs(origins[:, :2]), axis=0) + 50.0 + expected_scale = (*tuple(half_size / 50.0), 1.0) environment = sim.stage.GetPrimAtPath(f"{mesh_prim_path}/Environment") - assert tuple(environment.GetAttribute("xformOp:scale").Get()) == pytest.approx((2.6, 2.6, 1.0)) + assert tuple(environment.GetAttribute("xformOp:scale").Get()) == pytest.approx(expected_scale) visual_mesh = UsdGeom.Mesh(sim.stage.GetPrimAtPath(f"{mesh_prim_path}/Environment/Geometry")) - assert [tuple(uv) for uv in UsdGeom.PrimvarsAPI(visual_mesh).GetPrimvar("st").Get()] == [ - (-65.0, -65.0), - (65.0, -65.0), - (65.0, 65.0), - (-65.0, 65.0), - ] + uvs = np.asarray(UsdGeom.PrimvarsAPI(visual_mesh).GetPrimvar("st").Get()) + # A texture repeat remains 2 m even when the visual plane grows. + np.testing.assert_allclose(np.ptp(uvs, axis=0) * 2.0, half_size * 2.0) # Direct imports use the same bounded default instead of the legacy 2,000 km visual mesh. terrain_importer.import_ground_plane("direct") direct_environment = sim.stage.GetPrimAtPath(f"{terrain_importer.cfg.prim_path}/direct/Environment") - assert tuple(direct_environment.GetAttribute("xformOp:scale").Get()) == pytest.approx((2.6, 2.6, 1.0)) + assert tuple(direct_environment.GetAttribute("xformOp:scale").Get()) == pytest.approx(expected_scale) direct_mesh = UsdGeom.Mesh( sim.stage.GetPrimAtPath(f"{terrain_importer.cfg.prim_path}/direct/Environment/Geometry") ) - assert [tuple(uv) for uv in UsdGeom.PrimvarsAPI(direct_mesh).GetPrimvar("st").Get()] == [ - (-65.0, -65.0), - (65.0, -65.0), - (65.0, 65.0), - (-65.0, 65.0), - ] + np.testing.assert_allclose(UsdGeom.PrimvarsAPI(direct_mesh).GetPrimvar("st").Get(), uvs) # obtain underling mesh mesh = _obtain_collision_mesh(mesh_prim_path, mesh_type="Plane") From 5d208f81d4f8895b4e564ebebce232031171a3e8 Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Fri, 25 Sep 2026 04:32:52 -0700 Subject: [PATCH 2/7] [Core] Resolve nested local and remote USD dependencies (#8011) ## Summary USD composition could fail for `scene.usda -> local robot.usda -> remote asset.usd`, or when a downloaded layer still referenced a remote URL. The common USD reference loader now prepares both local and remote files through one recursive dependency traversal. Changed layers are written as working copies, preserving authored files and raw downloads. Renderer-provided MDL identifiers such as `OmniPBR.mdl` and self-contained USDZ packages retain their resolution behavior. Completed managed local trees skip discovery while their file stamps match; incomplete downloads remain retryable, and remote URL requests retain freshness checks. Fixes #7999 ## Type of change - Bug fix (non-breaking change which fixes an issue) ## Validation - Full asset test suite: 66 passed. - Extended the existing composition test with renderer module identifiers and a remote USDZ case; all four composition cases fail on the previous PR revision and pass with this change. - Verified the common reference loader composes a real remote Cartpole through two local wrapper layers. - All 48 previously failing CI cases passed locally with Isaac Sim 6.1.0.0, OVRTX 0.5.0.377615, and OVPhysX 0.6.3. These include the existing Kit and Kitless image comparisons, visualizers, scene partitioning, USDZ demos, and later core/contributed-environment failures. Golden images and rendering thresholds were unchanged. - `uv run isaaclab -f`: passed, including changelog validation against upstream develop. ## Checklist - [x] I have read and understood the contribution guidelines - [x] I have run the pre-commit checks with `uv run isaaclab -f` - [x] My changes generate no new warnings - [x] I have added a test that proves the fix - [x] I have added a changelog fragment for the touched package - [x] My name is already in `CONTRIBUTORS.md` --------- Co-authored-by: Octi Zhang --- .../resolve-local-usd-remote-dependencies.rst | 4 + .../sim/spawners/from_files/from_files.py | 7 - source/isaaclab/isaaclab/sim/utils/prims.py | 13 +- source/isaaclab/isaaclab/utils/assets.py | 292 +++++++++--------- source/isaaclab/test/utils/test_assets.py | 93 +++++- 5 files changed, 245 insertions(+), 164 deletions(-) create mode 100644 source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst diff --git a/source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst b/source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst new file mode 100644 index 00000000000..d853cf5d5b3 --- /dev/null +++ b/source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst @@ -0,0 +1,4 @@ +Fixed +^^^^^ + +* Followed USD dependencies through both local layers and remote assets before adding references, resolving nested remote references in working copies without editing authored layers or raw downloads. Preserved renderer-provided MDL identifiers and self-contained USDZ packages. Completed local trees skipped repeated traversal until their files changed or disappeared. 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 04a74addeda..29c13932c21 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -14,7 +14,6 @@ import numpy as np from filelock import FileLock -from isaaclab.utils.assets import check_file_path, retrieve_file_path from isaaclab.utils.version import has_kit from ... import converters, schemas @@ -711,17 +710,11 @@ def _spawn_from_usd_file( # the same cached USD files cause segfaults in Sdf_CrateFile::_MmapStream::Read. _world_size = int(os.environ.get("LOCAL_WORLD_SIZE", "1")) - file_status = check_file_path(usd_path) - if file_status == 0: - raise FileNotFoundError(f"USD file not found at path: '{usd_path}'.") - if _world_size > 1: lock = FileLock(os.path.join(tempfile.gettempdir(), "isaaclab_usd_spawn.lock")) else: lock = nullcontext() with lock: - if file_status == 2: - usd_path = retrieve_file_path(usd_path, force_download=False) stage = get_current_stage() if not stage.GetPrimAtPath(prim_path).IsValid(): create_prim( diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 185ef9f6529..27b2b0088e0 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -16,7 +16,7 @@ import torch -from ...utils.assets import check_file_path, retrieve_file_path +from ...utils.assets import retrieve_file_path from ...utils.string import to_camel_case from .queries import ( find_matching_prim_paths, @@ -912,16 +912,9 @@ def add_usd_reference( Raises: FileNotFoundError: When the input USD file is not found at the specified path. + RuntimeError: When retrieving the file or adding the USD reference fails. """ - # resolve remote USD paths to local (same as Newton / add_reference_to_stage) - file_status = check_file_path(usd_path) - if file_status == 0: - raise FileNotFoundError(f"Unable to open the usd file at path: {usd_path}") - if file_status == 2: - try: - usd_path = retrieve_file_path(usd_path, force_download=False) - except Exception as e: - raise FileNotFoundError(f"Failed to retrieve USD file from {usd_path}") from e + usd_path = retrieve_file_path(usd_path) stage = get_current_stage() if stage is None else stage prim = stage.GetPrimAtPath(prim_path) diff --git a/source/isaaclab/isaaclab/utils/assets.py b/source/isaaclab/isaaclab/utils/assets.py index 5f83354e799..8683da835ed 100644 --- a/source/isaaclab/isaaclab/utils/assets.py +++ b/source/isaaclab/isaaclab/utils/assets.py @@ -23,6 +23,7 @@ import subprocess import tempfile import uuid +from collections.abc import Iterator from types import ModuleType from typing import Literal, NotRequired, TypedDict from urllib.parse import urlparse @@ -34,7 +35,8 @@ logger = logging.getLogger(__name__) _UDIM_RE = re.compile(r"", re.IGNORECASE) -_USD_EXTENSIONS = {".usd", ".usda", ".usdc", ".usdz"} +# USDZ packages own their internal dependency layout and must not be rewritten. +_USD_EXTENSIONS = {".usd", ".usda", ".usdc"} _MDL_RESOURCE_RE = re.compile(r'"([^"\\]*(?:\\.[^"\\]*)*)"|/\*.*?\*/|//[^\r\n]*', re.DOTALL) _MDL_TEXTURE_RE = re.compile(r"\.(?:bmp|dds|exr|hdr|ies|jpe?g|ktx2?|png|tga|tiff?|tx)(?:[?#].*)?$", re.IGNORECASE) _MDL_MODULE_PATTERN = r"(?:(?:\.\.::)++|\.::)[A-Za-z_]\w*+(?:::[A-Za-z_]\w*+)*+(?:::\*)?" @@ -246,6 +248,9 @@ def _resolve_asset_root() -> str: """Source URL per locally cached copy, recorded as the copy is located rather than recovered from its path, so a cache path is never inferred from a directory that merely looks like one.""" +_LOCALIZED_ASSETS: dict[str, tuple[str, dict[str, tuple[int, int]]]] = {} +"""Original roots and file stamps of completed managed trees, recorded per process.""" + _GIT_SSH_RE = re.compile(r"^[^@/:]+@[^:]+:.+") @@ -620,9 +625,11 @@ def check_file_path(path: str) -> Literal[0, 1, 2]: def retrieve_file_path(path: str, download_dir: str | None = None, force_download: bool = False) -> str: """Retrieves the path to a file on the Nucleus Server or locally. - If the file exists locally, then the absolute path to the file is returned. - If the file exists on the Nucleus Server, then the file is downloaded to the local machine - and the absolute path to the file is returned. + Dependencies are traversed through local files and remote URLs. Changed USD layers + are written to working copies, preserving authored layers and raw downloads. Completed + managed copies skip discovery while their file stamps match. Remote URLs retain the + normal server freshness checks. Renderer module identifiers and USDZ packages + retain their existing resolution behavior. Args: path: The path to the file. @@ -636,95 +643,146 @@ def retrieve_file_path(path: str, download_dir: str | None = None, force_downloa Raises: FileNotFoundError: When the file not found locally or on Nucleus Server. - RuntimeError: When the file cannot be copied from the Nucleus Server to the local machine. This - can happen when the file already exists locally and :attr:`force_download` is set to False. + RuntimeError: When the root file or a forced dependency download fails, or a resolved USD copy + cannot be saved. """ - # check file status - file_status = check_file_path(path) - if file_status == 1: - return os.path.abspath(path) - elif file_status == 2: - omni_client = _get_omni_client() - - from ..app.loading_screen import report_activity + # Only trees completed by this process may bypass discovery. A download directory + # can also contain raw mirrors, caller-owned files, or interrupted downloads. + local_path = os.path.abspath(path) + completed = _LOCALIZED_ASSETS.get(local_path) + if completed is not None: + source, files = completed + if not force_download: + try: + if all(((stat := os.stat(file)).st_mtime_ns, stat.st_size) == stamp for file, stamp in files.items()): + return local_path + except OSError: + pass + path = source + else: + path = unmirror_file_path(local_path) or path - # resolve download directory - if download_dir is None: - download_dir = tempfile.gettempdir() + if check_file_path(path) == 0: + raise FileNotFoundError(f"Unable to find the file: {path}") + root = os.path.abspath(path) if os.path.isfile(path) else path.replace(os.sep, "/") + download_dir = os.path.abspath(download_dir or tempfile.gettempdir()) + prepared = {} + files = {} + complete = True + copy_dir = os.path.join(download_dir, f"isaaclab_usd_{uuid.uuid4().hex}") + + def prepare(source: str) -> str | None: + nonlocal complete + if source in prepared: + return prepared[source] + remote = bool(urlparse(source).scheme) and not os.path.isabs(source) + target = _mirror_path(source, download_dir) if remote else source + prepared[source] = None + if _UDIM_RE.search(source): + for tile in range(1001, 1101): + tile_path = _UDIM_RE.sub(str(tile), source) + if check_file_path(tile_path) == 0: + complete = complete and tile != 1001 + break + prepare(tile_path) + prepared[source] = target + return target + + # Read detached contents while the download lock protects the raw mirror. + suffix = os.path.splitext(target)[1].lower() + with _download_file(source, download_dir, force_download) as local_path: + if local_path is None: + if source == root or (force_download and remote): + raise RuntimeError(f"Unable to copy file: '{source}'") + complete = False + return None + stat = os.stat(local_path) + files[local_path] = (stat.st_mtime_ns, stat.st_size) + if suffix in _USD_EXTENSIONS: + from pxr import Ar, Sdf, UsdUtils # noqa: PLC0415 + + layer = Sdf.Layer.OpenAsAnonymous(local_path) + else: + refs = _find_mdl_dependencies(local_path) if suffix == ".mdl" else () + + prepared[source] = local_path + if suffix not in _USD_EXTENSIONS: + for ref in refs: + dependency = prepare(_resolve_reference_url(source, ref)) + complete = complete and dependency == _resolve_reference_url(local_path, ref) + return local_path + + # Reserve before descending so shared children and cycles have one destination. + output = os.path.join(copy_dir, str(len(prepared)), os.path.basename(local_path)) + prepared[source] = output + changed = False + + def rewrite(ref: str) -> str: + nonlocal changed + if not ref: + return ref + dependency = _resolve_reference_url(source, ref) + # Unresolved MDL search identifiers belong to the renderer's module path. + if ( + ref.endswith(".mdl") + and Ar.GetResolver().CreateIdentifier(ref, Ar.ResolvedPath(local_path)) == ref + and check_file_path(dependency) == 0 + ): + return ref + resolved = prepare(dependency) or dependency + changed |= resolved != _resolve_reference_url(local_path, ref) + return resolved + + UsdUtils.ModifyAssetPaths(layer, rewrite) + if changed: + os.makedirs(os.path.dirname(output)) + if not layer.Export(output): + raise RuntimeError(f"Unable to save resolved USD layer: {output}") + stat = os.stat(output) + files[output] = (stat.st_mtime_ns, stat.st_size) + if remote: + _MIRRORED_URLS[output] = source else: - download_dir = os.path.abspath(download_dir) - # create download directory if it does not exist - if not os.path.exists(download_dir): - os.makedirs(download_dir) - # recursive download: mirror remote tree under download_dir - remote_url = path.replace(os.sep, "/") - to_visit = [remote_url] - visited = set() - local_root = None - - report_activity("Loading assets") - while to_visit: - cur_url = to_visit.pop() - if cur_url in visited: - continue - visited.add(cur_url) - - # UDIM textures use a placeholder (e.g. texture..png) that does not - # correspond to a real file. Expand to individual tile URLs by probing tile numbers - # starting at 1001; UDIM tiles are contiguous so stop at the first missing tile. - if _UDIM_RE.search(cur_url): - for tile in range(1001, 1101): - tile_url = _UDIM_RE.sub(str(tile), cur_url) - if omni_client.stat(tile_url.replace(os.sep, "/"))[0] == omni_client.Result.OK: - if tile_url not in visited: - to_visit.append(tile_url) - else: - break - continue - - target_path = _mirror_path(cur_url, download_dir) - os.makedirs(os.path.dirname(target_path), exist_ok=True) - - is_root_asset = local_root is None - # Ranks can initialize against the same cold cache concurrently. Serialize a single - # mirrored file so no USD parser observes another rank overwriting it mid-read. - with FileLock(target_path + ".lock"): - # Re-check after acquiring the lock: another rank may have downloaded this asset - # while this rank was waiting. - if force_download or not _usable_mirror(cur_url, download_dir): - temporary_path = f"{target_path}.{uuid.uuid4().hex}.partial" - try: - result = omni_client.copy(cur_url, temporary_path, omni_client.CopyBehavior.OVERWRITE) - if result != omni_client.Result.OK: - if force_download or is_root_asset: - raise RuntimeError(f"Unable to copy file: '{cur_url}'. Is the Nucleus Server running?") - logger.debug("Skipping unavailable dependency: %s", cur_url) - continue - # A reader that does not take this process-local lock must never observe - # a partially copied USD file. - os.replace(temporary_path, target_path) - _write_mirror_fingerprint(cur_url, target_path) - finally: - with contextlib.suppress(OSError): - os.remove(temporary_path) - - # Resolve references while the mirror is stable. Each dependency gets its own - # lock below, preserving parallel initialization of unrelated asset trees. - references = _find_asset_dependencies(target_path) - - if local_root is None: - local_root = target_path - - # recurse into dependencies (USD references, payloads, MDL textures, etc.) - for ref in references: - ref_url = _resolve_reference_url(cur_url, ref) - if ref_url and ref_url not in visited: - to_visit.append(ref_url) + prepared[source] = local_path + return prepared[source] + from ..app.loading_screen import report_activity + + report_activity("Loading assets") + try: + result = prepare(root) + if complete: + for source, local_path in prepared.items(): + if source != local_path: + _LOCALIZED_ASSETS[local_path] = (source, files) + return result + finally: report_activity(None) - return os.path.abspath(local_root) - else: - raise FileNotFoundError(f"Unable to find the file: {path}") + + +@contextlib.contextmanager +def _download_file(source: str, download_dir: str, force_download: bool) -> Iterator[str | None]: + """Yield a local file while holding its remote mirror's download lock.""" + if os.path.isabs(source) or not urlparse(source).scheme: + yield source if os.path.isfile(source) else None + return + omni_client = _get_omni_client() + target = _mirror_path(source, download_dir) + os.makedirs(os.path.dirname(target), exist_ok=True) + with FileLock(target + ".lock"): + if force_download or not _usable_mirror(source, download_dir): + temporary_path = f"{target}.{uuid.uuid4().hex}.partial" + try: + result = omni_client.copy(source, temporary_path, omni_client.CopyBehavior.OVERWRITE) + if result != omni_client.Result.OK: + yield None + return + os.replace(temporary_path, target) + _write_mirror_fingerprint(source, target) + finally: + with contextlib.suppress(OSError): + os.remove(temporary_path) + yield target def read_file(path: str) -> io.BytesIO: @@ -764,61 +822,15 @@ def read_file(path: str) -> io.BytesIO: raise FileNotFoundError(f"Unable to find the file: {path}") -def _find_asset_dependencies(local_asset_path: str) -> set[str]: - """Collect external asset dependencies from a local asset file. - - USD layers are parsed with OpenUSD. MDL files are scanned for quoted texture - resources and relative module imports because those references are resolved - later by the MDL compiler and are not reported by USD dependency discovery. - """ - suffix = os.path.splitext(local_asset_path)[1].lower() - - if suffix == ".mdl": - try: - with open(local_asset_path, encoding="utf-8") as f: - source = f.read() - except OSError as e: - logger.warning("Failed to open MDL file: %s (%s)", local_asset_path, e) - return set() - - return _find_mdl_dependencies(source) - - if suffix not in _USD_EXTENSIONS: - return set() - - from pxr import Sdf, UsdUtils # noqa: PLC0415 - +def _find_mdl_dependencies(local_path: str) -> set[str]: + """Collect MDL resources and relative imports that USD does not discover.""" try: - layer = Sdf.Layer.FindOrOpen(local_asset_path) - except Exception: - logger.warning("Failed to open USD layer: %s", local_asset_path, exc_info=True) + with open(local_path, encoding="utf-8") as f: + source = f.read() + except OSError as e: + logger.warning("Failed to open MDL file: %s (%s)", local_path, e) return set() - if layer is None: - return set() - - refs: set[str] = set() - - def _collect(path: str) -> str: - """Record an asset path. - - Args: - path: Asset path from the USD layer. - - Returns: - The input path unchanged. - """ - if path: - refs.add(path) - return path - - UsdUtils.ModifyAssetPaths(layer, _collect) - - return refs - - -def _find_mdl_dependencies(source: str) -> set[str]: - """Collect local asset dependencies from MDL source text.""" refs = set() for match in _MDL_RESOURCE_RE.finditer(source): diff --git a/source/isaaclab/test/utils/test_assets.py b/source/isaaclab/test/utils/test_assets.py index cd107cf876d..d9bdd088d1c 100644 --- a/source/isaaclab/test/utils/test_assets.py +++ b/source/isaaclab/test/utils/test_assets.py @@ -245,7 +245,7 @@ def test_check_file_path_invalid(): assert assets_utils.check_file_path(usd_path) == 0 -def test_find_asset_dependencies_collects_mdl_texture_resources(tmp_path): +def test_find_mdl_dependencies_collects_mdl_texture_resources(tmp_path): """Test collecting texture resources from quoted MDL strings.""" mdl_path = tmp_path / "material.mdl" mdl_path.write_text( @@ -264,7 +264,7 @@ def test_find_asset_dependencies_collects_mdl_texture_resources(tmp_path): encoding="utf-8", ) - assert assets_utils._find_asset_dependencies(str(mdl_path)) == { + assert assets_utils._find_mdl_dependencies(str(mdl_path)) == { "./textures/Albedo.png", "../shared/Normal.EXR", "https://example.com/materials/orm..png", @@ -272,7 +272,7 @@ def test_find_asset_dependencies_collects_mdl_texture_resources(tmp_path): } -def test_find_asset_dependencies_collects_mdl_relative_import_modules(tmp_path): +def test_find_mdl_dependencies_collects_mdl_relative_import_modules(tmp_path): """Test collecting sibling MDL modules imported by material files.""" mdl_path = tmp_path / "material.mdl" mdl_path.write_text( @@ -293,7 +293,7 @@ def test_find_asset_dependencies_collects_mdl_relative_import_modules(tmp_path): encoding="utf-8", ) - assert assets_utils._find_asset_dependencies(str(mdl_path)) == { + assert assets_utils._find_mdl_dependencies(str(mdl_path)) == { "OmniUe4Function.mdl", "OmniUe4Translucent.mdl", "Shared/OmniUe4Base.mdl", @@ -307,11 +307,11 @@ def test_find_asset_dependencies_collects_mdl_relative_import_modules(tmp_path): } -def test_find_asset_dependencies_missing_mdl_does_not_log_traceback(tmp_path, caplog): +def test_find_mdl_dependencies_missing_mdl_does_not_log_traceback(tmp_path, caplog): """Test unavailable MDL dependencies do not emit tracebacks in training logs.""" missing_mdl = tmp_path / "missing.mdl" - assert assets_utils._find_asset_dependencies(str(missing_mdl)) == set() + assert assets_utils._find_mdl_dependencies(str(missing_mdl)) == set() assert "Traceback (most recent call last):" not in caplog.text @@ -495,6 +495,7 @@ def asset_cache(tmp_path, monkeypatch): monkeypatch.setattr(assets_utils, "_ANNOUNCED_MIRROR_DIRS", set()) monkeypatch.setattr(assets_utils, "_ANNOUNCED_MIRRORS", set()) monkeypatch.setattr(assets_utils, "_MIRRORED_URLS", {}) + monkeypatch.setattr(assets_utils, "_LOCALIZED_ASSETS", {}) return tmp_path @@ -529,6 +530,85 @@ def _cache_asset(cache_dir, url: str, payload: bytes, fingerprint: dict | None) return mirrored +@pytest.mark.parametrize("layout", ["direct", "nested", "remote", "package"]) +def test_local_usd_mirrors_remote_sublayer_without_editing_source(asset_cache, monkeypatch, layout): + """Compose local and remote dependency chains without editing the authored layers.""" + import omni.client + from pxr import Sdf, Usd + + layers = { + "local.usda": '#usda 1.0\ndef Shader "local" {\n asset info:mdl:sourceAsset = @OmniPBR.mdl@\n}\n', + "robot.usda": f"#usda 1.0\n(subLayers = [@{_REMOTE_URL}@, @local.usda@])\n", + "scene.usda": "#usda 1.0\n(subLayers = [@robot.usda@])\n", + } + for name, content in layers.items(): + (asset_cache / name).write_text(content, encoding="utf-8") + root_url = "https://example.com/scene.usda" + payloads = { + _REMOTE_URL: '#usda 1.0\ndef Xform "cartpole" {}\n', + root_url: layers["robot.usda"], + "https://example.com/local.usda": layers["local.usda"], + } + revision = {"hash": "abc123", "version": "", "size": 32, "modified_time": "2026-07-01 10:00:00"} + + def fake_copy(url, target_path, behavior): + if url not in payloads: + return omni.client.Result.ERROR_NOT_FOUND + data = payloads[url] + Path(target_path).write_bytes(data.encode() if isinstance(data, str) else data) + return omni.client.Result.OK + + monkeypatch.setattr(omni.client, "copy", fake_copy) + source = {"direct": str(asset_cache / "robot.usda"), "nested": str(asset_cache / "scene.usda"), "remote": root_url} + if layout == "package": + source[layout] = _REMOTE_URL + "z" + package_path = asset_cache / "scene.usdz" + package_root = asset_cache / "package.usda" + package_root.write_text('#usda 1.0\n(subLayers = [@local.usda@])\ndef Xform "cartpole" {}\n') + with Usd.ZipFileWriter.CreateNew(str(package_path)) as package: + package.AddFile(str(package_root), "package.usda") + package.AddFile(str(asset_cache / "local.usda"), "local.usda") + payloads[source[layout]] = package_path.read_bytes() + _serve(monkeypatch, dict.fromkeys(payloads, revision)) + resolved_path = assets_utils.retrieve_file_path(source[layout]) + stage = Usd.Stage.Open(resolved_path) + assert stage.GetPrimAtPath("/cartpole").IsValid() + assert stage.GetPrimAtPath("/local").IsValid() + assert stage.GetPrimAtPath("/local").GetAttribute("info:mdl:sourceAsset").Get().path == "OmniPBR.mdl" + assert {name: (asset_cache / name).read_text(encoding="utf-8") for name in layers} == layers + if layout == "package": + assert Path(resolved_path).read_bytes() == payloads[source[layout]] + + monkeypatch.setattr(Sdf.Layer, "OpenAsAnonymous", lambda _: pytest.fail("walked a completed tree")) + assert assets_utils.retrieve_file_path(resolved_path) == resolved_path + + +def test_retrieve_file_path_retries_incomplete_tree(asset_cache, monkeypatch): + """A downloaded root is not a completed tree when a dependency fails to download.""" + import omni.client + from pxr import Usd + + child_url = _REMOTE_URL.replace("example.usd", "child.usda") + revision = {"hash": "abc123", "version": "", "size": 32, "modified_time": "2026-07-01 10:00:00"} + mirrored = _cache_asset(asset_cache, _REMOTE_URL, b"#usda 1.0\n(subLayers = [@child.usda@])\n", revision) + _serve(monkeypatch, {_REMOTE_URL: revision, child_url: revision}) + fail_child = True + + def fake_copy(url, target_path, behavior): + assert url == child_url + if fail_child: + return omni.client.Result.ERROR_NOT_FOUND + Path(target_path).write_text('#usda 1.0\ndef Xform "child" {}\n', encoding="utf-8") + return omni.client.Result.OK + + monkeypatch.setattr(omni.client, "copy", fake_copy) + assets_utils.retrieve_file_path(_REMOTE_URL) + fail_child = False + resolved = assets_utils.retrieve_file_path(str(mirrored)) + stage = Usd.Stage.Open(resolved) + assert stage.GetPrimAtPath("/child").IsValid() + + def test_read_file_uses_the_local_copy_when_it_matches_the_server(asset_cache, monkeypatch): """Test an unchanged remote asset is read from disk instead of downloaded again.""" revision = {"hash": "abc123", "version": "", "size": 12, "modified_time": "2026-07-01 10:00:00"} @@ -590,7 +670,6 @@ def fake_copy(url, target_path, behavior): return omni.client.Result.OK monkeypatch.setattr(omni.client, "copy", fake_copy) - monkeypatch.setattr(assets_utils, "_find_asset_dependencies", lambda path: set()) start = threading.Barrier(2) def retrieve() -> str: From 595440500526fdc9b4bafb64e847b89620ee1a2f Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Fri, 25 Sep 2026 04:39:39 -0700 Subject: [PATCH 3/7] [Tests] Run package CI test files concurrently (#8006) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Package CI test jobs: 4:26 h → 2:09 h of runner time (−52%); longest job 29 → 13 min All package jobs pass. The comparison is a green `develop`-based PR run ([before](https://github.com/isaac-sim/IsaacLab/actions/runs/36102042654)) against this PR ([after](https://github.com/isaac-sim/IsaacLab/actions/runs/36126670863)): | Job | Before | After | |---|---:|---:| | `isaaclab_newton` | 26:35 | **7:45** (−71%) | | `isaaclab_ov` | 20:05 | **5:52** (−71%) | | `isaaclab_contrib` | 9:23 | **2:45** (−71%) | | `isaaclab (core)` 1/2/3 | 22:27 / 29:15 / 22:22 | **10:36 / 8:10 / 8:45** | | `isaaclab_tasks` 1/2/3 | 15:09 / 28:29 / 7:22 | **12:37 / 9:00 / 5:53** | | `isaaclab_rl` | 23:10 | **7:10** (−69%) | | `isaaclab_mimic` | 20:23 | **9:07** (−55%) | | `isaaclab_physx` | 14:49 | **8:46** (−41%) | | `isaaclab_visualizers` | 13:36 | 12:08 (−11%) | | `isaaclab_teleop` | 8:25 | 9:27 | | `isaaclab_assets` / `_experimental` | 3:04 / 1:44 | 5:15 / 5:45 | Assets and experimental lost to image-pull variance: their pulls took 1:25 and 3:58, against 0:02 in the baseline. Teleop and visualizers gain little because most of their files start a renderer, and renderer files run one at a time. ## Why xdist alone did not scale CI logs from a green run show most files spend more time starting than testing: - Kit files take a median 15.5 s before the first test, kitless files 3.2 s. - The first RTX renderer in a container waits 113–130 s. Splitting every file across xdist workers repeats that startup per worker, so the earlier renderer-file slowdowns were almost all startup: PhysX rigid-object rendering 113 s → 412 s, contrib visuotactile 119 s → 411 s. Only long files with cheap startup got faster, such as the leapp export. ## What changed - **`TEST_JOBS` / `test-jobs` input.** `tools/conftest.py` runs up to `TEST_JOBS` test files at once, each still in its own pytest process, so startup overlaps instead of repeating. Every package job sets `test-jobs: "4"`. Unset keeps today's serial, live-streamed behavior, and the multi-GPU lane is unchanged. - **Per-file settings** in `tools/test_settings.py`: - `PYTEST_WORKERS`: the few long-pole files that still split across xdist workers, holding one slot per worker and starting first. Today that is only `test_leapp_export_flow.py`. This replaces the job-wide `pytest-workers` input and `PYTEST_WORKER_LIMITS`. - `EXCLUSIVE_TESTS`: files that run alone. These are the two wall-clock performance tests; `test_robot_load_performance.py` failed with other files beside it. - **Renderers never overlap.** Files that start an RTX renderer (Kit cameras or OVRTX) never run beside each other; other files still run beside them. This avoids repeated cold shader compiles, which slowed startups several-fold, and garbled logs in the renderer log file they share. - **Output.** Output from concurrent files is printed whole per process attempt, so the job log does not interleave. - **Scheduling logic.** It lives in `tools/_file_scheduler.py`, with no pytest dependency. `tools/test_file_scheduler.py` covers it in the tools-tests job: slot limits, wide jobs not overtaken, renderer exclusivity, lazy claiming from the multi-GPU work queue, and error propagation. Each rule was checked by breaking it on purpose. - **Caches.** Every package job now restores the Warp kernel cache; nine did not. Jobs that render also restore the RTX shader cache. - **Pink IK race fix** (@ooctipus). Concurrent Pink IK controllers re-exported the same URDF and meshes into the shared temp dir, and the two GR1T2 mimic generation files read each other's half-written meshes. A file lock now covers export and load. This also affects users running several processes on one machine. ## Follow-ups - **RTX shader cache misses.** The restore finds no warmed entry for these runners' GPU and driver key (`sm89`, 595.58.03), so renderer startup still compiles shaders. `test_visualizer_golden_newton.py` still sometimes overruns the 120 s startup deadline and passes on retry. - **Non-atomic asset downloads.** `retrieve_file_path` downloads into the shared temp dir and reuses a file if it exists. Two processes fetching the same asset could read a partial file. This has not been observed. - **Slow asset-cache population.** `test_environments_isaacsim_physx.py` spends ~1,000 s populating the Git asset cache before its first test. ## Type of change - CI / test infrastructure (no user-facing change) ## 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 ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` --------- Co-authored-by: Octi Zhang --- .github/actions/run-package-tests/action.yml | 8 + .github/actions/run-tests/action.yml | 10 +- .github/actions/run-tests/run_tests.sh | 6 + .github/workflows/build.yaml | 37 ++ .github/workflows/tools-tests.yml | 5 +- conftest.py | 9 + pyproject.toml | 2 + .../performance-test-scheduling.skip | 1 + .../pink-ik-concurrent-conversion.rst | 5 + .../isaaclab/controllers/pink_ik/pink_ik.py | 45 +- .../test_test_orchestrator_result_handling.py | 34 +- .../controllers/test_pink_ik_components.py | 54 +++ tools/_file_scheduler.py | 165 +++++++ tools/conftest.py | 407 +++++++++++++----- tools/hang_dump.py | 4 +- tools/test_crash_journal.py | 36 ++ tools/test_file_scheduler.py | 231 ++++++++++ tools/test_settings.py | 19 + uv.lock | 25 ++ 19 files changed, 959 insertions(+), 144 deletions(-) create mode 100644 source/isaaclab/changelog.d/performance-test-scheduling.skip create mode 100644 source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst create mode 100644 tools/_file_scheduler.py create mode 100644 tools/test_file_scheduler.py diff --git a/.github/actions/run-package-tests/action.yml b/.github/actions/run-package-tests/action.yml index 2f32aa5f031..e0485935430 100644 --- a/.github/actions/run-package-tests/action.yml +++ b/.github/actions/run-package-tests/action.yml @@ -47,6 +47,13 @@ inputs: spawned by tools/conftest.py (combined with device-split selectors). default: '' required: false + test-jobs: + description: >- + Number of test-file slots tools/conftest.py runs at once. Each file still runs in + its own pytest process; files listed in tools/test_settings.py PYTEST_WORKERS split + across several slots with pytest-xdist. Empty or 1 runs files one by one. + default: '' + required: false shard-index: description: 'Zero-based shard index' default: '' @@ -333,6 +340,7 @@ runs: filter-pattern: ${{ inputs.filter-pattern }} exclude-pattern: ${{ inputs.exclude-pattern }} test-k-expr: ${{ inputs.test-k-expr }} + test-jobs: ${{ inputs.test-jobs }} shard-index: ${{ inputs.shard-index }} shard-count: ${{ inputs.shard-count }} curobo-only: ${{ inputs.curobo-only }} diff --git a/.github/actions/run-tests/action.yml b/.github/actions/run-tests/action.yml index 510c9e4cc03..96953cf647a 100644 --- a/.github/actions/run-tests/action.yml +++ b/.github/actions/run-tests/action.yml @@ -49,6 +49,13 @@ inputs: can deselect parametrized cases (e.g. "not ovphysx"). default: '' required: false + test-jobs: + description: >- + Number of test-file slots tools/conftest.py runs at once. Each file still runs in + its own pytest process; files listed in tools/test_settings.py PYTEST_WORKERS split + across several slots with pytest-xdist. Empty or 1 runs files one by one. + default: '' + required: false curobo-only: description: 'Run only cuRobo and SkillGen tests (requires the cuRobo Docker image)' default: 'false' @@ -155,13 +162,14 @@ runs: TEST_NODE_IDS_KEY: ${{ inputs.test-node-ids-key }} TEST_PATH: ${{ inputs.test-path }} TEST_K_EXPR_INPUT: ${{ inputs.test-k-expr }} + TEST_JOBS_INPUT: ${{ inputs.test-jobs }} CI_MARKER_INPUT: ${{ inputs.ci-marker }} VOLUME_MOUNT_SOURCE: ${{ inputs.volume-mount-source }} WARP_CACHE_HOST_DIR: ${{ inputs.warp-cache-host-dir }} WHEELHOUSE_HOST_DIR: ${{ inputs.wheelhouse-host-dir }} WHEELHOUSE_PACKAGES: ${{ inputs.wheelhouse-packages }} run: | - bash .github/actions/run-tests/run_tests.sh "$TEST_PATH" "$RESULT_FILE" "$CONTAINER_NAME" "$IMAGE_TAG" "$REPORTS_DIR" "$PYTEST_OPTIONS" "$FILTER_PATTERN" "$EXCLUDE_PATTERN" "$CUROBO_ONLY" "$INCLUDE_FILES" "$QUARANTINED_ONLY" "$SHARD_INDEX" "$SHARD_COUNT" "$VOLUME_MOUNT_SOURCE" "$EXTRA_PIP_PACKAGES" "$TEST_NODE_IDS_FILE" "$TEST_NODE_IDS_KEY" "$WHEELHOUSE_HOST_DIR" "$WHEELHOUSE_PACKAGES" "$TEST_K_EXPR_INPUT" "$CI_MARKER_INPUT" "$STANDALONE_SCRIPT_SCOPE" "$STANDALONE_SCRIPT_VISUALIZER" "$STANDALONE_SCRIPT_RUNTIME_GROUP" "$WARP_CACHE_HOST_DIR" "$EXTRA_UV_PACKAGES" "$OVRTX_SHADER_CACHE_HOST_DIR" + bash .github/actions/run-tests/run_tests.sh "$TEST_PATH" "$RESULT_FILE" "$CONTAINER_NAME" "$IMAGE_TAG" "$REPORTS_DIR" "$PYTEST_OPTIONS" "$FILTER_PATTERN" "$EXCLUDE_PATTERN" "$CUROBO_ONLY" "$INCLUDE_FILES" "$QUARANTINED_ONLY" "$SHARD_INDEX" "$SHARD_COUNT" "$VOLUME_MOUNT_SOURCE" "$EXTRA_PIP_PACKAGES" "$TEST_NODE_IDS_FILE" "$TEST_NODE_IDS_KEY" "$WHEELHOUSE_HOST_DIR" "$WHEELHOUSE_PACKAGES" "$TEST_K_EXPR_INPUT" "$CI_MARKER_INPUT" "$STANDALONE_SCRIPT_SCOPE" "$STANDALONE_SCRIPT_VISUALIZER" "$STANDALONE_SCRIPT_RUNTIME_GROUP" "$WARP_CACHE_HOST_DIR" "$EXTRA_UV_PACKAGES" "$OVRTX_SHADER_CACHE_HOST_DIR" "$TEST_JOBS_INPUT" - name: Kill container on cancellation if: cancelled() shell: bash diff --git a/.github/actions/run-tests/run_tests.sh b/.github/actions/run-tests/run_tests.sh index 655f3a3919f..aa05831cbb0 100755 --- a/.github/actions/run-tests/run_tests.sh +++ b/.github/actions/run-tests/run_tests.sh @@ -38,6 +38,7 @@ run_tests() { local warp_cache_host_dir="${25}" local extra_uv_packages="${26}" local ovrtx_shader_cache_host_dir="${27}" + local test_jobs="${28}" local logs_pid="" local wait_pid="" local docker_wait_file="/tmp/.docker_exit_${container_name}" @@ -194,6 +195,11 @@ run_tests() { echo "Setting per-file pytest -k expression: $test_k_expr" fi + if [ -n "$test_jobs" ]; then + docker_env_args+=(-e "TEST_JOBS=$test_jobs") + echo "Setting TEST_JOBS=$test_jobs" + fi + if [ -n "$ci_marker" ]; then docker_env_args+=(-e "CI_MARKER=$ci_marker") echo "Setting CI_MARKER=$ci_marker" diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 0eb5d8c4602..bafbebb8767 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -275,6 +275,8 @@ jobs: shard-index: "0" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-tasks-1-test test-isaaclab-tasks-2: @@ -301,6 +303,8 @@ jobs: shard-index: "1" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-tasks-2-test test-isaaclab-tasks-3: @@ -327,6 +331,8 @@ jobs: shard-index: "2" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-tasks-3-test test-isaaclab-core: @@ -352,6 +358,8 @@ jobs: shard-index: "0" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-core-1-test test-isaaclab-core-2: @@ -377,6 +385,8 @@ jobs: shard-index: "1" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-core-2-test test-isaaclab-core-3: @@ -402,6 +412,8 @@ jobs: shard-index: "2" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-core-3-test # Kit and non-Kit are written out rather than expressed as a matrix: these job @@ -486,6 +498,8 @@ jobs: isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_rl" extra-pip-packages: "leapp==0.6.1 torchrl>=0.13" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-rl-test test-isaaclab-mimic: @@ -507,6 +521,8 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_mimic" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-mimic-test test-isaaclab-contrib: @@ -528,6 +544,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_contrib" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-contrib-test test-isaaclab-teleop: @@ -549,6 +568,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_teleop" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-teleop-test test-isaaclab-visualizers: @@ -570,6 +592,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_visualizers" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-visualizers-test test-isaaclab-assets: @@ -591,6 +616,8 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_assets" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-assets-test test-isaaclab-experimental: @@ -612,6 +639,8 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_experimental" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-experimental-test test-isaaclab-newton: @@ -634,6 +663,8 @@ jobs: isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_newton" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-newton-test test-isaaclab-physx: @@ -655,6 +686,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_physx" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-physx-test test-isaaclab-ov: @@ -684,6 +718,9 @@ jobs: extra-pip-packages: ${{ env.USE_OVPHYSX_WHEELHOUSE == 'true' && steps.ov_pins.outputs.ovrtx || format('{0} {1}', steps.ov_pins.outputs.ovrtx, steps.ov_pins.outputs.ovphysx) }} wheelhouse-image: ${{ env.USE_OVPHYSX_WHEELHOUSE == 'true' && needs.config.outputs.ovphysx_wheelhouse_image || '' }} wheelhouse-packages: ${{ env.USE_OVPHYSX_WHEELHOUSE == 'true' && 'ovphysx' || '' }} + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-ov-test # Folded from the former standalone verify-base-non-root job: reuses the diff --git a/.github/workflows/tools-tests.yml b/.github/workflows/tools-tests.yml index 0903ebea572..c76c0c3c129 100644 --- a/.github/workflows/tools-tests.yml +++ b/.github/workflows/tools-tests.yml @@ -52,8 +52,9 @@ jobs: # flaky, so they run without an Isaac Sim install or a full project sync. flaky drives the # rerun in test_crash_during_a_flaky_retry_is_blamed_on_the_retried_test; without it # installed that test skips itself rather than failing, so keep it in this list. + # pytest-xdist likewise drives test_an_xdist_run_journals_each_event_once. - name: Install test dependencies - run: bash "$GITHUB_WORKSPACE/.github/actions/_lib/with-python-package-retries.sh" python3 -m pip install pytest junitparser flaky pyyaml + run: bash "$GITHUB_WORKSPACE/.github/actions/_lib/with-python-package-retries.sh" python3 -m pip install pytest pytest-xdist junitparser flaky pyyaml - name: Run tools tests env: @@ -65,5 +66,5 @@ jobs: # --noconftest keeps tools/conftest.py out of the session. That file is the CI test # orchestrator - its pytest_sessionstart scans source/ and scripts/ and runs the whole # suite, so loading it here would ignore the files named below. - run: python3 -m pytest tools/test_crash_journal.py tools/test_device_split.py + run: python3 -m pytest tools/test_crash_journal.py tools/test_device_split.py tools/test_file_scheduler.py .github/actions/_lib/test_registry_credential_fallback.py -v --noconftest diff --git a/conftest.py b/conftest.py index d5bf93c8ca1..05b3721a43b 100644 --- a/conftest.py +++ b/conftest.py @@ -51,10 +51,19 @@ def _journal_write(record: dict) -> None: The per-record flush is the whole point: it puts the data in the OS page cache before the next test starts, so a process killed by a signal cannot take down verdicts it had already reported. Journaling failures are swallowed — losing debug context must never fail a run. + + Under ``pytest-xdist`` the controller receives every worker's start, report and finish, so only + it journals those; each worker journaling too would record every event twice and leave a + worker crash that xdist recovered from looking like an in-flight test. The controller never + collects, so the ``collected`` record comes from the first worker instead (every worker + collects the same items). """ path = os.environ.get(JOURNAL_ENV_VAR) if not path: return + worker = os.environ.get("PYTEST_XDIST_WORKER") + if worker and (record["event"] != "collected" or worker != "gw0"): + return try: with open(path, "a", encoding="utf-8") as handle: handle.write(json.dumps(record, separators=(",", ":")) + "\n") diff --git a/pyproject.toml b/pyproject.toml index 493b18cd78e..8ecc273c0e9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -126,6 +126,8 @@ importers = [ test = [ "pytest", "pytest-mock", + # Splits a test file across worker processes; see TEST_PYTEST_WORKERS in tools/conftest.py. + "pytest-xdist", "junitparser", "flaky", # numba subclasses coverage.types.Tracer at import; >=7.6.1 restores that shim diff --git a/source/isaaclab/changelog.d/performance-test-scheduling.skip b/source/isaaclab/changelog.d/performance-test-scheduling.skip new file mode 100644 index 00000000000..df9521b1d5f --- /dev/null +++ b/source/isaaclab/changelog.d/performance-test-scheduling.skip @@ -0,0 +1 @@ +Regression coverage for performance test isolation and updated test fixtures for the parallel runner. diff --git a/source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst b/source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst new file mode 100644 index 00000000000..34dcf3eaf3d --- /dev/null +++ b/source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed concurrent Pink IK controller initialization corrupting shared USD-to-URDF exports by locking + the output directory through conversion and model loading. diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index af9a36b03cb..bed0f5e04fb 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -14,10 +14,14 @@ from __future__ import annotations +import os +import tempfile +from contextlib import ExitStack from typing import TYPE_CHECKING, cast import numpy as np import torch +from filelock import FileLock from pink import solve_ik from pink.tasks import Task from qpsolvers.exceptions import SolverNotFound @@ -81,27 +85,28 @@ def __init__( # Validate consistency between controlled_joint_indices and configuration self._validate_consistency(cfg, controlled_joint_indices) - # Resolve URDF/mesh paths at runtime. If only usd_path is provided, convert USD→URDF first. - if cfg.urdf_path is None and cfg.usd_path is not None: - import tempfile - - urdf_output_dir = cfg.urdf_output_dir or tempfile.gettempdir() - urdf_path, mesh_path = controller_utils.convert_usd_to_urdf( - cfg.usd_path, urdf_output_dir, force_conversion=True + with ExitStack() as stack: + # Resolve URDF/mesh paths at runtime. If only usd_path is provided, convert USD→URDF first. + if cfg.urdf_path is None and cfg.usd_path is not None: + urdf_output_dir = cfg.urdf_output_dir or tempfile.gettempdir() + # Conversions share mesh filenames. Protect the output directory until Pinocchio finishes reading it. + stack.enter_context(FileLock(os.path.join(urdf_output_dir, ".isaaclab_urdf.lock"))) + urdf_path, mesh_path = controller_utils.convert_usd_to_urdf( + cfg.usd_path, urdf_output_dir, force_conversion=True + ) + else: + urdf_path = retrieve_file_path(cfg.urdf_path) if cfg.urdf_path else cfg.urdf_path + mesh_path = retrieve_file_path(cfg.mesh_path) if cfg.mesh_path else cfg.mesh_path + + if urdf_path is None: + raise ValueError("Either urdf_path or usd_path must be provided in the controller configuration") + + # Initialize the Kinematics model used by pink IK to control robot + self.pink_configuration = PinkKinematicsConfiguration( + urdf_path=urdf_path, + mesh_path=mesh_path, + controlled_joint_names=cfg.joint_names, ) - else: - urdf_path = retrieve_file_path(cfg.urdf_path) if cfg.urdf_path else cfg.urdf_path - mesh_path = retrieve_file_path(cfg.mesh_path) if cfg.mesh_path else cfg.mesh_path - - if urdf_path is None: - raise ValueError("Either urdf_path or usd_path must be provided in the controller configuration") - - # Initialize the Kinematics model used by pink IK to control robot - self.pink_configuration = PinkKinematicsConfiguration( - urdf_path=urdf_path, - mesh_path=mesh_path, - controlled_joint_names=cfg.joint_names, - ) # Find the initial joint positions by matching Pink's joint names to robot_cfg.init_state.joint_pos, # where the joint_pos keys may be regex patterns and the values are the initial positions. diff --git a/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py b/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py index 98a501f7058..07d712f4ba0 100644 --- a/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py +++ b/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py @@ -144,6 +144,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[missing_node_id], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -180,6 +181,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -218,6 +220,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr="ovphysx", suffix="") @@ -251,6 +254,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -287,7 +291,7 @@ def test_abnormal_termination_report_quotes_bounded_renderer_log( # quietly turn this into a test of a log that fits inside it. filler_lines = orchestrator.ovrtx_log.LOG_LIMIT_BYTES // len("filler-line\n") + 1 - def _capture(cmd, timeout, env, *, startup_deadline, report_file): + def _capture(cmd, timeout, env, *, startup_deadline, report_file, options): # Render verbosely, then die without writing a report. Path(log_path).write_text("head-line\n" + "filler-line\n" * filler_lines + "tail-line\n", encoding="utf-8") report_paths.append(Path(report_file)) @@ -308,6 +312,7 @@ def _capture(cmd, timeout, env, *, startup_deadline, report_file): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) with caplog.at_level("INFO"): @@ -369,6 +374,7 @@ def _capture(_cmd, _timeout, env, *, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) _report, status, _was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -390,7 +396,7 @@ def test_shutdown_hang_after_report_is_not_a_failure(monkeypatch, tmp_path: Path test_file = tmp_path / "test_sample.py" test_file.write_text("def test_present():\n pass\n", encoding="utf-8") - def _capture(cmd, timeout, env, *, startup_deadline, report_file): + def _capture(cmd, timeout, env, *, startup_deadline, report_file, options): _write_partial_junit_report(report_file) return -1, b"", b"", "shutdown_hang", 30.0, "" @@ -407,6 +413,7 @@ def _capture(cmd, timeout, env, *, startup_deadline, report_file): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) _report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -444,6 +451,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) _report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -494,6 +502,7 @@ def _capture(_cmd, _timeout, env, *, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -571,6 +580,7 @@ def _capture(_cmd, _timeout, env, *, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -678,6 +688,26 @@ def test_hang_dump_plugin_is_inert_without_signal_support(monkeypatch) -> None: hang_dump.pytest_configure(config=None) # must not raise +def test_performance_files_reserve_the_runner_without_xdist(monkeypatch, tmp_path): + """Reserving every slot for timing tests must not split their measurements across pytest workers.""" + orchestrator = _load_orchestrator_module() + performance_files = ["test_kit_startup_performance.py", "test_robot_load_performance.py"] + files = [str(tmp_path / name) for name in (*performance_files, "test_regular.py")] + observed = {} + + def run(job, context, *, workers, **kwargs): + observed[Path(job.path).name] = (job.slots, workers) + return orchestrator._FileResult(reports=[], status={}, failed=False) + + monkeypatch.setenv("TEST_JOBS", "4") + monkeypatch.delenv("ISAACLAB_TEST_QUEUE", raising=False) + monkeypatch.setattr(orchestrator, "_run_test_file", run) + orchestrator.run_individual_tests(files, str(tmp_path), ci_marker=None) + + assert all(observed[name] == (4, 1) for name in performance_files) + assert observed["test_regular.py"] == (1, 1) + + def test_external_git_asset_tests_receive_extended_startup_deadline(): """Environment discovery must allow a cold external Git asset clone to complete.""" orchestrator = _load_orchestrator_module() diff --git a/source/isaaclab/test/controllers/test_pink_ik_components.py b/source/isaaclab/test/controllers/test_pink_ik_components.py index 8dca1995e1b..42ec7d19d85 100644 --- a/source/isaaclab/test/controllers/test_pink_ik_components.py +++ b/source/isaaclab/test/controllers/test_pink_ik_components.py @@ -5,7 +5,9 @@ """Test cases for PinkKinematicsConfiguration class.""" +from concurrent.futures import ThreadPoolExecutor from pathlib import Path +from threading import Event import numpy as np import pinocchio as pin @@ -17,6 +19,58 @@ pytestmark = pytest.mark.integration +@pytest.mark.parametrize("load_fails", [False, True]) +def test_concurrent_controller_conversion_preserves_model(monkeypatch, tmp_path, load_fails): + """Another controller must not overwrite an export before its consumer finishes loading it.""" + from isaaclab.assets import ArticulationCfg + from isaaclab.controllers.pink_ik import PinkIKControllerCfg, pink_ik + + urdf = (Path(__file__).parent / "urdfs/test_urdf_two_link_robot.urdf").read_text() + first_loading, second_started, second_converted = Event(), Event(), Event() + + def convert(usd_path, output_path, force_conversion): + output = Path(output_path) / "robot.urdf" + name = Path(usd_path).parent.name + output.write_text(urdf.replace("test_two_link_robot", name)) + if name == "second": + second_converted.set() + return str(output), "" + + def load(**kwargs): + if not first_loading.is_set(): + first_loading.set() + assert second_started.wait(5) + # Give the contender a chance to overwrite the URDF while this reader is paused. + second_converted.wait(1) + if load_fails: + raise RuntimeError("model loading failed") + return PinkKinematicsConfiguration(**kwargs) + + def initialize(name): + cfg = PinkIKControllerCfg( + usd_path=f"/{name}/robot.usd", + urdf_output_dir=str(tmp_path), + joint_names=["joint_1", "joint_2"], + all_joint_names=["joint_1", "joint_2"], + ) + if name == "second": + second_started.set() + return pink_ik.PinkIKController(cfg, ArticulationCfg(), "cpu", [0, 1]) + + monkeypatch.setattr(pink_ik.controller_utils, "convert_usd_to_urdf", convert) + monkeypatch.setattr(pink_ik, "PinkKinematicsConfiguration", load) + with ThreadPoolExecutor(max_workers=2) as pool: + first = pool.submit(initialize, "first") + assert first_loading.wait(5) + second = pool.submit(initialize, "second") + if load_fails: + with pytest.raises(RuntimeError, match="model loading failed"): + first.result(timeout=10) + else: + assert first.result(timeout=10).pink_configuration.full_model.name == "first" + assert second.result(timeout=10).pink_configuration.full_model.name == "second" + + class TestPinkKinematicsConfiguration: """Test suite for PinkKinematicsConfiguration class.""" diff --git a/tools/_file_scheduler.py b/tools/_file_scheduler.py new file mode 100644 index 00000000000..06111b5e3e8 --- /dev/null +++ b/tools/_file_scheduler.py @@ -0,0 +1,165 @@ +# 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 + +"""Run test files concurrently within a fixed budget of slots. + +``tools/conftest.py`` runs every test file in its own pytest process. Most of a process's life is spent +starting up -- booting Kit, importing, collecting -- rather than testing, so running several files at once +overlaps that fixed cost instead of paying it file after file. Splitting one file across ``pytest-xdist`` +workers does the opposite for most files, since every worker pays the startup again; it only pays off for +the few long files whose tests dwarf their startup, which is why a file may ask for several slots. + +The scheduler knows nothing about pytest. It is given :class:`TestFileJob` entries and a ``run`` callable, +and guarantees: + +* at most ``max_slots`` slots are in use, a job holding :attr:`TestFileJob.slots` of them; +* jobs start in the order they are given, so a job that does not fit yet is not overtaken (a wide job + would otherwise wait forever behind a stream of narrow ones); +* rendering jobs never overlap one another, though other jobs may overtake a rendering job waiting its turn. + The first renderer in a fresh container compiles shaders into a cache the rest reuse, and several starting + at once each compile the same shaders; renderers also share one log file, so overlapping ones would + garble each other's diagnostics. + +With ``max_slots <= 1`` jobs run one after the other on the calling thread, exactly as a plain loop would. +""" + +from __future__ import annotations + +import queue +import threading +from collections.abc import Callable, Iterable +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass +from typing import Generic, TypeVar + +R = TypeVar("R") + + +@dataclass(frozen=True) +class TestFileJob: + """One test file to run.""" + + __test__ = False # not a pytest test class, despite the name + + path: str + """Test file path; also the key of its result.""" + + slots: int = 1 + """Slots the job holds while it runs, e.g. one per ``pytest-xdist`` worker; more than the budget holds them all.""" + + renders: bool = False + """Whether the job starts a renderer, and so never runs alongside another job that does.""" + + +@dataclass(frozen=True) +class JobContext: + """What the scheduler tells a job it starts.""" + + renderer_cold: bool + """Whether this job renders and no rendering job had finished starting before it, so it compiles shaders.""" + + mark_started: Callable[[], None] + """Call once the job's process has finished starting; a rendering job's start warms the shader cache.""" + + +class _Scheduler(Generic[R]): + def __init__(self, run: Callable[[TestFileJob, JobContext], R], max_slots: int): + self._run = run + self._max_slots = max_slots + self._renderer_ready = threading.Event() + self._finished: queue.Queue[tuple[TestFileJob, R | BaseException]] = queue.Queue() + + def _context(self, job: TestFileJob) -> JobContext: + def mark_started(): + if job.renders: + self._renderer_ready.set() + + return JobContext(renderer_cold=job.renders and not self._renderer_ready.is_set(), mark_started=mark_started) + + def run_serially(self, jobs: Iterable[TestFileJob]) -> dict[str, R]: + results = {} + for job in jobs: + results[job.path] = self._run(job, self._context(job)) + return results + + def run_concurrently(self, jobs: Iterable[TestFileJob]) -> dict[str, R]: + source = iter(jobs) + pending: list[TestFileJob] = [] + running: list[TestFileJob] = [] + results: dict[str, R] = {} + order: list[str] = [] + exhausted = False + + def free_slots() -> int: + return self._max_slots - sum(self._cost(job) for job in running) + + def next_startable() -> TestFileJob | None: + nonlocal exhausted + index = 0 + while True: + # Pull only as far as the first startable job: a shared work queue hands a claimed file to no one else. + if index == len(pending): + if exhausted or free_slots() == 0: + return None + try: + pending.append(next(source)) + except StopIteration: + exhausted = True + return None + job = pending[index] + if job.renders and any(other.renders for other in running): + index += 1 + continue + return job if self._cost(job) <= free_slots() else None + + with ThreadPoolExecutor(max_workers=self._max_slots, thread_name_prefix="test-file") as pool: + while True: + while (job := next_startable()) is not None: + pending.remove(job) + running.append(job) + order.append(job.path) + pool.submit(self._run_and_report, job, self._context(job)) + if not running: + break + job, outcome = self._finished.get() + running.remove(job) + if isinstance(outcome, BaseException): + raise outcome + results[job.path] = outcome + return {path: results[path] for path in order} + + def _run_and_report(self, job: TestFileJob, context: JobContext) -> None: + try: + outcome = self._run(job, context) + except BaseException as error: # re-raised on the scheduling thread + outcome = error + self._finished.put((job, outcome)) + + def _cost(self, job: TestFileJob) -> int: + return max(1, min(job.slots, self._max_slots)) + + + +def run_test_files( + jobs: Iterable[TestFileJob], run: Callable[[TestFileJob, JobContext], R], max_slots: int = 1 +) -> dict[str, R]: + """Run ``run`` for every job, at most ``max_slots`` slots at a time. + + Args: + jobs: Jobs in the order they should start. Consumed lazily, so it may be a generator that claims + files from a queue shared with other runners. + run: Runs one job and returns its result. Called on a worker thread when ``max_slots > 1``. + max_slots: Slots available; a job wider than this is given all of them. + + Returns: + Each job's result keyed by :attr:`TestFileJob.path`, in the order the jobs started. + + Raises: + BaseException: Whatever ``run`` raised, once the jobs already running have finished. + """ + scheduler = _Scheduler(run, max_slots) + if max_slots <= 1: + return scheduler.run_serially(jobs) + return scheduler.run_concurrently(jobs) diff --git a/tools/conftest.py b/tools/conftest.py index a6120bb4619..ef87419bc87 100644 --- a/tools/conftest.py +++ b/tools/conftest.py @@ -10,7 +10,9 @@ import signal import subprocess import sys +import threading import time +from collections.abc import Callable from dataclasses import dataclass import pytest @@ -26,6 +28,7 @@ import test_settings as test_settings # isort: skip from crash_journal import JOURNAL_ENV_VAR, create_crash_report # isort: skip from _device_split import DEVICE_SPLIT_PASSES, is_device_split_file # isort: skip +from _file_scheduler import JobContext, TestFileJob, run_test_files # isort: skip logging.basicConfig(level=logging.INFO, format="%(message)s") logger = logging.getLogger(__name__) @@ -49,12 +52,22 @@ def pytest_ignore_collect(collection_path, config): AppLauncher prints ``[ISAACLAB] AppLauncher initialization complete`` to ``sys.__stderr__`` (never suppressed) when Kit finishes initializing, and pytest -prints ``collected N items`` to stdout after collection. If neither appears +prints ``collected N items`` to stdout after collection (``N workers [M items]`` +under ``pytest-xdist``, once every worker has collected). If none appears within this deadline the process is treated as hung. Kit startup can exceed 60 s on cold CI workers, so this catches real startup hangs without killing legitimate slow launches. """ +TEST_JOBS_ENV_VAR = "TEST_JOBS" +"""Environment variable naming how many test-file slots a run may use at once; unset or ``1`` runs files one by one. + +Each file still runs in its own pytest process. A file listed in :data:`test_settings.PYTEST_WORKERS` holds +several slots and splits its tests across that many ``pytest-xdist`` workers, one in +:data:`test_settings.EXCLUSIVE_TESTS` holds them all, and any other file holds one. See +``tools/_file_scheduler.py`` for the scheduling rules. +""" + STARTUP_HANG_RETRIES = 2 """Number of times to retry a test that hangs during startup before giving up.""" @@ -163,7 +176,7 @@ def resolve_exit_code(num_failing: int, num_timeout: int, num_crashed: int, num_ return 0 -def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1): +def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1, echo=True): """Read whatever is readable on the child's pipes, echoing it as it arrives. Args: @@ -171,11 +184,13 @@ def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1): stdout_fd: Read end of the child's stdout, already non-blocking. stderr_fd: Read end of the child's stderr, already non-blocking. timeout: Seconds to wait for either pipe to become readable. + echo: Whether to echo what was read to this process's own streams. Returns: - Tuple of ``(stdout_bytes, stderr_bytes)`` read in this pass. Both are - echoed to this process's own streams before being returned, so output - reaches the job log while the test is still running. + Tuple of ``(stdout_bytes, stderr_bytes)`` read in this pass. When + ``echo`` is set, both are echoed to this process's own streams before + being returned, so output reaches the job log while the test is still + running. """ stdout_chunk = b"" stderr_chunk = b"" @@ -188,20 +203,82 @@ def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1): chunk = process.stdout.read(1024) if chunk: stdout_chunk += chunk - sys.stdout.buffer.write(chunk) - sys.stdout.buffer.flush() + if echo: + sys.stdout.buffer.write(chunk) + sys.stdout.buffer.flush() elif fd == stderr_fd: chunk = process.stderr.read(1024) if chunk: stderr_chunk += chunk - sys.stderr.buffer.write(chunk) - sys.stderr.buffer.flush() + if echo: + sys.stderr.buffer.write(chunk) + sys.stderr.buffer.flush() except OSError: time.sleep(timeout) return stdout_chunk, stderr_chunk -def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): +@dataclass(frozen=True) +class _CaptureOptions: + """How a test process is watched, beyond its command and limits.""" + + echo: bool = True + """Stream the process's output to the job log as it arrives. Off when files run concurrently, whose output + would interleave; each pass's output is then printed whole once it ends.""" + + workers: int = 1 + """``pytest-xdist`` workers the process splits into; a hang dump asks each of them for a stack as well.""" + + on_started: Callable[[], None] | None = None + """Called once the process has finished starting up, i.e. reached pytest collection.""" + + +def _test_jobs() -> int: + """Return the number of test-file slots :data:`TEST_JOBS_ENV_VAR` allows, at least 1.""" + try: + return max(1, int(os.environ.get(TEST_JOBS_ENV_VAR, "") or 1)) + except ValueError: + logger.warning(f"Ignoring non-integer {TEST_JOBS_ENV_VAR}={os.environ[TEST_JOBS_ENV_VAR]!r}") + return 1 + + +_OUTPUT_LOCK = threading.Lock() +"""Serializes printing whole passes' output, so concurrently finishing files do not interleave in the job log.""" + + +def _print_captured_output(label, options, stdout_data, stderr_data): + """Print one process's captured output in a single block, unless it was already echoed as it arrived.""" + if options.echo: + return + with _OUTPUT_LOCK: + sys.stdout.write(f"\n{'=' * 30} output of {label} {'=' * 30}\n") + sys.stdout.flush() + sys.stdout.buffer.write(stdout_data) + sys.stdout.flush() + sys.stderr.buffer.write(stderr_data) + sys.stderr.flush() + sys.stdout.write(f"{'=' * 30} end of {label} {'=' * 30}\n") + sys.stdout.flush() + + +def _child_pids(pid: int) -> list[int]: + """Return the direct children of ``pid``, or an empty list where ``/proc`` is unavailable.""" + children = [] + for entry in os.listdir("/proc") if os.path.isdir("/proc") else []: + if not entry.isdigit(): + continue + try: + with open(f"/proc/{entry}/stat") as handle: + # The command name is parenthesized and may contain spaces; the parent PID follows it. + parent = int(handle.read().rsplit(")", 1)[1].split()[1]) + except (OSError, IndexError, ValueError): + continue + if parent == pid: + children.append(int(entry)) + return sorted(children) + + +def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env, options): """Ask a hung process for a stack of every thread, and collect what it writes. Sends :data:`hang_dump.DUMP_SIGNAL`, which ``tools/hang_dump.py`` registers with ``faulthandler`` in the @@ -210,13 +287,16 @@ def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): The signal goes to the test process itself rather than its group. The handler is registered there, and a standalone script the test launched as a grandchild has no handler -- ``SIGUSR1`` would simply kill it, - losing it from the process tree the caller has already recorded. + losing it from the process tree the caller has already recorded. Under ``pytest-xdist`` the tests run in + the worker processes, the controller's direct children, so each worker is asked in turn as well; one at + a time, because they all append to the same dump file. Args: process: The hung child. stdout_fd: Read end of the child's stdout, already non-blocking. stderr_fd: Read end of the child's stderr, already non-blocking. env: Environment the child was started with, read for the dump file it was told to write. + options: How the child is watched: its ``pytest-xdist`` worker count and whether to echo its output. Returns: Tuple of ``(dump_section, stdout_bytes, stderr_bytes)``. *dump_section* is a report section, or @@ -232,23 +312,28 @@ def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): if hang_dump.DUMP_SIGNAL is None or not dump_file: return "", stdout_data, stderr_data + targets = [process.pid] + if options.workers > 1: + targets += _child_pids(process.pid) + for _ in range(HANG_DUMP_PASSES): - # Only this pass's share of the file is the dump it asked for. - start = hang_dump.size(dump_file) - try: - os.kill(process.pid, hang_dump.DUMP_SIGNAL) - except OSError: - break + for pid in targets: + # Only this request's share of the file is the dump it asked for. + start = hang_dump.size(dump_file) + try: + os.kill(pid, hang_dump.DUMP_SIGNAL) + except OSError: + continue - # Keep draining while the handler runs, so a full pipe cannot be what stops it answering. - deadline = time.time() + HANG_DUMP_GRACE - while time.time() < deadline: - stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd) - stdout_data += stdout_chunk - stderr_data += stderr_chunk + # Keep draining while the handler runs, so a full pipe cannot be what stops it answering. + deadline = time.time() + HANG_DUMP_GRACE + while time.time() < deadline: + stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd, echo=options.echo) + stdout_data += stdout_chunk + stderr_data += stderr_chunk - if dumped := hang_dump.read_since(dump_file, start): - dumps.append(dumped) + if dumped := hang_dump.read_since(dump_file, start): + dumps.append(dumped if len(targets) == 1 else f"(pid {pid})\n{dumped}") # exit early if the process died if process.poll() is not None: break @@ -260,7 +345,7 @@ def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): return f"=== HANG STACK DUMP (all threads) ===\n{body}", stdout_data, stderr_data -def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, report_file=""): +def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, report_file="", options=None): """Run a command with timeout and capture all output while streaming in real-time. Args: @@ -273,6 +358,7 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo report_file: Path to the JUnit XML report file. When set, the process is given only :data:`SHUTDOWN_GRACE_PERIOD` seconds to exit after the file appears on disk. + options: How the process is watched. Defaults to :class:`_CaptureOptions`. Returns: Tuple of ``(returncode, stdout_bytes, stderr_bytes, kill_reason, @@ -281,6 +367,7 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo did not reach pytest collection in time, or ``"shutdown_hang"`` when the test completed but the process hung during shutdown. """ + options = options or _CaptureOptions() stdout_data = b"" stderr_data = b"" process = None @@ -312,15 +399,21 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo pass start_time = time.time() - startup_done = startup_deadline <= 0 + started = False shutdown_deadline = 0.0 while process.poll() is None: elapsed = time.time() - start_time - if not startup_done: - if b"AppLauncher initialization complete" in stderr_data or b"collected " in stdout_data: - startup_done = True + if not started and ( + b"AppLauncher initialization complete" in stderr_data + or b"collected " in stdout_data + or b" workers [" in stdout_data + ): + started = True + if options.on_started is not None: + options.on_started() + startup_done = started or startup_deadline <= 0 if report_file and not shutdown_deadline and os.path.exists(report_file): shutdown_deadline = time.time() + SHUTDOWN_GRACE_PERIOD @@ -339,7 +432,9 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo # Ask the process where it is stuck before killing it -- SIGKILL below cannot be caught, # so this is the only chance to get a stack out of it. - hang_stacks, dump_stdout, dump_stderr = _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env) + hang_stacks, dump_stdout, dump_stderr = _dump_hung_process_stacks( + process, stdout_fd, stderr_fd, env, options + ) stdout_data += dump_stdout stderr_data += dump_stderr if hang_stacks: @@ -360,7 +455,7 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo wall_time = time.time() - start_time return -1, stdout_data, stderr_data, kill_reason, wall_time, pre_kill_diag - stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd) + stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd, echo=options.echo) stdout_data += stdout_chunk stderr_data += stderr_chunk @@ -739,6 +834,7 @@ def _retry_failed_test_in_fresh_process( kill_reason, wall_time, pre_kill_diag, + options, ): """Retry selected failed test files in a fresh subprocess. @@ -772,8 +868,9 @@ def _retry_failed_test_in_fresh_process( retry_wall_time, pre_kill_diag, ) = capture_test_output_with_timeout( - cmd, timeout, env, startup_deadline=startup_deadline, report_file=report_file + cmd, timeout, env, startup_deadline=startup_deadline, report_file=report_file, options=options ) + _print_captured_output(file_name, options, stdout_data, stderr_data) wall_time += retry_wall_time if not os.path.exists(report_file): # The attempt died before pytest wrote its report; the caller rebuilds the result from @@ -827,6 +924,7 @@ class _PassContext: inject_shard_select: Whether to load the multi-GPU ``mgpu_shard_select`` plugin in the pytest subprocess; true only on a non-default-GPU shard. pytest_targets: Test file or node IDs passed to the pytest subprocess. + capture: How each pytest subprocess is watched, including its ``pytest-xdist`` worker count. """ test_file: str @@ -838,6 +936,7 @@ class _PassContext: env: dict inject_shard_select: bool pytest_targets: list[str] + capture: _CaptureOptions _RESULT_PRIORITY = { @@ -992,6 +1091,8 @@ def _run_one_pass( cmd += ["-p", "mgpu_shard_select"] if ctx.ci_marker: cmd += ["-m", ctx.ci_marker] + if ctx.capture.workers > 1: + cmd += ["-n", str(ctx.capture.workers)] if k_expr is not None: cmd += ["-k", k_expr] cmd += ctx.pytest_targets @@ -1010,8 +1111,14 @@ def _run_one_pass( os.remove(stale_file) returncode, stdout_data, stderr_data, kill_reason, wall_time, pre_kill_diag = capture_test_output_with_timeout( - cmd, ctx.timeout, pass_env, startup_deadline=ctx.startup_deadline, report_file=report_file + cmd, + ctx.timeout, + pass_env, + startup_deadline=ctx.startup_deadline, + report_file=report_file, + options=ctx.capture, ) + _print_captured_output(pass_file_label, ctx.capture, stdout_data, stderr_data) total_wall_time += wall_time has_report = os.path.exists(report_file) @@ -1205,6 +1312,7 @@ def _run_one_pass( kill_reason=kill_reason, wall_time=wall_time, pre_kill_diag=pre_kill_diag, + options=ctx.capture, ) if not os.path.exists(report_file): @@ -1286,117 +1394,180 @@ def _run_one_pass( ) +def _starts_renderer(test_content: str) -> bool: + """Return whether a test file's source starts an RTX renderer: Kit's, by enabling cameras, or OVRTX.""" + return "enable_cameras=True" in test_content or "ovrtx" in test_content.lower() + + +def _test_file_job(test_file: str, max_slots: int) -> TestFileJob: + """Describe a test file to the scheduler: how many slots it holds and whether it starts a renderer.""" + try: + with open(test_file) as fh: + test_content = fh.read() + except OSError: + test_content = "" + file_name = os.path.basename(test_file) + slots = max_slots if file_name in test_settings.EXCLUSIVE_TESTS else _pytest_workers(file_name) + return TestFileJob(path=test_file, slots=slots, renders=_starts_renderer(test_content)) + + +def _pytest_workers(file_name: str) -> int: + """Return how many ``pytest-xdist`` workers a test file is split across, 1 when it is not split.""" + return test_settings.PYTEST_WORKERS.get(file_name, 1) + + +@dataclass +class _FileResult: + """Outcome of every pass of one test file.""" + + reports: list[JUnitXml] + status: dict + failed: bool + + def run_individual_tests(test_files, workspace_root, ci_marker, test_node_ids_by_file=None): - """Run each test file separately, ensuring one finishes before starting the next. + """Run each test file in its own pytest process, up to :data:`TEST_JOBS_ENV_VAR` slots at a time. When ``ISAACLAB_TEST_QUEUE`` names a shared work-queue file, files are claimed from it (work-stealing across sibling shard containers) instead of iterating ``test_files``; each file still runs once, on this container's pinned GPU. """ - failed_tests = [] - test_status = {} - xml_reports = [] - cold_cache_applied = False test_node_ids_by_file = test_node_ids_by_file or {} global_k_expr = os.environ.get("TEST_K_EXPR", "").strip() or None if global_k_expr is not None: logger.info(f"Applying global pytest -k expression to every test file: '{global_k_expr}'") + max_slots = _test_jobs() queue_path = os.environ.get("ISAACLAB_TEST_QUEUE", "") - file_source = _queued_files(queue_path) if queue_path else test_files - - for test_file in file_source: - logger.info(f"\n\n🚀 Running {test_file} independently...\n") - file_name = os.path.basename(test_file) - env = os.environ.copy() - env["PYTHONFAULTHANDLER"] = "1" - - # Multi-GPU lane only: make the device-selection plugin importable in this - # per-file subprocess (injected via ``-p`` in _run_one_pass, not as a - # repo-root conftest). Detect a shard by the runtime device mask excluding cpu - # (position 0) and cuda:0 (position 1) -- the same ISAACLAB_TEST_DEVICES the - # plugin and test_devices() read. The plugin re-checks this; the cheap - # prefix test here leaves single-GPU CI's command (mask unset or "11...") - # unchanged. - _mask = os.environ.get("ISAACLAB_TEST_DEVICES", "") - _inject_shard_select = _mask[:2] == "00" - if _inject_shard_select: - _plugin_dir = os.path.join(workspace_root, ".github", "actions", "multi-gpu") - env["PYTHONPATH"] = _plugin_dir + os.pathsep + env.get("PYTHONPATH", "") - - timeout = test_settings.PER_TEST_TIMEOUTS.get(file_name, test_settings.DEFAULT_TIMEOUT) - - # Read the test file once for cold-cache and device-split detection. - try: - with open(test_file) as fh: - test_content = fh.read() - except OSError: - test_content = "" - - # The first camera-enabled test in a fresh container compiles shaders - # (~600 s). Give it extra time so that doesn't look like a test timeout. - is_cold_cache_test = not cold_cache_applied and "enable_cameras=True" in test_content - if is_cold_cache_test: - timeout += COLD_CACHE_BUFFER - cold_cache_applied = True - logger.info(f"⏱️ Adding {COLD_CACHE_BUFFER}s cold-cache buffer (timeout now {timeout}s)") - - startup_deadline = _resolve_startup_deadline(file_name, timeout, is_cold_cache_test) - - pytest_targets = test_node_ids_by_file.get(os.path.normpath(test_file), [str(test_file)]) - - ctx = _PassContext( - test_file=test_file, - file_name=file_name, + if queue_path: + jobs = (_test_file_job(test_file, max_slots) for test_file in _queued_files(queue_path)) + else: + # Wide files first: split ones are the long poles, and wide ones can only start once enough slots are free. + jobs = sorted((_test_file_job(test_file, max_slots) for test_file in test_files), key=lambda job: -job.slots) + if max_slots > 1: + logger.info(f"Running test files {max_slots} slots at a time; each file's output is printed when it ends") + + def run(job: TestFileJob, context: JobContext) -> _FileResult: + result = _run_test_file( + job, + context, workspace_root=workspace_root, ci_marker=ci_marker, - timeout=timeout, - startup_deadline=startup_deadline, - env=env, - inject_shard_select=_inject_shard_select, - pytest_targets=pytest_targets, + pytest_targets=test_node_ids_by_file.get(os.path.normpath(job.path), [str(job.path)]), + global_k_expr=global_k_expr, + workers=min(_pytest_workers(os.path.basename(job.path)), max_slots), + echo=max_slots == 1, ) - - # On a multi-GPU shard, test_devices() already resolves to this shard's single - # GPU and mgpu_shard_select drops every other variant, so the device_split - # CPU/GPU two-pass (which exists to dodge the process-global device lock when - # CPU and GPU share one container) is unnecessary here — the CPU pass would - # collect zero tests yet still pay full Kit-startup cost. Run once on a shard. - if _inject_shard_select: - passes = [("", None)] - elif is_device_split_file(test_file, source=test_content): - logger.info(f"⚙️ device_split detected — invoking {file_name} once per device (CPU then GPU)") - passes = DEVICE_SPLIT_PASSES - else: - passes = [("", None)] - - merged_status: dict | None = None - for suffix, k_expr in passes: - if global_k_expr is not None: - k_expr = f"({k_expr}) and ({global_k_expr})" if k_expr else global_k_expr - report, status, was_failure = _run_one_pass(ctx, k_expr=k_expr, suffix=suffix) - if report is not None: - xml_reports.append(report) - if was_failure and test_file not in failed_tests: - failed_tests.append(test_file) - merged_status = _merge_pass_status(merged_status, status) - - assert merged_status is not None # the pass list is never empty - test_status[test_file] = merged_status - # When running under the directory-based work queue (option 2), move the # claim entry from inflight// to done// so the post-run # reconciler can distinguish "ran to completion" from "claimed but # crashed mid-test". A claim that stays in inflight at job-end is a # silent drop signal. if queue_path: - _mark_queued_file_done(queue_path, test_file) + _mark_queued_file_done(queue_path, job.path) + return result + + results = run_test_files(jobs, run, max_slots=max_slots) logger.info("~~~~~~~~~~~~ Finished running all tests") + failed_tests = [test_file for test_file, result in results.items() if result.failed] + test_status = {test_file: result.status for test_file, result in results.items()} + xml_reports = [report for result in results.values() for report in result.reports] return failed_tests, test_status, xml_reports +def _run_test_file( + job: TestFileJob, + context: JobContext, + *, + workspace_root: str, + ci_marker: str | None, + pytest_targets: list[str], + global_k_expr: str | None, + workers: int, + echo: bool, +) -> _FileResult: + """Run every pass of one test file and merge their results.""" + test_file = job.path + logger.info(f"\n\n🚀 Running {test_file} independently...\n") + file_name = os.path.basename(test_file) + env = os.environ.copy() + env["PYTHONFAULTHANDLER"] = "1" + + # Multi-GPU lane only: make the device-selection plugin importable in this + # per-file subprocess (injected via ``-p`` in _run_one_pass, not as a + # repo-root conftest). Detect a shard by the runtime device mask excluding cpu + # (position 0) and cuda:0 (position 1) -- the same ISAACLAB_TEST_DEVICES the + # plugin and test_devices() read. The plugin re-checks this; the cheap + # prefix test here leaves single-GPU CI's command (mask unset or "11...") + # unchanged. + _mask = os.environ.get("ISAACLAB_TEST_DEVICES", "") + _inject_shard_select = _mask[:2] == "00" + if _inject_shard_select: + _plugin_dir = os.path.join(workspace_root, ".github", "actions", "multi-gpu") + env["PYTHONPATH"] = _plugin_dir + os.pathsep + env.get("PYTHONPATH", "") + + timeout = test_settings.PER_TEST_TIMEOUTS.get(file_name, test_settings.DEFAULT_TIMEOUT) + + # Read the test file once for device-split detection. + try: + with open(test_file) as fh: + test_content = fh.read() + except OSError: + test_content = "" + + # The first renderer in a fresh container compiles shaders (~600 s). + # Give it extra time so that doesn't look like a test timeout. + is_cold_cache_test = context.renderer_cold + if is_cold_cache_test: + timeout += COLD_CACHE_BUFFER + logger.info(f"⏱️ Adding {COLD_CACHE_BUFFER}s cold-cache buffer (timeout now {timeout}s)") + + startup_deadline = _resolve_startup_deadline(file_name, timeout, is_cold_cache_test) + + ctx = _PassContext( + test_file=test_file, + file_name=file_name, + workspace_root=workspace_root, + ci_marker=ci_marker, + timeout=timeout, + startup_deadline=startup_deadline, + env=env, + inject_shard_select=_inject_shard_select, + pytest_targets=pytest_targets, + capture=_CaptureOptions(echo=echo, workers=workers, on_started=context.mark_started), + ) + + # On a multi-GPU shard, test_devices() already resolves to this shard's single + # GPU and mgpu_shard_select drops every other variant, so the device_split + # CPU/GPU two-pass (which exists to dodge the process-global device lock when + # CPU and GPU share one container) is unnecessary here — the CPU pass would + # collect zero tests yet still pay full Kit-startup cost. Run once on a shard. + if _inject_shard_select: + passes = [("", None)] + elif is_device_split_file(test_file, source=test_content): + logger.info(f"⚙️ device_split detected — invoking {file_name} once per device (CPU then GPU)") + passes = DEVICE_SPLIT_PASSES + else: + passes = [("", None)] + + reports = [] + failed = False + merged_status: dict | None = None + for suffix, k_expr in passes: + if global_k_expr is not None: + k_expr = f"({k_expr}) and ({global_k_expr})" if k_expr else global_k_expr + report, status, was_failure = _run_one_pass(ctx, k_expr=k_expr, suffix=suffix) + if report is not None: + reports.append(report) + failed = failed or was_failure + merged_status = _merge_pass_status(merged_status, status) + + assert merged_status is not None # the pass list is never empty + return _FileResult(reports=reports, status=merged_status, failed=failed) + + def _resolve_startup_deadline(file_name: str, timeout: int, is_cold_cache_test: bool) -> int: """Resolve the startup deadline for one independently launched test file.""" base_deadline = test_settings.PER_TEST_STARTUP_TIMEOUTS.get(file_name, STARTUP_DEADLINE) diff --git a/tools/hang_dump.py b/tools/hang_dump.py index ad519be57c5..9fef9b8b4b9 100644 --- a/tools/hang_dump.py +++ b/tools/hang_dump.py @@ -107,8 +107,10 @@ def register(): path = dump_path() if not path or not is_supported(): return False + # pytest-xdist workers share the controller's dump file, which the controller already truncated. + mode = "a" if os.environ.get("PYTEST_XDIST_WORKER") else "w" try: - _dump_file = open(path, "w") # noqa: SIM115 (held open for the process lifetime, see above) + _dump_file = open(path, mode) # noqa: SIM115 (held open for the process lifetime, see above) except OSError: return False faulthandler.register(DUMP_SIGNAL, file=_dump_file, all_threads=True, chain=False) diff --git a/tools/test_crash_journal.py b/tools/test_crash_journal.py index 86eb6531c1c..a07e6f471b8 100644 --- a/tools/test_crash_journal.py +++ b/tools/test_crash_journal.py @@ -429,6 +429,42 @@ def test_drop(): assert read_journal(str(journal_file)).collected == [f"{_FILE}::test_keep"] +def test_an_xdist_run_journals_each_event_once(tmp_path): + """Regression test for ``pytest-xdist`` runs journaling every event twice. + + The workers and the controller both fire the per-test hooks, and every worker fires the + collection hook. Duplicated starts turn a worker crash that xdist recovered from into an + unmatched start, so a later session crash would be blamed on a test that already reported. + """ + pytest.importorskip("xdist") + _write_test_module( + tmp_path, + """ + def test_a(): + pass + + def test_b(): + assert 1 == 2 + + def test_c(): + pass + """, + ) + journal_file = tmp_path / "journal.jsonl" + junit_file = tmp_path / "report.xml" + _run_pytest(tmp_path, journal_file, junit_file, "-p", "xdist.plugin", "-n", "2") + + records = [json.loads(line) for line in journal_file.read_text(encoding="utf-8").splitlines()] + node_ids = [f"{_FILE}::test_{name}" for name in "abc"] + assert [record["event"] for record in records].count("collected") == 1 + for event in ("start", "result", "finish"): + assert sorted(record["node_id"] for record in records if record["event"] == event) == node_ids + + journal = read_journal(str(journal_file)) + assert journal.collected == node_ids + assert journal.culprit is None + + # -- artificial crashes in a real pytest run -------------------------------------------------- diff --git a/tools/test_file_scheduler.py b/tools/test_file_scheduler.py new file mode 100644 index 00000000000..7d1791f9a6d --- /dev/null +++ b/tools/test_file_scheduler.py @@ -0,0 +1,231 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Unit tests for ``tools/_file_scheduler.py``.""" + +from __future__ import annotations + +import threading + +import pytest +from _file_scheduler import JobContext, TestFileJob, run_test_files + +_TIMEOUT = 10.0 +"""Seconds any wait in these tests may take before it counts as a deadlock.""" + + +class _Harness: + """A ``run`` callable whose jobs start and finish only when the test says so. + + Records which jobs are in flight, so a test can assert on concurrency at each step instead of relying + on sleeps. + """ + + def __init__(self): + self._lock = threading.Condition() + self.running: dict[str, TestFileJob] = {} + self.started: list[str] = [] + self.contexts: dict[str, JobContext] = {} + self.peak_slots = 0 + self._release: dict[str, threading.Event] = {} + + def __call__(self, job: TestFileJob, context: JobContext) -> str: + release = threading.Event() + with self._lock: + self.running[job.path] = job + self.started.append(job.path) + self.contexts[job.path] = context + self._release[job.path] = release + self.peak_slots = max(self.peak_slots, sum(j.slots for j in self.running.values())) + self._lock.notify_all() + assert release.wait(_TIMEOUT), f"{job.path} was never released" + with self._lock: + del self.running[job.path] + self._lock.notify_all() + return f"result:{job.path}" + + def wait_running(self, *paths: str) -> None: + """Wait until exactly ``paths`` are in flight.""" + with self._lock: + assert self._lock.wait_for(lambda: set(self.running) == set(paths), _TIMEOUT), ( + f"expected {sorted(paths)} running, have {sorted(self.running)}" + ) + + def finish(self, path: str) -> None: + with self._lock: + self._release[path].set() + assert self._lock.wait_for(lambda: path not in self.running, _TIMEOUT) + + +def _run_in_background(jobs, harness, max_slots): + outcome = {} + + def target(): + try: + outcome["results"] = run_test_files(jobs, harness, max_slots=max_slots) + except BaseException as error: + outcome["error"] = error + + thread = threading.Thread(target=target, daemon=True) + thread.start() + return thread, outcome + + +def test_one_slot_runs_jobs_in_order_on_the_calling_thread(): + seen = [] + + def run(job, context): + seen.append((job.path, threading.current_thread() is threading.main_thread())) + return job.path.upper() + + results = run_test_files([TestFileJob("a"), TestFileJob("b", slots=4)], run, max_slots=1) + + assert seen == [("a", True), ("b", True)] + assert results == {"a": "A", "b": "B"} + + +def test_jobs_fill_the_slots_and_the_next_starts_when_one_finishes(): + harness = _Harness() + jobs = [TestFileJob(name) for name in "abcd"] + thread, outcome = _run_in_background(jobs, harness, max_slots=3) + + harness.wait_running("a", "b", "c") + harness.finish("b") + harness.wait_running("a", "c", "d") + for name in "acd": + harness.finish(name) + thread.join(_TIMEOUT) + + assert harness.peak_slots == 3 + assert outcome["results"] == {name: f"result:{name}" for name in "abcd"} + + +def test_a_wide_job_holds_its_slots_and_is_not_overtaken(): + harness = _Harness() + jobs = [TestFileJob("narrow"), TestFileJob("wide", slots=3), TestFileJob("after")] + thread, outcome = _run_in_background(jobs, harness, max_slots=3) + + # "wide" needs all three slots; "after" fits beside "narrow" but must not jump the queue. + harness.wait_running("narrow") + with harness._lock: + assert not harness._lock.wait_for(lambda: "wide" in harness.started, timeout=0.2) + harness.finish("narrow") + harness.wait_running("wide") + harness.finish("wide") + harness.wait_running("after") + harness.finish("after") + thread.join(_TIMEOUT) + + assert harness.started == ["narrow", "wide", "after"] + assert harness.peak_slots == 3 + + +def test_a_job_wider_than_the_budget_gets_every_slot(): + harness = _Harness() + thread, outcome = _run_in_background([TestFileJob("huge", slots=8), TestFileJob("next")], harness, max_slots=2) + + harness.wait_running("huge") + harness.finish("huge") + harness.wait_running("next") + harness.finish("next") + thread.join(_TIMEOUT) + + assert "error" not in outcome + + +def test_rendering_jobs_never_overlap_and_others_overtake_them(): + harness = _Harness() + jobs = [ + TestFileJob("render-1", renders=True), + TestFileJob("render-2", renders=True), + TestFileJob("plain"), + TestFileJob("sentinel"), + ] + thread, outcome = _run_in_background(jobs, harness, max_slots=2) + + harness.wait_running("render-1", "plain") + assert harness.contexts["render-1"].renderer_cold + assert not harness.contexts["plain"].renderer_cold + + # Starting up is not enough: the slot "plain" frees goes past the second renderer to the job behind it. + harness.contexts["render-1"].mark_started() + harness.finish("plain") + harness.wait_running("render-1", "sentinel") + harness.finish("render-1") + harness.wait_running("render-2", "sentinel") + assert not harness.contexts["render-2"].renderer_cold + + harness.finish("render-2") + harness.finish("sentinel") + thread.join(_TIMEOUT) + assert "error" not in outcome + + +def test_a_renderer_that_ends_without_starting_releases_the_next_which_is_still_cold(): + harness = _Harness() + jobs = [TestFileJob("render-1", renders=True), TestFileJob("render-2", renders=True)] + thread, outcome = _run_in_background(jobs, harness, max_slots=2) + + harness.wait_running("render-1") + harness.finish("render-1") # e.g. killed by a startup hang, never reaching collection + harness.wait_running("render-2") + assert harness.contexts["render-2"].renderer_cold + harness.finish("render-2") + thread.join(_TIMEOUT) + + assert "error" not in outcome + + +def test_only_the_first_renderer_is_cold_when_run_serially(): + contexts = {} + + def run(job, context): + contexts[job.path] = context + context.mark_started() + + run_test_files([TestFileJob("render-1", renders=True), TestFileJob("render-2", renders=True)], run, max_slots=1) + + assert contexts["render-1"].renderer_cold + assert not contexts["render-2"].renderer_cold + + +def test_jobs_are_pulled_only_when_they_can_start(): + harness = _Harness() + pulled = [] + + def claim(): + for name in "abc": + pulled.append(name) + yield TestFileJob(name) + + thread, outcome = _run_in_background(claim(), harness, max_slots=2) + + harness.wait_running("a", "b") + assert pulled == ["a", "b"] + harness.finish("a") + harness.wait_running("b", "c") + harness.finish("b") + harness.finish("c") + thread.join(_TIMEOUT) + + assert pulled == ["a", "b", "c"] + + +def test_an_error_in_one_job_is_raised_after_the_others_finish(): + finished = threading.Event() + other_started = threading.Event() + + def run(job, context): + if job.path == "bad": + assert other_started.wait(_TIMEOUT) + raise RuntimeError("boom") + other_started.set() + finished.wait(0.2) + finished.set() + return job.path + + with pytest.raises(RuntimeError, match="boom"): + run_test_files([TestFileJob("bad"), TestFileJob("good")], run, max_slots=2) + assert finished.is_set() diff --git a/tools/test_settings.py b/tools/test_settings.py index bcba63b6b65..ca63b8d0aa3 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -80,6 +80,25 @@ } """Per-test startup timeouts for cold external asset downloads.""" +PYTEST_WORKERS = { + # 20 independent export round trips, ~18 min serially: the RL job's long pole. + "test_leapp_export_flow.py": 4, +} +"""Test files split across ``pytest-xdist`` workers, and how many. + +Every worker starts its own process -- and its own Kit app and simulation, for files that launch one -- so +splitting only pays off for files whose tests take far longer than that startup. List a file here when it +is the long pole of its CI job. Each worker holds one of the job's ``TEST_JOBS`` slots; a file never gets +more workers than the job has slots. +""" + +EXCLUSIVE_TESTS = [ + # Both assert wall-clock limits, which other files running at the same time would eat into. + "test_kit_startup_performance.py", + "test_robot_load_performance.py", +] +"""Test files that run with no other test file alongside them, when a job runs several files at once.""" + CUROBO_PLANNER_TESTS = [ "test_curobo_planner_franka.py", "test_curobo_planner_cube_stack.py", diff --git a/uv.lock b/uv.lock index 7cc1458e74b..3f98b1aa491 100644 --- a/uv.lock +++ b/uv.lock @@ -1105,6 +1105,15 @@ epath = [ { name = "zipp" }, ] +[[package]] +name = "execnet" +version = "2.1.2" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/bf/89/780e11f9588d9e7128a3f87788354c7946a9cbb1401ad38a48c4db9a4f07/execnet-2.1.2.tar.gz", hash = "sha256:63d83bfdd9a23e35b9c6a3261412324f964c2ec8dcd8d3c6916ee9373e0befcd", size = 166622, upload-time = "2025-11-12T09:56:37.75Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/ab/84/02fc1827e8cdded4aa65baef11296a9bbe595c474f0d6d758af082d849fd/execnet-2.1.2-py3-none-any.whl", hash = "sha256:67fba928dd5a544b783f6056f449e5e3931a5c378b128bc18501f7ea79e296ec", size = 40708, upload-time = "2025-11-12T09:56:36.333Z" }, +] + [[package]] name = "executing" version = "2.2.1" @@ -1818,6 +1827,7 @@ dev = [ { name = "myst-parser" }, { name = "pytest" }, { name = "pytest-mock" }, + { name = "pytest-xdist" }, { name = "sphinx" }, { name = "sphinx-book-theme" }, { name = "sphinx-copybutton" }, @@ -1903,6 +1913,7 @@ test = [ { name = "junitparser" }, { name = "pytest" }, { name = "pytest-mock" }, + { name = "pytest-xdist" }, ] tetrahedralization = [ { name = "pytetwild", extra = ["all"] }, @@ -2007,6 +2018,7 @@ requires-dist = [ { name = "pyopengl-accelerate", specifier = ">=3.1.0" }, { name = "pytest", marker = "extra == 'test'" }, { name = "pytest-mock", marker = "extra == 'test'" }, + { name = "pytest-xdist", marker = "extra == 'test'" }, { name = "pytetwild", extras = ["all"], marker = "extra == 'tetrahedralization'", specifier = ">=0.3.0,<0.4" }, { name = "ray", extras = ["default"], marker = "extra == 'rlinf'", specifier = ">=2.47.0" }, { name = "requests", specifier = ">=2.25.0" }, @@ -4405,6 +4417,19 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/5a/cc/06253936f4a7fa2e0f48dfe6d851d9c56df896a9ab09ac019d70b760619c/pytest_mock-3.15.1-py3-none-any.whl", hash = "sha256:0a25e2eb88fe5168d535041d09a4529a188176ae608a6d249ee65abc0949630d", size = 10095, upload-time = "2025-09-16T16:37:25.734Z" }, ] +[[package]] +name = "pytest-xdist" +version = "3.8.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "execnet" }, + { name = "pytest" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/78/b4/439b179d1ff526791eb921115fca8e44e596a13efeda518b9d845a619450/pytest_xdist-3.8.0.tar.gz", hash = "sha256:7e578125ec9bc6050861aa93f2d59f1d8d085595d6551c2c90b6f4fad8d3a9f1", size = 88069, upload-time = "2025-07-01T13:30:59.346Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/ca/31/d4e37e9e550c2b92a9cbc2e4d0b7420a27224968580b5a447f420847c975/pytest_xdist-3.8.0-py3-none-any.whl", hash = "sha256:202ca578cfeb7370784a8c33d6d05bc6e13b4f25b5053c30a152269fd10f0b88", size = 46396, upload-time = "2025-07-01T13:30:56.632Z" }, +] + [[package]] name = "pytetwild" version = "0.3.0" From 9c7cfc64333b4962d6420da67f1d8bb3a2f55880 Mon Sep 17 00:00:00 2001 From: Lynn Date: Fri, 25 Sep 2026 07:42:47 -0400 Subject: [PATCH 4/7] Fix LSTM torque-speed clipping with explicit joint velocity (#7988) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description `ActuatorNetLSTM.compute` used DC-motor clipping without updating its cached joint velocity, so the torque-speed limits were always evaluated at zero velocity. With the ANYmal actuator settings (saturation 120 N·m, effort limit 80 N·m, velocity limit 7.5 rad/s), it allowed 80 N·m of positive torque at 7.5 rad/s when the limit should be zero. Explicit PD and neural-network compute pass the current velocity directly into DC-motor clipping. This removes the cached `_joint_vel` tensor, its allocation and copies, and the redundant `DCMotor.compute` override. Clipping uses a local clamped value, preserving the measured velocity and the existing torque-speed equations. The base `_clip_effort(effort, *args, **kwargs)` only clamps effort and leaves model-specific inputs to subclasses. `DCMotor._clip_effort(effort, joint_vel)` owns velocity-dependent clipping; implicit actuators retain `_clip_effort(effort)`. Custom overrides used by explicit PD and neural-network actuators must accept the velocity argument, as documented in the changelog. ## Type of change - Bug fix and removal of redundant actuator state ## Tests Extended the existing DC-motor and ideal-PD tests; no new test file. - DC-motor, ideal-PD, and implicit-actuator suites: **41 passed**, including CPU and CUDA cases. Formatting and pre-commit checks passed. - The LSTM regression calls real `compute` with a constant 100 N·m network and checks expected efforts `80, 60, 0, 80` at velocities `0, 3.75, 7.5, -7.5`. Restoring the original production implementation fails numerically: two values differ, including 80 instead of zero at the velocity limit. - Existing four-quadrant DC-motor compute cases also verify that measured velocity stays unchanged. The LSTM regression guards against reintroducing cached `_joint_vel` state. - One assertion in the existing ideal-PD compute test verifies that base clipping needs only effort. It fails against the previous required-velocity signature. - A temporary two-step smoke check exercised DC-motor, LSTM, and MLP compute on CPU and CUDA with changing velocities beyond the curve's corner. Efforts matched the expected values and measured velocities stayed unchanged. ## 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 - [x] I have run the `pre-commit` checks with `./isaaclab.sh --format` - [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/isaaclab/changelog.d/` - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --------- Signed-off-by: Lynn Co-authored-by: Octi Zhang --- ...ix-actuator-net-lstm-velocity-clipping.rst | 8 ++++ .../isaaclab/actuators/actuator_base.py | 8 +++- .../isaaclab/actuators/actuator_net.py | 7 +-- .../isaaclab/actuators/actuator_pd.py | 26 +++-------- .../isaaclab/test/actuators/test_dc_motor.py | 45 +++++++++++++++++-- .../test/actuators/test_ideal_pd_actuator.py | 1 + 6 files changed, 64 insertions(+), 31 deletions(-) create mode 100644 source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst diff --git a/source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst b/source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst new file mode 100644 index 00000000000..b5dd14c2ad8 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst @@ -0,0 +1,8 @@ +Fixed +^^^^^ + +* Fixed :class:`~isaaclab.actuators.ActuatorNetLSTM` clipping its output with a zero joint velocity. The DC-motor + torque-speed limits now use the current joint velocity, as :class:`~isaaclab.actuators.ActuatorNetMLP` does. +* Passed measured joint velocity directly into DC-motor clipping, removing the cached velocity and its redundant + copies. The base ``_clip_effort`` accepts ``(effort, *args, **kwargs)``; custom overrides used by explicit PD and + neural-network actuators must accept ``(effort, joint_vel)``. Implicit actuator clipping still takes only effort. diff --git a/source/isaaclab/isaaclab/actuators/actuator_base.py b/source/isaaclab/isaaclab/actuators/actuator_base.py index 3b8f181908c..b904b6f512f 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_base.py +++ b/source/isaaclab/isaaclab/actuators/actuator_base.py @@ -310,11 +310,15 @@ def compute( Helper functions. """ - def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: - """Clip the desired torques based on the motor limits. + def _clip_effort(self, effort: torch.Tensor, *args, **kwargs) -> torch.Tensor: + """Clip the desired effort using actuator effort limits. + + Model-specific inputs are handled by subclasses. Args: effort: The effort to clip [N or N·m, depending on joint type]. + *args: Model-specific positional inputs. Unused by the base implementation. + **kwargs: Model-specific keyword inputs. Unused by the base implementation. Returns: The clipped effort [N or N·m, depending on joint type]. diff --git a/source/isaaclab/isaaclab/actuators/actuator_net.py b/source/isaaclab/isaaclab/actuators/actuator_net.py index 148dce18481..f96717d2bd0 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_net.py +++ b/source/isaaclab/isaaclab/actuators/actuator_net.py @@ -88,7 +88,7 @@ def compute( self.computed_effort = torques.reshape(self._num_envs, self.num_joints) # clip the computed effort based on the motor limits - self.applied_effort = self._clip_effort(self.computed_effort) + self.applied_effort = self._clip_effort(self.computed_effort, joint_vel) # return torques control_action.joint_efforts = self.applied_effort @@ -152,9 +152,6 @@ def compute( # -- velocity self._joint_vel_history = self._joint_vel_history.roll(1, 1) self._joint_vel_history[:, 0] = joint_vel - # save current joint vel for dc-motor clipping - self._joint_vel[:] = joint_vel - # compute network inputs # -- positions pos_input = torch.cat([self._joint_pos_error_history[:, i].unsqueeze(2) for i in self.cfg.input_idx], dim=2) @@ -178,7 +175,7 @@ def compute( self.computed_effort = torques.view(self._num_envs, self.num_joints) * self.cfg.torque_scale # clip the computed effort based on the motor limits - self.applied_effort = self._clip_effort(self.computed_effort) + self.applied_effort = self._clip_effort(self.computed_effort, joint_vel) # return torques control_action.joint_efforts = self.applied_effort diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd.py b/source/isaaclab/isaaclab/actuators/actuator_pd.py index 70d2d21ecd3..b11efb4ad10 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_pd.py +++ b/source/isaaclab/isaaclab/actuators/actuator_pd.py @@ -325,7 +325,7 @@ def compute( # calculate the desired joint torques self.computed_effort = self.stiffness * error_pos + self.damping * error_vel + control_action.joint_efforts # clip the torques based on the motor limits - self.applied_effort = self._clip_effort(self.computed_effort) + self.applied_effort = self._clip_effort(self.computed_effort, joint_vel) # set the computed actions back into the control action control_action.joint_efforts = self.applied_effort control_action.joint_positions = None @@ -413,33 +413,19 @@ def __init__(self, cfg: DCMotorCfg, *args, **kwargs): self._vel_at_effort_lim = self.actuator_velocity_limit * ( 1 + self.actuator_effort_limit / self._saturation_effort ) - # prepare joint vel buffer for max effort computation - self._joint_vel = torch.zeros_like(self.computed_effort) # create buffer for zeros effort self._zeros_effort = torch.zeros_like(self.computed_effort) - """ - Operations. - """ - - def compute( - self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor - ) -> ArticulationActions: - # save current joint vel - self._joint_vel[:] = joint_vel - # calculate the desired joint torques - return super().compute(control_action, joint_pos, joint_vel) - """ Helper functions. """ - def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: - # save current joint vel - self._joint_vel[:] = torch.clip(self._joint_vel, min=-self._vel_at_effort_lim, max=self._vel_at_effort_lim) + def _clip_effort(self, effort: torch.Tensor, joint_vel: torch.Tensor) -> torch.Tensor: + # Clamp the local value without modifying the measured joint velocity. + joint_vel = torch.clip(joint_vel, min=-self._vel_at_effort_lim, max=self._vel_at_effort_lim) # compute torque limits - torque_speed_top = self._saturation_effort * (1.0 - self._joint_vel / self.actuator_velocity_limit) - torque_speed_bottom = self._saturation_effort * (-1.0 - self._joint_vel / self.actuator_velocity_limit) + torque_speed_top = self._saturation_effort * (1.0 - joint_vel / self.actuator_velocity_limit) + torque_speed_bottom = self._saturation_effort * (-1.0 - joint_vel / self.actuator_velocity_limit) # -- max limit max_effort = torch.clip(torque_speed_top, max=self.actuator_effort_limit) # -- min limit diff --git a/source/isaaclab/test/actuators/test_dc_motor.py b/source/isaaclab/test/actuators/test_dc_motor.py index 67ea950f99a..22022f95097 100644 --- a/source/isaaclab/test/actuators/test_dc_motor.py +++ b/source/isaaclab/test/actuators/test_dc_motor.py @@ -6,7 +6,7 @@ import pytest import torch -from isaaclab.actuators import DCMotorCfg +from isaaclab.actuators import ActuatorNetLSTMCfg, DCMotorCfg from isaaclab.utils.types import ArticulationActions pytestmark = pytest.mark.integration @@ -130,13 +130,15 @@ def test_dc_motor_clip(test_point): torque, speed = torque_speed_pairs[test_point] zeros = torch.zeros(num_envs, num_joints, device=device) + joint_vel = torch.full_like(zeros, speed) control_action = ArticulationActions( joint_positions=zeros.clone(), joint_velocities=zeros.clone(), joint_efforts=torch.full_like(zeros, torque) ) - applied = actuator.compute(control_action, joint_pos=zeros, joint_vel=torch.full_like(zeros, speed)) + applied = actuator.compute(control_action, joint_pos=zeros, joint_vel=joint_vel) expected = torch.full_like(zeros, expected_clipped_effort[test_point]) torch.testing.assert_close(actuator.applied_effort, expected) torch.testing.assert_close(applied.joint_efforts, expected) + torch.testing.assert_close(joint_vel, torch.full_like(joint_vel, speed)) def test_dc_motor_clip_with_per_joint_saturation_effort(): @@ -167,6 +169,41 @@ def test_dc_motor_clip_with_per_joint_saturation_effort(): # at half the no-load speed each joint delivers half of its own stall torque, and the shared # effort limit is high enough to clip neither - actuator._joint_vel[:] = 25.0 - clipped_effort = actuator._clip_effort(torch.full((1, 2), 500.0, device=device)) + joint_vel = torch.full((1, 2), 25.0, device=device) + clipped_effort = actuator._clip_effort(torch.full((1, 2), 500.0, device=device), joint_vel) torch.testing.assert_close(clipped_effort, torch.tensor([[50.0, 95.0]], device=device)) + + +class _ConstantTorqueLSTM(torch.nn.Module): + """LSTM-shaped network that always requests 100 N·m.""" + + def __init__(self): + super().__init__() + self.lstm = torch.nn.LSTM(input_size=2, hidden_size=4, num_layers=1, batch_first=True) + + def forward( + self, x: torch.Tensor, hc: tuple[torch.Tensor, torch.Tensor] + ) -> tuple[torch.Tensor, tuple[torch.Tensor, torch.Tensor]]: + return torch.full((x.shape[0], 1), 100.0), hc + + +def test_lstm_actuator_clips_with_torque_speed_curve(tmp_path): + """LSTM compute must supply the current velocity to DC-motor clipping.""" + network_file = tmp_path / "constant_lstm.pt" + torch.jit.script(_ConstantTorqueLSTM()).save(str(network_file)) + cfg = ActuatorNetLSTMCfg( + joint_names_expr=["joint_.*"], + network_file=str(network_file), + saturation_effort=120.0, + actuator_effort_limit=80.0, + actuator_velocity_limit=7.5, + ) + actuator = cfg.class_type(cfg, joint_names=["joint_0", "joint_1"], joint_ids=[0, 1], num_envs=2, device="cpu") + + zeros = torch.zeros(2, 2) + joint_vel = torch.tensor([[0.0, 3.75], [7.5, -7.5]]) + action = actuator.compute(ArticulationActions(joint_positions=zeros), zeros, joint_vel) + + # Positive torque falls to zero at the velocity limit; braking torque remains available. + torch.testing.assert_close(action.joint_efforts, torch.tensor([[80.0, 60.0], [0.0, 80.0]])) + assert "_joint_vel" not in vars(actuator) diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py index 6d0a54a8167..e809aab7f50 100644 --- a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -175,6 +175,7 @@ def test_ideal_pd_compute(effort_lim): actuator.applied_effort, computed_control_action.joint_efforts, ) + torch.testing.assert_close(actuator._clip_effort(actuator.computed_effort), actuator.applied_effort) if __name__ == "__main__": From 417aafcba64292e7859f1cc1888f6462cd8e59a7 Mon Sep 17 00:00:00 2001 From: Lynn Date: Fri, 25 Sep 2026 08:13:56 -0400 Subject: [PATCH 5/7] Fix quaternion rotation broadcasting and body gravity observations (#7987) # Description `quat_apply` and `quat_apply_inverse` flattened quaternion and vector batches independently, losing broadcasting and causing `body_projected_gravity_b` to fail for multiple bodies. Both helpers now broadcast leading dimensions following NumPy rules and return the broadcast result shape. They use tensor views for broadcasting, without materializing repeated inputs. The observation only preserves an explicit body dimension, including integer body selection. The frame-transformer tutorial explicitly selects its single quaternion so its configured offset remains a flat 3-vector. Incompatible batch shapes now raise an error even when their element counts match; callers intentionally pairing flattened arrays must reshape them explicitly. The changelog also documents retained singleton batch dimensions and potentially noncontiguous outputs for transposed inputs; use `reshape` when flattening those outputs. Consolidated forward/inverse coverage in the existing math test and moved the gravity regression into `test_gravity_randomization.py`, using different gravity directions per environment. No new test file. Merged current `develop` and retained its multidimensional rotation coverage. ## Validation - Existing math, gravity, and reorientation checks passed. The caller audit additionally passed 66 tests: 36 CPU/CUDA quaternion cases, 21 operational-space controller cases, and 9 existing dexterous-task, FORGE, visualization, and sampler cases. - 42 CPU/CUDA comparisons of consumer tensor paths and gradients agreed with the old helpers. The tutorial offset failed backend-style stacking before its one-line correction and passed afterward with the analytic -90-degree yaw result. - Confirmed the broadcasting and gravity regressions fail with the old helpers and pass with the fix, including rejection of incompatible equal-element-count batches. - All pre-commit checks passed. - Single-thread CPU / RTX 5090 microbenchmarks: matching `(4096, 16)` GPU batches improved from about 27 to 25 microseconds; the gravity broadcast case improved from about 33 to 27 microseconds. Matching large CPU batches were about 17% slower (1.03 to 1.20 ms); matching 4096-element CPU batches were about unchanged (73 to 71 microseconds). ## Type of change - Bug fix, with an explicit compatibility change for callers relying on implicit flattened pairing. ## 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 with `./isaaclab.sh --format` - [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/isaaclab/changelog.d/` - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --------- Signed-off-by: Lynn Co-authored-by: Octi Zhang --- .../04_sensors/run_frame_transformer.py | 2 +- .../fix-body-projected-gravity-multi-body.rst | 10 +++ .../isaaclab/envs/mdp/observations.py | 2 +- source/isaaclab/isaaclab/utils/math.py | 28 +++---- .../test/envs/test_gravity_randomization.py | 29 ++++++- source/isaaclab/test/utils/test_math.py | 75 +++++++++---------- 6 files changed, 84 insertions(+), 62 deletions(-) create mode 100644 source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst diff --git a/scripts/tutorials/04_sensors/run_frame_transformer.py b/scripts/tutorials/04_sensors/run_frame_transformer.py index 7346cd11e50..4b44fcf38d5 100644 --- a/scripts/tutorials/04_sensors/run_frame_transformer.py +++ b/scripts/tutorials/04_sensors/run_frame_transformer.py @@ -64,7 +64,7 @@ def define_sensor() -> FrameTransformer: """Defines the FrameTransformer sensor to add to the scene.""" # define offset rot_offset = math_utils.quat_from_euler_xyz(torch.zeros(1), torch.zeros(1), torch.tensor(-math.pi / 2)) - pos_offset = math_utils.quat_apply(rot_offset, torch.tensor([0.08795, 0.01305, -0.33797])) + pos_offset = math_utils.quat_apply(rot_offset[0], torch.tensor([0.08795, 0.01305, -0.33797])) # Example using .* to get full body + LF_FOOT frame_transformer_cfg = FrameTransformerCfg( diff --git a/source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst b/source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst new file mode 100644 index 00000000000..1d1edd535f4 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst @@ -0,0 +1,10 @@ +Fixed +^^^^^ + +* Fixed :func:`~isaaclab.utils.math.quat_apply` and :func:`~isaaclab.utils.math.quat_apply_inverse` to broadcast + leading dimensions following NumPy rules. This fixed :func:`~isaaclab.envs.mdp.observations.body_projected_gravity_b` + for multiple selected bodies. **Breaking change:** results retain the broadcast batch shape, including singleton + dimensions. Use a quaternion of shape ``(4,)`` for an unbatched vector result of shape ``(3,)``. Incompatible batch + shapes now raise an error even when their element counts match; callers relying on flattened pairing must explicitly + reshape their inputs to matching batch shapes. Outputs may be noncontiguous for transposed inputs; use ``reshape`` + instead of ``view`` when flattening these results. diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 383b391555f..3ca6b9b1d40 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -169,7 +169,7 @@ def body_projected_gravity_b( [x,y,z]. Output is stacked horizontally per body. """ asset: Articulation = env.scene[asset_cfg.name] - body_quat = asset.data.body_quat_w.torch[:, asset_cfg.body_ids] + body_quat = asset.data.body_quat_w.torch[:, asset_cfg.body_ids].reshape(env.num_envs, -1, 4) # ``GRAVITY_VEC_W`` carries the per-env world-frame gravity in m/s^2 (Newton # backend) or scene-wide gravity (PhysX backend). gravity_w = asset.data.GRAVITY_VEC_W.torch diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index 1c298a9d856..1a454d80acc 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -634,44 +634,36 @@ def quat_box_plus(q: torch.Tensor, delta: torch.Tensor, eps: float = 1.0e-6) -> def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Apply a quaternion rotation to a vector. + Leading dimensions follow NumPy broadcasting rules. Incompatible batch shapes raise an error. + Args: quat: The quaternion in (x, y, z, w). Shape is (..., 4). vec: The vector in (x, y, z). Shape is (..., 3). Returns: - The rotated vector in (x, y, z). Shape is (..., 3). + The rotated vector in (x, y, z). Shape is the broadcast batch shape followed by (3,). """ - # store shape - shape = vec.shape - # reshape to (N, 3) for multiplication - quat = quat.reshape(-1, 4) - vec = vec.reshape(-1, 3) - # extract components from quaternions (xyzw format) - xyz = quat[:, :3] + xyz, vec = torch.broadcast_tensors(quat[..., :3], vec) t = xyz.cross(vec, dim=-1) * 2 - return (vec + quat[:, 3:4] * t + xyz.cross(t, dim=-1)).view(shape) + return vec + quat[..., 3:4] * t + xyz.cross(t, dim=-1) @torch.jit.script def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Apply an inverse quaternion rotation to a vector. + Leading dimensions follow NumPy broadcasting rules. Incompatible batch shapes raise an error. + Args: quat: The quaternion in (x, y, z, w). Shape is (..., 4). vec: The vector in (x, y, z). Shape is (..., 3). Returns: - The rotated vector in (x, y, z). Shape is (..., 3). + The rotated vector in (x, y, z). Shape is the broadcast batch shape followed by (3,). """ - # store shape - shape = vec.shape - # reshape to (N, 3) for multiplication - quat = quat.reshape(-1, 4) - vec = vec.reshape(-1, 3) - # extract components from quaternions (xyzw format) - xyz = quat[:, :3] + xyz, vec = torch.broadcast_tensors(quat[..., :3], vec) t = xyz.cross(vec, dim=-1) * 2 - return (vec - quat[:, 3:4] * t + xyz.cross(t, dim=-1)).view(shape) + return vec - quat[..., 3:4] * t + xyz.cross(t, dim=-1) @torch.jit.script diff --git a/source/isaaclab/test/envs/test_gravity_randomization.py b/source/isaaclab/test/envs/test_gravity_randomization.py index 0668cfc0b7b..710dfbb0508 100644 --- a/source/isaaclab/test/envs/test_gravity_randomization.py +++ b/source/isaaclab/test/envs/test_gravity_randomization.py @@ -3,15 +3,17 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for scene-wide gravity randomization.""" +"""Tests for gravity randomization and observations.""" +import math from types import SimpleNamespace import pytest import torch from isaaclab.envs.mdp.events import randomize_physics_scene_gravity -from isaaclab.managers import EventTermCfg +from isaaclab.envs.mdp.observations import body_projected_gravity_b +from isaaclab.managers import EventTermCfg, SceneEntityCfg @pytest.mark.parametrize("backend", ["physx", "ovphysx"]) @@ -45,3 +47,26 @@ def test_scene_wide_backends_use_configured_distribution(monkeypatch: pytest.Mon torch.manual_seed(0) gravity_event(env, env_ids=None, **cfg.params) assert gravity_sink.value == pytest.approx((1.0, 2.0, 3.0)) + + +@pytest.mark.unit +def test_body_projected_gravity_b_stacks_every_selected_body(): + """Each selected body receives its own environment's gravity, including integer selections.""" + num_envs = 2 + angles = (0.0, 0.5 * math.pi, math.pi) + body_quat = torch.tensor([[math.sin(0.5 * a), 0.0, 0.0, math.cos(0.5 * a)] for a in angles]).repeat(num_envs, 1, 1) + asset = SimpleNamespace( + data=SimpleNamespace( + body_quat_w=SimpleNamespace(torch=body_quat), + GRAVITY_VEC_W=SimpleNamespace(torch=torch.tensor([[0.0, 0.0, -9.81], [0.0, 9.81, 0.0]])), + ) + ) + env = SimpleNamespace(scene={"robot": asset}, num_envs=num_envs) + # R_x(a)^T applied to -Z in the first environment and +Y in the second. + expected_z = [[0.0, -math.sin(a), -math.cos(a)] for a in angles] + expected_y = [[0.0, math.cos(a), -math.sin(a)] for a in angles] + expected = torch.tensor([expected_z, expected_y]).reshape(num_envs, -1) + torch.testing.assert_close(body_projected_gravity_b(env, SceneEntityCfg("robot")), expected) + for body_ids in ([1], 1): + asset_cfg = SceneEntityCfg("robot", body_ids=body_ids) + torch.testing.assert_close(body_projected_gravity_b(env, asset_cfg), expected[:, 3:6]) diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index 41e411793f9..b041cca8664 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -904,46 +904,41 @@ def test_matrix_from_euler(device, euler_angles, convention): @pytest.mark.parametrize("device", test_devices()) -def test_quat_apply(device): - """Test for quat_apply against scipy.""" - # prepare random quaternions and vectors - n = 1024 - q_rand = math_utils.random_orientation(num=n, device=device) - # Our quaternions are already in xyzw format, which scipy expects - Rotation = scipy_tf.Rotation.from_quat(q_rand.to(device="cpu").numpy()) - - v_rand = math_utils.sample_uniform(-1000, 1000, (n, 3), device=device) - - # compute the result using the new implementation - scipy_result = torch.tensor(Rotation.apply(v_rand.to(device="cpu").numpy()), device=device, dtype=torch.float) - apply_result = math_utils.quat_apply(q_rand, v_rand) - torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4) - # batched (..., 4) inputs keep their leading dimensions - batched_result = math_utils.quat_apply(q_rand.reshape(n // 8, 2, 4, 4), v_rand.reshape(n // 8, 2, 4, 3)) - torch.testing.assert_close(batched_result, scipy_result.reshape(n // 8, 2, 4, 3), atol=2e-4, rtol=2e-4) - - -@pytest.mark.parametrize("device", test_devices()) -def test_quat_apply_inverse(device): - """Test for quat_apply against scipy.""" - - # prepare random quaternions and vectors - n = 1024 - q_rand = math_utils.random_orientation(num=n, device=device) - # Our quaternions are already in xyzw format, which scipy expects - Rotation = scipy_tf.Rotation.from_quat(q_rand.to(device="cpu").numpy()) - - v_rand = math_utils.sample_uniform(-1000, 1000, (n, 3), device=device) - - # compute the result using the new implementation - scipy_result = torch.tensor( - Rotation.apply(v_rand.to(device="cpu").numpy(), inverse=True), device=device, dtype=torch.float - ) - apply_result = math_utils.quat_apply_inverse(q_rand, v_rand) - torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4) - # batched (..., 4) inputs keep their leading dimensions - batched_result = math_utils.quat_apply_inverse(q_rand.reshape(n // 8, 2, 4, 4), v_rand.reshape(n // 8, 2, 4, 3)) - torch.testing.assert_close(batched_result, scipy_result.reshape(n // 8, 2, 4, 3), atol=2e-4, rtol=2e-4) +@pytest.mark.parametrize("inverse", [False, True]) +@pytest.mark.parametrize( + "quat_shape,vec_shape", + [ + ((1024,), (1024,)), + ((128, 2, 4), (128, 2, 4)), + ((), ()), + ((1,), ()), + ((), (2, 3)), + ((2, 3), ()), + ((2, 3), (2, 1)), + ((2, 1), (3,)), + ((2, 3), (3, 2)), + ], +) +def test_quat_apply(device, inverse, quat_shape, vec_shape): + """Rotations follow NumPy broadcasting and agree with SciPy, including strided inputs.""" + quat = math_utils.random_orientation(num=2 * math.prod(quat_shape), device=device)[::2].reshape(*quat_shape, 4) + vec = math_utils.sample_uniform(-1000, 1000, (2 * math.prod(vec_shape), 3), device=device)[::2] + vec = vec.reshape(*vec_shape, 3) + apply = math_utils.quat_apply_inverse if inverse else math_utils.quat_apply + try: + shape = np.broadcast_shapes(quat_shape, vec_shape) + (3,) + except ValueError: + # Equal element counts do not make incompatible batch dimensions broadcastable. + with pytest.raises((RuntimeError, torch.jit.Error)): + apply(quat, vec) + return + + # Broadcast with NumPy before flattening for SciPy versions that only accept a single batch axis. + quat_np = np.broadcast_to(quat.cpu().numpy(), shape[:-1] + (4,)).reshape(-1, 4) + vec_np = np.broadcast_to(vec.cpu().numpy(), shape).reshape(-1, 3) + expected = scipy_tf.Rotation.from_quat(quat_np).apply(vec_np, inverse=inverse).reshape(shape) + result = apply(quat, vec) + torch.testing.assert_close(result, torch.tensor(expected, device=device, dtype=vec.dtype), atol=2e-4, rtol=2e-4) @pytest.mark.parametrize("device", test_devices()) From 04573e6c80594f85e90dae39d6c69013fde3146d Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Fri, 25 Sep 2026 05:31:42 -0700 Subject: [PATCH 6/7] [Tests] Prune Newton tests with the test-audit skill (#8008) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Newton test suite: 11:23 → 7:54 (−31%) The `isaaclab_newton` suite runs through `pytest tools` on the same machine with a warm Warp cache. All 38 files pass. | | `develop` | this PR | |---|---:|---:| | Suite wall time | 11:23 | **7:54** | | Test functions | 411 | 297 | | Test lines | 19,085 | 15,880 | ## What changed The whole test surface was pruned with the `isaaclab-auditing-tests` skill (#8007). **Method.** Every test was marked keep, fix, merge or delete. Whenever a deletion's keeper does not carry the assertions verbatim, a deliberate production mutation proves the keeper fails. An independent review then restored every gap it found. The only production change is `transform_to_vec_quat` raising its documented `ValueError`; it has a changelog entry. - **Merged:** tests that rebuilt the same scene now share one build: - frame transformer: 5 scenes → 1 - contact filter + stale-reset - IMU/PVA init + freefall - joint-wrench init/reset - limits: 14 builds → 4 - root/COM writes - dynamics accessors - rigid/collection write tests - **Deleted:** - manager class checks implied by the end-to-end solver test - `test_wrench_kernels.py` - private helper replays in `test_site_injection.py` - the joint-wrench oracle that repeated the production transform - the duplicate effort-limit test (the PhysX copy is kept) - **Fixed tests that could not fail:** - delayed/remotized PD equivalence, which passed with no delay authored - DC-motor clamp - IMU stale-data (#4970) - `set_coms_index` - joint targets, now checked by value instead of with a launch spy - **Restored:** four tests #8003 removed without a remaining proof (`TestDelayedPDAuthoring` and the material-property tests). - **Device axes:** device-independent bookkeeping runs on CUDA only. Every public writer family keeps a real-simulation CPU test. ## Product defects found (not fixed here) - **Newton `ArticulationView` misaddresses per-env shapes** for rigid-object collections. - **`RigidObject.body_link_vel_w` can read stale** after a COM velocity write. ## 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] My changes generate no new warnings - [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 --------- Signed-off-by: Octi Zhang Co-authored-by: Octi Zhang --- AGENTS.md | 2 + .../changelog.d/test-audit-newton-tests.rst | 5 + .../isaaclab/isaaclab/utils/warp/math_ops.py | 8 +- .../changelog.d/test-audit-newton-tests.skip | 1 + .../test/assets/test_articulation.py | 2515 +++++------------ .../test_articulation_ordering_kernels.py | 3 +- .../test/assets/test_joint_coordinates.py | 16 +- .../test/assets/test_mpm_object.py | 3 +- .../assets/test_newton_actuators_newton.py | 435 +-- .../test/assets/test_rigid_object.py | 861 ++---- .../assets/test_rigid_object_collection.py | 739 ++--- .../test/assets/test_wrench_kernels.py | 73 - .../cloner/test_collision_approximation.py | 41 +- .../test/cloner/test_visual_shape_import.py | 34 +- .../test/controllers/test_newton_ik_solver.py | 17 +- .../physics/test_mjwarp_tendon_control.py | 4 +- .../physics/test_mpm_reset_mask_contract.py | 11 +- .../physics/test_newton_fabric_body_sync.py | 64 +- .../test_newton_manager_abstraction.py | 230 +- .../test/physics/test_newton_solver_reset.py | 3 +- .../test/physics/test_vbd_core.py | 24 +- ...on_warp_renderer_rigid_object_rendering.py | 3 +- .../test/renderers/test_segmentation.py | 30 +- .../test/renderers/test_visual_material.py | 3 +- .../test_camera_opencv_distortion_newton.py | 72 +- .../test/sensors/test_contact_sensor.py | 422 +-- .../sensors/test_contact_sensor_history.py | 114 +- .../test/sensors/test_frame_transformer.py | 630 ++--- .../isaaclab_newton/test/sensors/test_imu.py | 152 +- .../test/sensors/test_joint_wrench_sensor.py | 294 +- .../sensors/test_newton_raycast_sensor.py | 62 +- .../isaaclab_newton/test/sensors/test_pva.py | 163 +- .../test/sensors/test_site_injection.py | 142 +- .../test/sim/test_cable_usd_import.py | 12 +- .../test/sim/test_mpm_visualization.py | 14 +- .../test/sim/test_newton_schemas.py | 165 +- .../test/sim/test_views_xform_prim_newton.py | 53 +- 37 files changed, 2032 insertions(+), 5388 deletions(-) create mode 100644 source/isaaclab/changelog.d/test-audit-newton-tests.rst create mode 100644 source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip delete mode 100644 source/isaaclab_newton/test/assets/test_wrench_kernels.py diff --git a/AGENTS.md b/AGENTS.md index 78a307848df..4474a56e436 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 Newton solver schema registration in the active manager's builder factory; the cloner must not depend on solver modules. +- Resolve Newton raycast BVH requirements before builder finalization; sensor task registration must not add a late BVH fallback. - 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. - Keep articulation ordering maps on articulation data; do not mirror maps or add cached ordering flags. diff --git a/source/isaaclab/changelog.d/test-audit-newton-tests.rst b/source/isaaclab/changelog.d/test-audit-newton-tests.rst new file mode 100644 index 00000000000..6686d1ea4a6 --- /dev/null +++ b/source/isaaclab/changelog.d/test-audit-newton-tests.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed :func:`~isaaclab.utils.warp.math_ops.transform_to_vec_quat` raising a Warp ``RuntimeError`` instead of its + documented ``ValueError`` for 4D transform arrays. diff --git a/source/isaaclab/isaaclab/utils/warp/math_ops.py b/source/isaaclab/isaaclab/utils/warp/math_ops.py index 36a0f960a8b..e23e0676f0e 100644 --- a/source/isaaclab/isaaclab/utils/warp/math_ops.py +++ b/source/isaaclab/isaaclab/utils/warp/math_ops.py @@ -21,14 +21,16 @@ def transform_to_vec_quat( Raises: TypeError: If *t* does not have dtype ``wp.transformf``. + ValueError: If *t* has more than 3 dimensions. """ if t.dtype != wp.transformf: raise TypeError(f"Expected wp.transformf array, got dtype={t.dtype}") + # Check before viewing: the float view adds a dimension, which Warp rejects for a 4D input. + if t.ndim > 3: + raise ValueError(f"Expected 1D, 2D, or 3D transform array, got ndim={t.ndim}") floats = t.view(wp.float32) if t.ndim == 1: return floats[:, :3].view(wp.vec3f), floats[:, 3:].view(wp.quatf) if t.ndim == 2: return floats[:, :, :3].view(wp.vec3f), floats[:, :, 3:].view(wp.quatf) - if t.ndim == 3: - return floats[:, :, :, :3].view(wp.vec3f), floats[:, :, :, 3:].view(wp.quatf) - raise ValueError(f"Expected 1D, 2D, or 3D transform array, got ndim={t.ndim}") + return floats[:, :, :, :3].view(wp.vec3f), floats[:, :, :, 3:].view(wp.quatf) diff --git a/source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip b/source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip new file mode 100644 index 00000000000..df6d36b6bed --- /dev/null +++ b/source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip @@ -0,0 +1 @@ +Restored Newton tests removed without a remaining proof and repaired Newton regression tests that could not fail. diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index d0731f63a22..834db2c6ef6 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -39,16 +39,14 @@ from isaaclab_newton.assets import Articulation from isaaclab_newton.assets.articulation.actuator_control import NewtonActuatorControl from isaaclab_newton.assets.articulation.articulation import _configure_builder_joint_target_modes -from isaaclab_newton.assets.articulation.articulation_data import ArticulationData from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics import NewtonManager as SimulationManager from isaaclab_physx.sim.schemas import PhysxJointCfg -from newton import JointTargetMode, JointType, ModelBuilder, ModelFlags +from newton import JointTargetMode, JointType, ModelBuilder, ModelFlags, ShapeFlags from newton.solvers import SolverMuJoCo from pxr import UsdPhysics -import isaaclab.assets.articulation.ordering_kernels as ordering_kernels import isaaclab.assets.articulation.ordering_resolvers as ordering_resolvers import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -61,18 +59,12 @@ from isaaclab.assets import ArticulationCfg, AssetBaseCfg from isaaclab.assets.articulation.ordering_resolvers import get_articulation_name_ordering from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate -from isaaclab.controllers import ( - DifferentialIKController, - DifferentialIKControllerCfg, - OperationalSpaceController, - OperationalSpaceControllerCfg, -) -from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit -from isaaclab.managers import SceneEntityCfg +from isaaclab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg +from isaaclab.envs.mdp.events import randomize_rigid_body_collider_offsets, randomize_rigid_body_material +from isaaclab.managers import EventTermCfg, SceneEntityCfg from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.math import compute_pose_error, matrix_from_quat, quat_inv, subtract_frame_transforms -from isaaclab.utils.warp.proxy_array import ProxyArray ## # Pre-defined configs @@ -442,7 +434,7 @@ def generate_articulation( # --------------------------------------------------------------------------- -# Franka task-space tracking helpers (shared between IK and OSC tests). +# Franka task-space tracking helpers (shared between the OSC tests). # --------------------------------------------------------------------------- @@ -521,7 +513,7 @@ def _compute_ee_vel_root(jacobian_b, joint_vel): velocity buffers can return stale/zero values until forced materialization, while ``joint_vel`` and ``J`` are already pulled by the loop. ``J`` correctness is pinned independently by - ``test_get_jacobians_link_origin_contract``. + ``test_get_gravity_compensation_forces_matches_jacobian_gravity``. """ return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) @@ -533,10 +525,10 @@ def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device): return torch.cat([target_pos_b, initial_ee_quat_b], dim=-1) -def _summarize_history(history, tail: int = 200): - """Return ``(min, mean)`` over the last ``tail`` samples.""" +def _tail_mean(history, tail: int = 200): + """Return the mean over the last ``tail`` samples.""" tail_slice = history[-tail:] - return min(tail_slice), sum(tail_slice) / len(tail_slice) + return sum(tail_slice) / len(tail_slice) @pytest.fixture @@ -600,55 +592,21 @@ def _make_target_mode_builder( return builder -def test_viscous_writer_updates_finalized_newton_model(monkeypatch): - """Test the production viscous writer updates a finalized Newton model binding.""" - builder = ModelBuilder() - link = builder.add_link(mass=1.0, inertia=wp.mat33(1.0)) - joint = builder.add_joint_revolute(-1, link, label="joint") - builder.add_articulation([joint], label="articulation") - model = builder.finalize(device="cpu") - model_damping = wp.array( - ptr=model.joint_damping.ptr, - dtype=wp.float32, - shape=(1, 1), - strides=(model.joint_damping.strides[0], model.joint_damping.strides[0]), - device="cpu", - copy=False, - ) - - data_type = type( - "_Data", - (), - {"joint_viscous_friction_coeff": ArticulationData.joint_viscous_friction_coeff}, - ) - data = data_type() - data.has_joint_ordering = False - data.joint_ordering = None - data._joint_viscous_friction_user = None - data._sim_bind_joint_viscous_friction_coeff = model_damping - data._joint_viscous_friction_coeff_ta = ProxyArray(model_damping) - - articulation = object.__new__(Articulation) - articulation._device = "cpu" - articulation._data = data - articulation._root_view = SimpleNamespace(count=1) - articulation._ALL_INDICES = wp.array([0], dtype=wp.int32, device="cpu") - articulation._ALL_JOINT_INDICES = wp.array([0], dtype=wp.int32, device="cpu") - articulation._initialize_handle = None - articulation._invalidate_initialize_handle = None - articulation._prim_deletion_handle = None - monkeypatch.setattr(SimulationManager, "add_model_change", lambda flags: None) - - articulation.write_joint_viscous_friction_coefficient_to_sim_index( - joint_viscous_friction_coeff=torch.tensor([[0.25]], dtype=torch.float32), - ) - - torch.testing.assert_close(data.joint_viscous_friction_coeff.torch, torch.tensor([[0.25]])) - torch.testing.assert_close(torch.from_numpy(model.joint_damping.numpy()), torch.tensor([0.25])) - +@pytest.mark.parametrize( + ("actuator_cfg", "expected_native_groups"), + [ + (ImplicitActuatorCfg(joint_names_expr=["joint"], stiffness=10.0, damping=1.0), set()), + (IdealPDActuatorCfg(joint_names_expr=["joint"], stiffness=None, damping=None), {"explicit"}), + ], + ids=["implicit", "explicit"], +) +def test_prepare_native_actuators_activates_only_explicit_groups(monkeypatch, actuator_cfg, expected_native_groups): + """Keep implicit-only articulations on the solver-drive path and leave solver gains untouched. -def test_prepare_native_actuators_does_not_zero_solver_gains(monkeypatch): - """Leave solver gains untouched until collection construction resolves actuator defaults.""" + Explicit groups activate the Newton-actuator path without writing gains; collection construction resolves + the actuator defaults later. + """ + activation_calls = [] gain_writes = [] articulation = SimpleNamespace( _sim_cfg=SimpleNamespace(use_newton_actuators=True), @@ -657,33 +615,20 @@ def test_prepare_native_actuators_does_not_zero_solver_gains(monkeypatch): write_joint_stiffness_to_sim_index=lambda **_: gain_writes.append("stiffness"), write_joint_damping_to_sim_index=lambda **_: gain_writes.append("damping"), ) - monkeypatch.setattr(SimulationManager, "activate_newton_actuator_path", lambda: None) - - native_groups = NewtonActuatorControl(articulation).prepare_native_actuators( - collection=None, - actuator_cfgs={"explicit": IdealPDActuatorCfg(joint_names_expr=["joint"], stiffness=None, damping=None)}, - ) - - assert native_groups == {"explicit"} - assert gain_writes == [] - - -def test_prepare_native_actuators_leaves_implicit_only_articulation_on_standard_path(monkeypatch): - """Keep implicit-only articulations on the unchanged solver-drive path.""" - activation_calls = [] - articulation = SimpleNamespace(_sim_cfg=SimpleNamespace(use_newton_actuators=True)) monkeypatch.setattr(SimulationManager, "activate_newton_actuator_path", lambda: activation_calls.append(True)) control = NewtonActuatorControl(articulation) - native_groups = control.prepare_native_actuators( - collection=None, - actuator_cfgs={"implicit": ImplicitActuatorCfg(joint_names_expr=["joint"], stiffness=10.0, damping=1.0)}, - ) + group_name = "explicit" if expected_native_groups else "implicit" + native_groups = control.prepare_native_actuators(collection=None, actuator_cfgs={group_name: actuator_cfg}) - assert native_groups == set() - assert not control.native_actuator_path_active - assert not articulation._has_newton_actuators - assert activation_calls == [] + assert native_groups == expected_native_groups + assert gain_writes == [] + if expected_native_groups: + assert activation_calls == [True] + else: + assert not control.native_actuator_path_active + assert not articulation._has_newton_actuators + assert activation_calls == [] @pytest.mark.parametrize( @@ -787,7 +732,7 @@ def test_actuator_cfg_leaves_excluded_joint_types_imported(joint_type): assert builder.joint_target_mode == [int(JointTargetMode.NONE)] -def test_actuator_cfg_keeps_imported_newton_target_mode_for_none_gain(): +def test_actuator_cfg_uses_imported_gain_for_none_stiffness(): """Retain the imported stiffness when an implicit actuator config leaves it unset.""" articulation_cfg = ArticulationCfg( prim_path="/World/Env_[^/]*/Robot", @@ -839,90 +784,7 @@ def test_actuator_cfg_aligns_partial_dictionary_gains_by_joint_name(stiffness, d assert builder.joint_target_mode == [int(mode) for mode in expected_modes] -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_write_joint_state_accepts_int64_selector(sim, device, gravity_enabled, articulation_type) -> None: - """Write joint state with int64 selectors.""" - articulation_cfg = generate_articulation_cfg(articulation_type="spatial_tendon_test_asset") - articulation, _ = generate_articulation(articulation_cfg, 2, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.num_joints >= 2 - - env_ids = torch.tensor([1, 0], dtype=torch.int64, device=device) - joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int64, device=device) - position = torch.tensor([[0.21, 0.11], [0.22, 0.12]], device=device) - velocity = torch.tensor([[1.21, 1.11], [1.22, 1.12]], device=device) - - expected_position = articulation.data.joint_pos.torch.clone() - expected_velocity = articulation.data.joint_vel.torch.clone() - - articulation.write_joint_state_to_sim_index( - position=position, velocity=velocity, env_ids=env_ids, joint_ids=joint_ids - ) - expected_position[env_ids[:, None], joint_ids[None, :]] = position - expected_velocity[env_ids[:, None], joint_ids[None, :]] = velocity - torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) - torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) - - -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) -def test_mjwarp_ordering_resolver_matches_newton_backend_names(sim, device, gravity_enabled, articulation_type): - """Compare the resolver's emulated MJWarp ordering against the live Newton backend view. - - The articulation below is already native to the Newton backend, so - :func:`~isaaclab.assets.get_articulation_name_ordering` with the ``"mjwarp"`` - convention takes the same-backend identity fast path and never exercises the - temporary Newton USD builder used for cross-backend discovery (the path a - PhysX-backed articulation would take). That fast path is checked below, - but it is not sufficient by itself: it would pass even if the emulation's - BFS/DFS traversal had silently diverged from the live backend. To close - that gap, this test also calls the private builder helper directly — - forcing the temporary-builder emulation to run — and compares its output - against the live backend view. A branching (non-single-joint) fixture is - required for this comparison to be meaningful, since BFS and DFS produce - the same order on a single-joint chain. - """ - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - sim_utils.create_prim("/World/Env_0", "Xform") - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_0/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - ) - clone_plan_from_env_0(CloneCfg(clone_template="/World/Env_{}"), (articulation_cfg,), 1, 0.0) - articulation = Articulation(articulation_cfg) - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - # Newton's native traversal is depth-first (see NewtonManager.instantiate_builder_from_stage), - # so the live backend view already reflects MJWarp order on this branching fixture. These - # values are the same ground truth isaaclab_physx's own - # test_branching_fixture_resolves_distinct_conventions asserts for expected_mjwarp_*_names. - assert tuple(articulation.backend_joint_names) == BRANCHING_MJWARP_JOINT_NAMES - assert tuple(articulation.backend_body_names) == BRANCHING_MJWARP_BODY_NAMES - - # Force the cross-backend emulation path (bypassing the same-backend identity fast path) and - # compare its independently rebuilt Newton view against the live backend view above. A - # BFS/DFS regression in the emulation would fail this even though the fixture is small. - emulated_names = ordering_resolvers._get_mjwarp_names_from_newton_usd_builder(articulation) - assert emulated_names is not None - assert emulated_names["joint"] == tuple(articulation.backend_joint_names) - assert emulated_names["body"] == tuple(articulation.backend_body_names) - - # The public resolver still returns live names without discovery for a same-backend request. - assert get_articulation_name_ordering(articulation, "mjwarp", kind="joint") == tuple( - articulation.backend_joint_names - ) - assert get_articulation_name_ordering(articulation, "mjwarp", kind="body") == tuple(articulation.backend_body_names) - - -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +@pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("gravity_enabled", [False]) @pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gravity_enabled, articulation_type): @@ -937,6 +799,9 @@ def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gr The branching fixture is shared between both backends; a copy lives in this package's test data directory so the two backends assert against the same ground-truth asset. + + The same articulation also checks the MJWarp-order emulation used for cross-backend discovery and that + selected inertial-property writes keep Newton's inverse arrays current under the body ordering. """ fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" sim_utils.create_prim("/World/Env_0", "Xform") @@ -958,6 +823,18 @@ def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gr assert tuple(articulation.backend_joint_names) == BRANCHING_MJWARP_JOINT_NAMES assert tuple(articulation.backend_body_names) == BRANCHING_MJWARP_BODY_NAMES + # The same-backend "mjwarp" request takes an identity fast path, so also force the cross-backend + # emulation (the temporary Newton USD builder a PhysX-backed articulation would use) and compare its + # independently rebuilt view against the live backend view. BFS and DFS differ on this branching fixture. + emulated_names = ordering_resolvers._get_mjwarp_names_from_newton_usd_builder(articulation) + assert emulated_names is not None + assert emulated_names["joint"] == tuple(articulation.backend_joint_names) + assert emulated_names["body"] == tuple(articulation.backend_body_names) + assert get_articulation_name_ordering(articulation, "mjwarp", kind="joint") == tuple( + articulation.backend_joint_names + ) + assert get_articulation_name_ordering(articulation, "mjwarp", kind="body") == tuple(articulation.backend_body_names) + # Cross-backend discovery (bypassing the same-backend fast path) resolves the breadth-first PhysX order. assert get_articulation_name_ordering(articulation, "physx", kind="joint") == BRANCHING_PHYSX_JOINT_NAMES assert get_articulation_name_ordering(articulation, "physx", kind="body") == BRANCHING_PHYSX_BODY_NAMES @@ -968,6 +845,27 @@ def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gr assert articulation.joint_ordering is not None assert articulation.body_ordering is not None + # Selected mass and inertia writes with int64 selectors update Newton's inverse arrays in backend order. + env_ids = torch.tensor([0], dtype=torch.int64, device=device) + body_ids = torch.tensor([2, articulation.num_bodies - 1], dtype=torch.int64, device=device) + backend_body_ids = torch.tensor( + [articulation.data.body_ordering.user_to_backend_indices[index] for index in body_ids.tolist()], + dtype=torch.int64, + device=device, + ) + assert backend_body_ids[0] != body_ids[0] + model = SimulationManager.get_model() + + masses = articulation.data.body_mass.torch[env_ids][:, body_ids].clone() + torch.tensor([[1.0, 2.0]], device=device) + articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + model_inv_mass = wp.to_torch(articulation.root_view.get_attribute("body_inv_mass", model)[:, 0]) + torch.testing.assert_close(model_inv_mass[env_ids][:, backend_body_ids], masses.reciprocal()) + + inertia_matrices = torch.diag_embed(torch.tensor([[[2.0, 3.0, 4.0], [5.0, 6.0, 7.0]]], device=device)) + articulation.set_inertias_index(inertias=inertia_matrices.reshape(1, 2, 9), env_ids=env_ids, body_ids=body_ids) + model_inv_inertia = wp.to_torch(articulation.root_view.get_attribute("body_inv_inertia", model)[:, 0]) + torch.testing.assert_close(model_inv_inertia[env_ids][:, backend_body_ids], torch.linalg.inv(inertia_matrices)) + def test_num_shapes_per_body_follows_public_body_order() -> None: """Align Newton shape counts with the public body-name axis.""" @@ -1449,12 +1347,21 @@ def _other_callback() -> None: @pytest.mark.parametrize("articulation_type", ["anymal"]) @pytest.mark.parametrize("use_newton_actuators", [False, True]) @pytest.mark.parametrize("ordering_mode", ["none", "reversed"]) -def test_write_data_to_sim_gathers_joint_targets_only_when_ordering_active( - sim, num_articulations, device, gravity_enabled, articulation_type, use_newton_actuators, ordering_mode, monkeypatch +def test_write_data_to_sim_writes_joint_targets_in_backend_order( + sim, num_articulations, device, gravity_enabled, articulation_type, use_newton_actuators, ordering_mode ): - """Gather joint targets only when non-identity joint ordering is active.""" + """Write the published joint position targets into the backend buffer in backend joint order. + + Explicit actuators on the Newton-actuator path publish their raw targets; implicit actuators on the Lab + path publish the processed targets. Both must reach the solver-bound buffer permuted to backend order. + """ + actuator_cfg = ( + IdealPDActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0) + if use_newton_actuators + else ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0) + ) articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - actuators={"legs": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0)}, + actuators={"legs": actuator_cfg}, ) if ordering_mode == "reversed": articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES))) @@ -1462,141 +1369,57 @@ def test_write_data_to_sim_gathers_joint_targets_only_when_ordering_active( replicate(sim.get_clone_plan()) sim.reset() assert articulation.is_initialized + assert (articulation.data.joint_ordering is not None) is (ordering_mode == "reversed") + assert articulation._has_newton_actuators is use_newton_actuators - has_ordering = ordering_mode == "reversed" - assert (articulation.data.joint_ordering is not None) is has_ordering - on_newton_path = getattr(articulation, "_has_newton_actuators", False) - if use_newton_actuators and not on_newton_path: - pytest.skip("newton.actuators unavailable; the Newton-actuator branch is not exercised") - - # Drive an in-limits position target so the write path has data to forward. - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - - # Record every kernel launched during write_data_to_sim, then delegate to the - # real launch so the sim-bound buffers are still written. - launched_kernels: list = [] - real_launch = wp.launch - - def recording_launch(kernel, *args, **kwargs): - launched_kernels.append(kernel) - return real_launch(kernel, *args, **kwargs) - - monkeypatch.setattr(wp, "launch", recording_launch) + # Distinct per-joint targets away from the defaults, so a skipped or unpermuted write is visible. + target = articulation.data.default_joint_pos.torch.clone() + target += 0.01 * torch.arange(1, articulation.num_joints + 1, device=device) + articulation.set_joint_position_target_index(target=target) articulation.write_data_to_sim() - monkeypatch.undo() - - target_gather = ordering_kernels.reorder_joint_targets_user_to_backend - if has_ordering: - assert target_gather in launched_kernels - else: - # Identity ordering binds the user-order source directly. - assert target_gather not in launched_kernels - expected_source = ( - articulation.actuators.target_command.position.warp - if on_newton_path - else articulation.actuators.output_command.position.warp - ) - np.testing.assert_allclose(articulation.data._sim_bind_joint_position_target.numpy(), expected_source.numpy()) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) -@pytest.mark.parametrize("index_dtype", [torch.int32, torch.int64]) -def test_set_body_inertial_properties_updates_inverses( - sim, device, gravity_enabled, articulation_type, index_dtype, monkeypatch -): - """Selected inertial-property writes keep Newton inverse arrays current under body ordering.""" - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - sim_utils.create_prim("/World/Env_0", "Xform") - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_0/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - body_ordering="physx", - ) - clone_plan_from_env_0(CloneCfg(clone_template="/World/Env_{}"), (articulation_cfg,), 1, 0.0) - articulation = Articulation(articulation_cfg) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.data.body_ordering is not None - env_ids = torch.tensor([0], dtype=index_dtype, device=device) - body_ids = torch.tensor([2, articulation.num_bodies - 1], dtype=index_dtype, device=device) - backend_body_ids = torch.tensor( - [articulation.data.body_ordering.user_to_backend_indices[index] for index in body_ids.tolist()], - dtype=torch.int64, - device=device, + source = ( + articulation.actuators.target_command.position.torch + if use_newton_actuators + else articulation.actuators.output_command.position.torch ) - assert backend_body_ids[0] != body_ids[0] - - launches = [] - real_launch = wp.launch - - def recording_launch(kernel, *args, **kwargs): - launches.append(kernel) - return real_launch(kernel, *args, **kwargs) - - monkeypatch.setattr(wp, "launch", recording_launch) - masses = articulation.data.body_mass.torch[env_ids][:, body_ids].clone() + torch.tensor([[1.0, 2.0]], device=device) - articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) - assert len(launches) == 1 - - raw_model_inv_mass = articulation.root_view.get_attribute("body_inv_mass", SimulationManager.get_model())[:, 0] - assert articulation.data._sim_bind_body_inv_mass.ptr == raw_model_inv_mass.ptr - model_inv_mass = wp.to_torch(articulation.data._sim_bind_body_inv_mass) - torch.testing.assert_close(model_inv_mass[env_ids][:, backend_body_ids], masses.reciprocal()) - - inertia_matrices = torch.diag_embed(torch.tensor([[[2.0, 3.0, 4.0], [5.0, 6.0, 7.0]]], device=device)) - inertias = inertia_matrices.reshape(1, 2, 9) - launches.clear() - articulation.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) - assert len(launches) == 1 - - raw_model_inv_inertia = articulation.root_view.get_attribute("body_inv_inertia", SimulationManager.get_model())[ - :, 0 - ] - assert articulation.data._sim_bind_body_inv_inertia.ptr == raw_model_inv_inertia.ptr - model_inv_inertia = wp.to_torch(articulation.data._sim_bind_body_inv_inertia) - torch.testing.assert_close( - model_inv_inertia[env_ids][:, backend_body_ids], - torch.linalg.inv(inertia_matrices), + torch.testing.assert_close(source, target) + user_to_backend = ( + list(articulation.joint_ordering.user_to_backend_indices) + if articulation.joint_ordering is not None + else list(range(articulation.num_joints)) ) + expected_backend_target = torch.empty_like(source) + expected_backend_target[:, user_to_backend] = source + torch.testing.assert_close(wp.to_torch(articulation.data._sim_bind_joint_position_target), expected_backend_target) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("num_articulations", [3]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_initialization_floating_base_non_root(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test initialization for a floating-base with articulation root on a rigid body. +def test_gravity_vec_w_tracks_model_gravity(sim, num_articulations, device, add_ground_plane, articulation_type): + """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is not fixed base - 3. All buffers have correct shapes - 4. The articulation can be simulated + Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's + gravity broadcast to every env, hiding per-env gravity randomization (e.g. + :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on + The humanoid's articulation root sits on a rigid body, so this also checks floating-base initialization. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation( - articulation_cfg, num_articulations, device=sim.device, add_ground_plane=True - ) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) # Check that the framework doesn't hold excessive strong references. assert sys.getrefcount(articulation) < 10 - # Play sim replicate(sim.get_clone_plan()) sim.reset() # Check if articulation is initialized assert articulation.is_initialized - # Check that is fixed base + # Check that is floating base assert not articulation.is_fixed_base # Check buffers that exists and have correct shapes assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) @@ -1608,31 +1431,6 @@ def test_initialization_floating_base_non_root(sim, num_articulations, device, a is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_articulations", [2, 3]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_gravity_vec_w_tracks_model_gravity(sim, num_articulations, device, add_ground_plane, articulation_type): - """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - - Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's - gravity broadcast to every env, hiding per-env gravity randomization (e.g. - :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - replicate(sim.get_clone_plan()) - sim.reset() - # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. model = SimulationManager.get_model() model_gravity_arr = model.gravity[: model.world_count] @@ -1662,68 +1460,22 @@ def test_gravity_vec_w_tracks_model_gravity(sim, num_articulations, device, add_ torch.testing.assert_close(articulation.data.projected_gravity_b.torch, expected, atol=1e-5, rtol=1e-5) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_initialization_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test initialization for a floating-base with articulation root on provided prim path. - - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is not fixed base - 3. All buffers have correct shapes - 4. The articulation can be simulated - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # Check that floating base - assert not articulation.is_fixed_base - # Check buffers that exists and have correct shapes - assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 12) - assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) - assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) - - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("articulation_type", ["panda"]) -def test_initialization_fixed_base(sim, num_articulations, device, articulation_type): - """Test initialization for fixed base. +def test_fixed_base_reports_body_velocities(sim, num_articulations, device, articulation_type): + """Test that fixed-base articulations report live body velocities while their joints move. + + Regression test: the fixed-base fallback in ``_create_buffers`` zeroed the body + center-of-mass velocity binding together with the (genuinely unavailable) root velocity, + so :attr:`body_lin_vel_w` and :attr:`body_ang_vel_w` read zeros for every fixed-base + robot regardless of motion. This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is fixed base - 3. All buffers have correct shapes - 4. The articulation maintains its default state + 1. The articulation is initialized as fixed base with correctly shaped buffers + 2. Commanding a joint-space motion moves the bodies (finite difference of positions) while the root holds + its default state + 3. The reported body velocities track the finite-difference ground truth Args: sim: The simulation fixture @@ -1755,50 +1507,10 @@ def test_initialization_fixed_base(sim, num_articulations, device, articulation_ is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_pose = articulation.data.default_root_pose.torch.clone() - default_root_vel = articulation.data.default_root_vel.torch.clone() - default_root_pose[:, :3] = default_root_pose[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_fixed_base_reports_body_velocities(sim, num_articulations, device, articulation_type): - """Test that fixed-base articulations report live body velocities while their joints move. - - Regression test: the fixed-base fallback in ``_create_buffers`` zeroed the body - center-of-mass velocity binding together with the (genuinely unavailable) root velocity, - so :attr:`body_lin_vel_w` and :attr:`body_ang_vel_w` read zeros for every fixed-base - robot regardless of motion. - - This test verifies that: - 1. The articulation is fixed base - 2. Commanding a joint-space motion moves the bodies (finite difference of positions) - 3. The reported body velocities track the finite-difference ground truth - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_fixed_base + # the root holds its default state as it is fixed base + default_root_pose = articulation.data.default_root_pose.torch.clone() + default_root_pose[:, :3] = default_root_pose[:, :3] + translations + default_root_vel = articulation.data.default_root_vel.torch.clone() # command a step away from the default pose so the distal bodies move joint_pos_target = articulation.data.default_joint_pos.torch.clone() @@ -1811,6 +1523,8 @@ def test_fixed_base_reports_body_velocities(sim, num_articulations, device, arti articulation.write_data_to_sim() sim.step() articulation.update(sim.cfg.dt) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) + torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) body_pos = articulation.data.body_link_pos_w.torch reported_max.append(articulation.data.body_lin_vel_w.torch.norm(dim=-1).amax()) fin_diff_max.append(((body_pos - prev_body_pos) / sim.cfg.dt).norm(dim=-1).amax()) @@ -1824,67 +1538,6 @@ def test_fixed_base_reports_body_velocities(sim, num_articulations, device, arti assert reported_max > 0.5 * fin_diff_max -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) -def test_initialization_fixed_base_single_joint(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test initialization for fixed base articulation with a single joint. - - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is fixed base - 3. All buffers have correct shapes - 4. The articulation maintains its default state - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, translations = generate_articulation( - articulation_cfg, num_articulations, device=device, add_ground_plane=True - ) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # Check that fixed base - assert articulation.is_fixed_base - # Check buffers that exists and have correct shapes - assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 1) - assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) - assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) - - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_pose = articulation.data.default_root_pose.torch.clone() - default_root_vel = articulation.data.default_root_vel.torch.clone() - default_root_pose[:, :3] = default_root_pose[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("articulation_type", ["shadow_hand"]) @@ -1979,11 +1632,16 @@ def test_fixed_tendon_properties_reach_solver(sim, num_articulations, device, ar @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["anymal"]) def test_fragment_fix_root_link_uses_base_manager(sim, device, add_ground_plane, articulation_type): - """Newton consumes the base manager's world joint without relocating the root API.""" + """Newton consumes the base manager's world joint without relocating the root API. + + The floating-base ANYmal made fixed-base must then hold its root at the default state. + """ articulation_cfg = deepcopy(generate_articulation_cfg(articulation_type=articulation_type)) articulation_cfg.spawn.articulation_props = [] articulation_cfg.spawn.fix_root_link = True - articulation, _ = generate_articulation(articulation_cfg, num_articulations=1, device=device, add_ground_plane=True) + articulation, translations = generate_articulation( + articulation_cfg, num_articulations=1, device=device, add_ground_plane=True + ) root = sim_utils.get_first_matching_child_prim( "/World/Env_0/Robot", @@ -1997,54 +1655,10 @@ def test_fragment_fix_root_link_uses_base_manager(sim, device, add_ground_plane, sim.reset() assert articulation.is_initialized assert articulation.is_fixed_base + assert articulation.data.joint_pos.torch.shape == (1, 12) - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_initialization_floating_base_made_fixed_base( - sim, num_articulations, device, add_ground_plane, articulation_type -): - """Test initialization for a floating-base articulation made fixed-base using schema properties. - - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is fixed base after modification - 3. All buffers have correct shapes - 4. The articulation maintains its default state - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).copy() - # Fix root link by making it kinematic - articulation_cfg.spawn.fix_root_link = True - articulation, translations = generate_articulation( - articulation_cfg, num_articulations, device=device, add_ground_plane=True - ) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # Check that is fixed base - assert articulation.is_fixed_base - # Check buffers that exists and have correct shapes - assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 12) - - # Simulate physics for _ in range(10): - # perform rendering sim.step() - # update articulation articulation.update(sim.cfg.dt) # check that the root is at the correct state - its default state as it is fixed base @@ -2057,7 +1671,7 @@ def test_initialization_floating_base_made_fixed_base( @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["panda"]) def test_initialization_fixed_base_made_floating_base( @@ -2069,7 +1683,6 @@ def test_initialization_fixed_base_made_floating_base( 1. The articulation is properly initialized 2. The articulation is floating base after modification 3. All buffers have correct shapes - 4. The articulation can be simulated Args: sim: The simulation fixture @@ -2097,13 +1710,6 @@ def test_initialization_fixed_base_made_floating_base( assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) assert articulation.data.joint_pos.torch.shape == (num_articulations, 9) - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - @pytest.mark.parametrize("device", ["cpu"]) @pytest.mark.parametrize("articulation_type", ["panda"]) @@ -2120,133 +1726,23 @@ def test_out_of_range_default_joint_state(sim, device, articulation_type, state_ # Check that the framework doesn't hold excessive strong references. assert sys.getrefcount(articulation) < 10 + quantity = "positions" if state_field == "joint_pos" else "velocities" replicate(sim.get_clone_plan()) - with pytest.raises(ValueError): + with pytest.raises(ValueError, match=f"default {quantity} out of the limits"): + replicate(sim.get_clone_plan()) sim.reset() -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test write_joint_limits_to_sim API and when default pos falls outside of the new limits. - - This test verifies that: - 1. Joint limits can be set correctly - 2. Default positions are preserved when setting new limits - 3. Joint limits can be set with indexing - 4. Invalid joint positions are properly handled - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device, add_ground_plane=True) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - - # Get current default joint pos - default_joint_pos = articulation._data.default_joint_pos.torch.clone() - - # Set new joint limits - limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) - limits[..., 0] = (torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0) * -1.0 - limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 - articulation.write_joint_position_limit_to_sim_index(limits=limits) - - # Check new limits are in place - torch.testing.assert_close(articulation._data.joint_pos_limits.torch, limits) - torch.testing.assert_close(articulation._data.default_joint_pos.torch, default_joint_pos) - - # Set new joint limits with indexing - env_ids = torch.arange(1, device=device, dtype=torch.int32) - joint_ids = torch.arange(2, device=device, dtype=torch.int32) - limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) - limits[..., 0] = (torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0) * -1.0 - limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0 - articulation.write_joint_position_limit_to_sim_index(limits=limits, env_ids=env_ids, joint_ids=joint_ids) - - # Check new limits are in place - torch.testing.assert_close(articulation._data.joint_pos_limits.torch[env_ids][:, joint_ids], limits) - torch.testing.assert_close(articulation._data.default_joint_pos.torch, default_joint_pos) - - # Set new joint limits that invalidate default joint pos - limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) - limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1 - limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 - articulation.write_joint_position_limit_to_sim_index(limits=limits) - - # Check if all values are within the bounds - default_joint_pos_torch = articulation._data.default_joint_pos.torch - within_bounds = (default_joint_pos_torch >= limits[..., 0]) & (default_joint_pos_torch <= limits[..., 1]) - assert torch.all(within_bounds) - - # Set new joint limits that invalidate default joint pos with indexing - limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) - limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 - limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 - articulation.write_joint_position_limit_to_sim_index(limits=limits, env_ids=env_ids, joint_ids=joint_ids) - - # Check if all values are within the bounds - default_joint_pos_torch = articulation._data.default_joint_pos.torch - within_bounds = (default_joint_pos_torch[env_ids][:, joint_ids] >= limits[..., 0]) & ( - default_joint_pos_torch[env_ids][:, joint_ids] <= limits[..., 1] - ) - assert torch.all(within_bounds) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_joint_effort_limits(sim, num_articulations, device, add_ground_plane, articulation_type): - """Validate joint effort limits via joint_effort_out_of_limit().""" - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device, add_ground_plane=True) - - # Minimal env wrapper exposing scene["robot"] - class _Env: - def __init__(self, art): - self.scene = {"robot": art} - - env = _Env(articulation) - robot_all = SceneEntityCfg(name="robot") - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - # Case A: no clipping → should NOT terminate - articulation._data.computed_torque.torch.zero_() - articulation._data.applied_torque.torch.zero_() - out = joint_effort_out_of_limit(env, robot_all) # [N] - assert torch.all(~out) - - # Case B: simulate clipping → should terminate - articulation._data.computed_torque.torch.fill_(100.0) # pretend controller commanded 100 - articulation._data.applied_torque.torch.fill_(50.0) # pretend actuator clipped to 50 - out = joint_effort_out_of_limit(env, robot_all) # [N] - assert torch.all(out) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_buffer(sim, num_articulations, device, articulation_type): - """Test if external force buffer correctly updates in the force value is zero case. +def test_external_force_on_multiple_bodies(sim, num_articulations, device, articulation_type): + """Test application of external force on the legs of the articulation. This test verifies that: - 1. External forces can be applied correctly - 2. Force buffers are updated properly - 3. Zero forces are handled correctly + 1. External forces can be applied to multiple bodies + 2. The forces affect the articulation's motion correctly + 3. The articulation responds to the forces as expected Args: sim: The simulation fixture @@ -2255,84 +1751,49 @@ def test_external_force_buffer(sim, num_articulations, device, articulation_type articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # play the simulator + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() - # find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") - - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) + # Find bodies to apply the force + body_ids, _ = articulation.find_bodies(".*_SHANK") + # Sample a large force + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_b[..., 1] = 200.0 # reset articulation articulation.reset() - + # apply force + articulation.permanent_wrench_composer.set_forces_and_torques_index( + forces=external_wrench_b[..., :3], torques=external_wrench_b[..., 3:], body_ids=body_ids + ) # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - - # check if the articulation's force and torque buffers are correctly updated - for i in range(num_articulations): - assert articulation.permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force - assert articulation.permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force - - # Check if the instantaneous wrench is correctly added to the permanent wrench - articulation.instantaneous_wrench_composer.add_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - + for _ in range(100): # apply action to the articulation articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) articulation.write_data_to_sim() - # perform step sim.step() - # update buffers articulation.update(sim.cfg.dt) + # check condition + for i in range(num_articulations): + # since there is a moment applied on the articulation, the articulation should rotate + assert articulation.data.root_ang_vel_w.torch[i, 2].item() > 0.1 @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_single_body(sim, num_articulations, device, articulation_type): - """Test application of external force on the base of the articulation. +def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device, articulation_type): + """Test application of external force on the legs of the articulation at a given position. This test verifies that: - 1. External forces can be applied to specific bodies - 2. The forces affect the articulation's motion correctly - 3. The articulation responds to the forces as expected + 1. External forces can be applied to multiple bodies at a given position + 2. External forces can be applied to multiple bodies in the global frame + 3. External forces are calculated and composed correctly + 4. The forces affect the articulation's motion correctly + 5. The articulation responds to the forces as expected Args: sim: The simulation fixture @@ -2340,241 +1801,18 @@ def test_external_force_on_single_body(sim, num_articulations, device, articulat """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() # Find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") + body_ids, _ = articulation.find_bodies(".*_SHANK") # Sample a large force external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 1] = 100.0 - - # Now we are ready! - for _ in range(2): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset articulation - articulation.reset() - # apply force - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], torques=external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition that the articulations have fallen down - for i in range(num_articulations): - assert articulation.data.root_pos_w.torch[i, 2].item() < 0.2 - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_single_body_at_position(sim, num_articulations, device, articulation_type): - """Test application of external force on the base of the articulation at a given position. - - This test verifies that: - 1. External forces can be applied to specific bodies at a given position - 2. External forces can be applied to specific bodies in the global frame - 3. External forces are calculated and composed correctly - 4. The forces affect the articulation's motion correctly - 5. The articulation responds to the forces as expected - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 2] = 100.0 - external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - external_wrench_positions_b[..., 1] = 1.0 - - desired_force = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_force[..., 2] = 200.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 200.0 - - # Now we are ready! - for i in range(2): - # reset root state - root_pose = articulation.data.default_root_pose.torch.clone() - root_pose[0, 0] = 2.5 # space them apart by 2.5m - - 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 = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset articulation - articulation.reset() - # apply force - is_global = False - - if i % 2 == 0: - body_com_pos_w = articulation.data.body_com_pos_w.torch[:, body_ids, :3] - # is_global = True - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - external_wrench_positions_b += body_com_pos_w - else: - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=external_wrench_positions_b, - body_ids=body_ids, - is_global=is_global, - ) - articulation.permanent_wrench_composer.add_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=external_wrench_positions_b, - body_ids=body_ids, - is_global=is_global, - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition that the articulations have fallen down - for i in range(num_articulations): - assert articulation.data.root_pos_w.torch[i, 2].item() < 0.2 - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_multiple_bodies(sim, num_articulations, device, articulation_type): - """Test application of external force on the legs of the articulation. - - This test verifies that: - 1. External forces can be applied to multiple bodies - 2. The forces affect the articulation's motion correctly - 3. The articulation responds to the forces as expected - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies(".*_SHANK") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 1] = 200.0 - - # Now we are ready! - for _ in range(2): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset articulation - articulation.reset() - # apply force - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], torques=external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition - for i in range(num_articulations): - # since there is a moment applied on the articulation, the articulation should rotate - assert articulation.data.root_ang_vel_w.torch[i, 2].item() > 0.1 - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device, articulation_type): - """Test application of external force on the legs of the articulation at a given position. - - This test verifies that: - 1. External forces can be applied to multiple bodies at a given position - 2. External forces can be applied to multiple bodies in the global frame - 3. External forces are calculated and composed correctly - 4. The forces affect the articulation's motion correctly - 5. The articulation responds to the forces as expected - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies(".*_SHANK") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 2] = 50.0 - external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - external_wrench_positions_b[..., 1] = 1.0 - - desired_force = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_force[..., 2] = 200.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 200.0 + external_wrench_b[..., 2] = 50.0 + external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + external_wrench_positions_b[..., 1] = 1.0 # Now we are ready! for i in range(2): @@ -2635,7 +1873,7 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["humanoid"]) def test_loading_gains_from_usd(sim, num_articulations, device, articulation_type): """Test that gains are loaded from USD file if actuator model has them as None. @@ -2697,70 +1935,41 @@ def test_loading_gains_from_usd(sim, num_articulations, device, articulation_typ torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that gains are loaded from the configuration correctly. - - This test verifies that: - 1. Gains are loaded correctly from configuration - 2. The gains match the expected values - 3. The gains are applied correctly to the actuators - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device, add_ground_plane=True - ) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Expected gains - expected_stiffness = torch.full( - (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device - ) - expected_damping = torch.full_like(expected_stiffness, 2.0) - - # Check that gains are loaded from USD file - torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) - torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_velocity_limit", [1e5, None]) -@pytest.mark.parametrize("vel_limit", [1e2, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) # consumed by the sim fixture -def test_setting_velocity_limit_implicit( - sim, articulation_type, num_articulations, device, joint_velocity_limit, vel_limit +@pytest.mark.parametrize( + ("joint_limit", "actuator_limit"), + [(1e5, None), (None, 1e2), (1e5, 1e2)], + ids=["joint_limit", "actuator_limit", "both_limits"], +) +@pytest.mark.parametrize("articulation_type", ["single_joint_implicit", "single_joint_explicit"]) +@pytest.mark.parametrize("use_newton_actuators", [False]) # consumed by the sim fixture +def test_setting_joint_limits_from_cfg( + sim, articulation_type, num_articulations, device, joint_limit, actuator_limit, use_newton_actuators ): - """Test setting of velocity limit for implicit actuators. + """Test the velocity and effort limit resolution for implicit and explicit actuators. This test verifies that: - 1. The solver clamp ``joint_velocity_limit`` is applied to the simulation; when unset, the - USD-authored value is kept - 2. The actuator velocity limit ``actuator_velocity_limit`` is never pushed to the solver and keeps its - configured value; when unset, it falls back to the solver clamp + 1. The solver clamps ``joint_velocity_limit`` and ``joint_effort_limit`` are applied to the simulation; + when unset, the USD-authored values are kept + 2. The actuator limits keep their configured values and are never pushed to the solver + 3. When unset, the actuator velocity limit falls back to the solver clamp, implicit actuators track the + solver effort clamp, and an explicit actuator's effort limit falls back to the USD-authored value Args: sim: The simulation fixture + articulation_type: The implicit or explicit single-joint articulation num_articulations: Number of articulations to test device: The device to run the simulation on - joint_velocity_limit: The velocity limit to set in simulation - vel_limit: The velocity limit to set in actuator + joint_limit: The velocity and effort limits to set in simulation + actuator_limit: The velocity and effort limits to set in the actuator """ - # create simulation articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=vel_limit, + articulation_type=articulation_type, + joint_velocity_limit=joint_limit, + joint_effort_limit=joint_limit, + actuator_velocity_limit=actuator_limit, + actuator_effort_limit=actuator_limit, ) articulation, _ = generate_articulation( articulation_cfg=articulation_cfg, @@ -2772,208 +1981,56 @@ def test_setting_velocity_limit_implicit( sim.reset() # read the values set into the simulation - newton_vel_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_velocity_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - # check data buffer - torch.testing.assert_close(articulation.data.joint_vel_limits.torch, newton_vel_limit) - # the solver clamp comes from joint_velocity_limit when set, otherwise the USD-authored value - if joint_velocity_limit is None: - sim_limit = next( - p.max_joint_velocity for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, PhysxJointCfg) - ) - else: - sim_limit = joint_velocity_limit - expected_velocity_limit = torch.full_like(newton_vel_limit, sim_limit) - torch.testing.assert_close(newton_vel_limit, expected_velocity_limit) - - # the joint velocity limit keeps its configured value and is not pushed to the solver; - # when unset it falls back to the solver clamp - joint_limit = vel_limit if vel_limit is not None else sim_limit - expected_joint_limit = torch.full_like(newton_vel_limit, joint_limit) - torch.testing.assert_close(articulation.actuators["joint"].actuator_velocity_limit, expected_joint_limit) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_velocity_limit", [1e5, None]) -@pytest.mark.parametrize("vel_limit", [1e2, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) # consumed by the sim fixture -@pytest.mark.parametrize("use_newton_actuators", [False]) -def test_setting_velocity_limit_explicit( - sim, articulation_type, num_articulations, device, joint_velocity_limit, vel_limit, use_newton_actuators -): - """Test setting of velocity limit for explicit actuators.""" - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_explicit", - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=vel_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # collect limit init values - newton_vel_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_velocity_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - actuator_vel_limit = articulation.actuators["joint"].actuator_velocity_limit - - # check data buffer for joint_vel_limits - torch.testing.assert_close(articulation.data.joint_vel_limits.torch, newton_vel_limit) - - if vel_limit is not None: - expected_actuator_vel_limit = torch.full( - (articulation.num_instances, articulation.num_joints), - vel_limit, - device=articulation.device, - ) - # check actuator is set - torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit) - # check physx is not actuator_velocity_limit - assert not torch.allclose(actuator_vel_limit, newton_vel_limit) - else: - # check actuator_velocity_limit is the same as the PhysX default - torch.testing.assert_close(actuator_vel_limit, newton_vel_limit) - - # simulation velocity limit is set to USD value unless user overrides - if joint_velocity_limit is not None: - limit = joint_velocity_limit - else: - limit = next( - p.max_joint_velocity for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, PhysxJointCfg) - ) - # check physx is set to expected value - expected_vel_limit = torch.full_like(newton_vel_limit, limit) - torch.testing.assert_close(newton_vel_limit, expected_vel_limit) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_effort_limit", [1e5, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) # consumed by the sim fixture -def test_setting_effort_limit_implicit(sim, articulation_type, num_articulations, device, joint_effort_limit): - """Test setting of effort limit for implicit actuators. - - This test verifies the effort limit resolution logic for actuator models implemented in :class:`ActuatorBase`: - - Case 1: If USD value == actuator config value: values match correctly - - Case 2: If USD value != actuator config value: actuator config value is used - - Case 3: If actuator config value is None: USD value is used as default - """ - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - joint_effort_limit=joint_effort_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # obtain the physx effort limits - newton_effort_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_effort_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - - torch.testing.assert_close(articulation.data.joint_effort_limits.torch, newton_effort_limit) - torch.testing.assert_close(articulation.actuators["joint"].joint_effort_limit, newton_effort_limit) - # without a separately configured rated limit, the actuator limit tracks the solver clamp - torch.testing.assert_close(articulation.actuators["joint"].actuator_effort_limit, newton_effort_limit) - - # decide the limit based on what is set - if joint_effort_limit is None: - limit = next( - p.max_force for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, sim_utils.UsdPhysicsDriveCfg) - ) - else: - limit = joint_effort_limit - - # check that the max force is what we set - expected_effort_limit = torch.full_like(newton_effort_limit, limit) - torch.testing.assert_close(newton_effort_limit, expected_effort_limit) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_effort_limit", [1e5, None]) -@pytest.mark.parametrize("actuator_effort_limit", [1e2, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) # consumed by the sim fixture -@pytest.mark.parametrize("use_newton_actuators", [False]) -def test_setting_effort_limit_explicit( - sim, - articulation_type, - num_articulations, - device, - joint_effort_limit, - actuator_effort_limit, - use_newton_actuators, -): - """Test setting of effort limit for explicit actuators. - - This test verifies the effort limit resolution logic for actuator models implemented in :class:`ActuatorBase`: - - Case 1: If USD value == actuator config value: values match correctly - - Case 2: If USD value != actuator config value: actuator config value is used - - Case 3: If actuator config value is None: USD value is used as default - - """ - - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_explicit", - joint_effort_limit=joint_effort_limit, - actuator_effort_limit=actuator_effort_limit, + model = SimulationManager.get_model() + newton_vel_limit = wp.to_torch(articulation.root_view.get_attribute("joint_velocity_limit", model)).to(device)[ + :, 0, : + ] + newton_effort_limit = wp.to_torch(articulation.root_view.get_attribute("joint_effort_limit", model)).to(device)[ + :, 0, : + ] + usd_vel_limit = next( + p.max_joint_velocity for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, PhysxJointCfg) ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, + usd_effort_limit = next( + p.max_force for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, sim_utils.UsdPhysicsDriveCfg) ) - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # usd default effort limit is set to 80 - usd_default_effort_limit = 80.0 - - # collect limit init values - newton_effort_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_effort_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - actuator_effort_limit_actual = articulation.actuators["joint"].actuator_effort_limit + actuator = articulation.actuators["joint"] - if actuator_effort_limit is not None: - expected_actuator_effort_limit = torch.full_like(actuator_effort_limit_actual, actuator_effort_limit) - # check actuator is set - torch.testing.assert_close(actuator_effort_limit_actual, expected_actuator_effort_limit) + # check data buffers + torch.testing.assert_close(articulation.data.joint_vel_limits.torch, newton_vel_limit) + torch.testing.assert_close(articulation.data.joint_effort_limits.torch, newton_effort_limit) + # the solver clamps come from the joint limits when set, otherwise the USD-authored values + expected_vel_limit = usd_vel_limit if joint_limit is None else joint_limit + expected_effort_limit = usd_effort_limit if joint_limit is None else joint_limit + torch.testing.assert_close(newton_vel_limit, torch.full_like(newton_vel_limit, expected_vel_limit)) + torch.testing.assert_close(newton_effort_limit, torch.full_like(newton_effort_limit, expected_effort_limit)) + # the actuator velocity limit keeps its configured value and is not pushed to the solver; + # when unset it falls back to the solver clamp + if actuator_limit is not None: + torch.testing.assert_close(actuator.actuator_velocity_limit, torch.full_like(newton_vel_limit, actuator_limit)) + assert not torch.allclose(actuator.actuator_velocity_limit, newton_vel_limit) else: - # When actuator_effort_limit is None, actuator should use USD default values - expected_actuator_effort_limit = torch.full_like(newton_effort_limit, usd_default_effort_limit) - torch.testing.assert_close(actuator_effort_limit_actual, expected_actuator_effort_limit) - - # the solver keeps the authored limit unless the user overrides it explicitly - if joint_effort_limit is not None: - limit = joint_effort_limit + torch.testing.assert_close(actuator.actuator_velocity_limit, newton_vel_limit) + + if articulation_type == "single_joint_implicit": + # without a separately configured rated limit, the implicit actuator limits track the solver clamp + torch.testing.assert_close(actuator.joint_effort_limit, newton_effort_limit) + torch.testing.assert_close(actuator.actuator_effort_limit, newton_effort_limit) + elif actuator_limit is not None: + torch.testing.assert_close(actuator.actuator_effort_limit, torch.full_like(newton_effort_limit, actuator_limit)) else: - limit = usd_default_effort_limit - # check physx internal value matches the expected sim value - expected_effort_limit = torch.full_like(newton_effort_limit, limit) - torch.testing.assert_close(articulation.data.joint_effort_limits.torch, expected_effort_limit) - torch.testing.assert_close(newton_effort_limit, expected_effort_limit) + # an unset explicit actuator effort limit falls back to the USD-authored value, not the solver clamp + torch.testing.assert_close( + actuator.actuator_effort_limit, torch.full_like(newton_effort_limit, usd_effort_limit) + ) @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["humanoid"]) def test_reset(sim, num_articulations, device, articulation_type, monkeypatch): - """Test that reset method works properly.""" + """Test that the actuator gains come from the configuration and that reset method works properly.""" articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation( articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device @@ -2983,6 +2040,14 @@ def test_reset(sim, num_articulations, device, articulation_type, monkeypatch): replicate(sim.get_clone_plan()) sim.reset() + # Check that gains are loaded from the configuration + expected_stiffness = torch.full( + (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device + ) + expected_damping = torch.full_like(expected_stiffness, 2.0) + torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) + torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) + # Now we are ready! # reset articulation actuator = next(iter(articulation.actuators.values())) @@ -2997,91 +2062,51 @@ def record_actuator_reset(env_ids=None): articulation.reset() assert reset_env_ids == [None] - # Reset should zero external forces and torques - assert not articulation._instantaneous_wrench_composer.active - assert not articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == 0 - - if num_articulations > 1: - num_bodies = articulation.num_bodies - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=torch.ones((num_articulations, num_bodies, 3), device=device), - torques=torch.ones((num_articulations, num_bodies, 3), device=device), - ) - articulation.instantaneous_wrench_composer.add_forces_and_torques_index( - forces=torch.ones((num_articulations, num_bodies, 3), device=device), - torques=torch.ones((num_articulations, num_bodies, 3), device=device), - ) - articulation.reset(env_ids=torch.tensor([0], device=device)) - assert articulation._instantaneous_wrench_composer.active - assert articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == num_bodies * 3 - + instantaneous_composer = articulation.instantaneous_wrench_composer + permanent_composer = articulation.permanent_wrench_composer -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_apply_joint_command(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test applying of joint position target functions correctly for a robotic arm.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device, add_ground_plane=True + # Reset should zero external forces and torques + assert not instantaneous_composer.active + assert not permanent_composer.active + assert torch.count_nonzero(instantaneous_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(instantaneous_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(permanent_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(permanent_composer.out_torque_b.torch) == 0 + + # A partial reset clears only the selected environment's wrenches + num_bodies = articulation.num_bodies + permanent_composer.set_forces_and_torques_index( + forces=torch.ones((num_articulations, num_bodies, 3), device=device), + torques=torch.ones((num_articulations, num_bodies, 3), device=device), ) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # reset dof state - joint_pos = articulation.data.default_joint_pos.torch.clone() - joint_pos[:, 3] = 0.0 - - # apply action to the articulation - articulation.set_joint_position_target_index(target=joint_pos) - articulation.write_data_to_sim() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # Check that current joint position is not the same as default joint position, meaning - # the articulation moved. We can't check that it reached its desired joint position as the gains - # are not properly tuned - assert not torch.allclose(articulation.data.joint_pos.torch, joint_pos) + instantaneous_composer.add_forces_and_torques_index( + forces=torch.ones((num_articulations, num_bodies, 3), device=device), + torques=torch.ones((num_articulations, num_bodies, 3), device=device), + ) + articulation.reset(env_ids=torch.tensor([0], device=device)) + assert instantaneous_composer.active + assert permanent_composer.active + assert torch.count_nonzero(instantaneous_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(instantaneous_composer.out_torque_b.torch) == num_bodies * 3 + assert torch.count_nonzero(permanent_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(permanent_composer.out_torque_b.torch) == num_bodies * 3 @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) -def test_body_root_state(sim, num_articulations, device, with_offset, articulation_type): +def test_body_root_state(sim, num_articulations, device, articulation_type): """Test for reading the `body_state_w` property. This test verifies that: - 1. Body states can be read correctly - 2. States are correct with and without offsets - 3. States are consistent across different devices + 1. The single-joint articulation initializes as fixed base with correctly shaped buffers + 2. Body link and center-of-mass states match an analytic pendulum with a center-of-mass offset + 3. The fixed root holds its default state Args: sim: The simulation fixture num_articulations: Number of articulations to test device: The device to run the simulation on - with_offset: Whether to test with offset """ sim._app_control_on_stop_handle = None articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) @@ -3095,16 +2120,24 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati assert articulation.is_initialized, "Articulation is not initialized" # Check that fixed base assert articulation.is_fixed_base, "Articulation is not a fixed base" + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) + assert articulation.data.joint_pos.torch.shape == (num_articulations, 1) + assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) + + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg # Resolve body indices by name (ordering may differ across physics backends) root_idx = articulation.body_names.index("CenterPivot") arm_idx = articulation.body_names.index("Arm") # change center of mass offset from link frame - if with_offset: - offset = [0.5, 0.0, 0.0] - else: - offset = [0.0, 0.0, 0.0] + offset = [0.5, 0.0, 0.0] # create com offsets — apply offset to the Arm body num_bodies = articulation.num_bodies @@ -3127,8 +2160,14 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati # update buffers articulation.update(sim.cfg.dt) + # check that the root is at the correct state - its default state as it is fixed base + default_root_pose = articulation.data.default_root_pose.torch.clone() + default_root_vel = articulation.data.default_root_vel.torch.clone() + default_root_pose[:, :3] = default_root_pose[:, :3] + env_pos + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) + torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) + # get state properties - root_link_pose_w = articulation.data.root_link_pose_w.torch root_link_vel_w = articulation.data.root_link_vel_w.torch root_com_pose_w = articulation.data.root_com_pose_w.torch root_com_vel_w = articulation.data.root_com_vel_w.torch @@ -3137,167 +2176,63 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati body_com_pose_w = articulation.data.body_com_pose_w.torch body_com_vel_w = articulation.data.body_com_vel_w.torch - if with_offset: - # get joint state - joint_pos = articulation.data.joint_pos.torch.unsqueeze(-1) - joint_vel = articulation.data.joint_vel.torch.unsqueeze(-1) - - # LINK state - # angular velocity should be the same for both COM and link frames - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # lin_vel arm - lin_vel_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) - vx = -(link_offset[0]) * joint_vel * torch.sin(joint_pos) - vy = torch.zeros(num_articulations, 1, 1, device=device) - vz = (link_offset[0]) * joint_vel * torch.cos(joint_pos) - lin_vel_gt[:, arm_idx, :] = torch.cat([vx, vy, vz], dim=-1).squeeze(-2) - - # linear velocity of root link should be zero - torch.testing.assert_close(lin_vel_gt[:, root_idx, :], root_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) - # linear velocity of pendulum link should be - torch.testing.assert_close(lin_vel_gt, body_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) - - # ang_vel - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # COM state - # position and orientation shouldn't match for the _state_com_w but everything else will - pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) - px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) - py = torch.zeros(num_articulations, 1, 1, device=device) - pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) - pos_gt[:, arm_idx, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) - pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) - torch.testing.assert_close(pos_gt[:, root_idx, :], root_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) - torch.testing.assert_close(pos_gt, body_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) - - # orientation - com_quat_b = articulation.data.body_com_quat_b.torch - com_quat_w = math_utils.quat_mul(body_link_pose_w[..., 3:], com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:]) - torch.testing.assert_close(com_quat_w[:, root_idx, :], root_com_pose_w[..., 3:]) - - # angular velocity should be the same for both COM and link frames - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - else: - # single joint center of masses are at link frames so they will be the same - torch.testing.assert_close(root_link_pose_w, root_com_pose_w) - torch.testing.assert_close(root_com_vel_w, root_link_vel_w) - torch.testing.assert_close(body_link_pose_w, body_com_pose_w) - torch.testing.assert_close(body_com_vel_w, body_link_vel_w) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_write_root_state( - sim, num_articulations, device, with_offset, state_location, gravity_enabled, articulation_type -): - """Test the setters for root_state using both the link frame and center of mass as reference frame. - - This test verifies that: - 1. Root states can be written correctly - 2. States are correct with and without offsets - 3. States can be written for both COM and link frames - 4. States are consistent across different devices - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - with_offset: Whether to test with offset - state_location: Whether to test COM or link frame - """ - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) - env_idx = torch.tensor([x for x in range(num_articulations)], device=device, dtype=torch.int32) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Resolve root body index by name (ordering may differ across physics backends) - root_idx = articulation.find_bodies("base")[0][0] - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([1.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - - # create com offsets - com = wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())) - new_com = offset - com[:, 0, root_idx, :] = new_com.squeeze(-2) - articulation.root_view.set_attribute("body_com", SimulationManager.get_model(), wp.from_torch(com, dtype=wp.vec3f)) - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) - - # check they are set - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())), com - ) - - rand_state = torch.zeros(num_articulations, 13, device=device) - rand_state[..., :7] = articulation.data.default_root_pose.torch - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_idx = env_idx.to(device) - for i in range(10): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - if state_location == "com": - if i % 2 == 0: - articulation.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - articulation.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - articulation.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) - elif state_location == "link": - if i % 2 == 0: - articulation.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - articulation.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - articulation.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) - - if state_location == "com": - torch.testing.assert_close(rand_state[..., :7], articulation.data.root_com_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_com_vel_w.torch) - elif state_location == "link": - torch.testing.assert_close(rand_state[..., :7], articulation.data.root_link_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_link_vel_w.torch) + # get joint state + joint_pos = articulation.data.joint_pos.torch.unsqueeze(-1) + joint_vel = articulation.data.joint_vel.torch.unsqueeze(-1) + + # LINK state + # angular velocity should be the same for both COM and link frames + torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) + torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) + + # lin_vel arm + lin_vel_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) + vx = -(link_offset[0]) * joint_vel * torch.sin(joint_pos) + vy = torch.zeros(num_articulations, 1, 1, device=device) + vz = (link_offset[0]) * joint_vel * torch.cos(joint_pos) + lin_vel_gt[:, arm_idx, :] = torch.cat([vx, vy, vz], dim=-1).squeeze(-2) + + # linear velocity of root link should be zero + torch.testing.assert_close(lin_vel_gt[:, root_idx, :], root_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) + # linear velocity of pendulum link should be + torch.testing.assert_close(lin_vel_gt, body_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) + + # COM state + # position and orientation shouldn't match for the _state_com_w but everything else will + pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) + px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) + py = torch.zeros(num_articulations, 1, 1, device=device) + pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) + pos_gt[:, arm_idx, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) + pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) + torch.testing.assert_close(pos_gt[:, root_idx, :], root_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) + torch.testing.assert_close(pos_gt, body_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) + + # orientation + com_quat_b = articulation.data.body_com_quat_b.torch + com_quat_w = math_utils.quat_mul(body_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:]) + torch.testing.assert_close(com_quat_w[:, root_idx, :], root_com_pose_w[..., 3:]) @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) @pytest.mark.parametrize("state_location", ["com", "link", "root"]) @pytest.mark.parametrize("gravity_enabled", [False]) @pytest.mark.parametrize("articulation_type", ["anymal"]) def test_write_root_state_functions_data_consistency( - sim, num_articulations, device, with_offset, state_location, gravity_enabled, articulation_type + sim, num_articulations, device, state_location, gravity_enabled, articulation_type ): - """A root pose/velocity write must refresh the derived cross-frame caches without a sim step. + """A root pose/velocity write must read back in the written frame and refresh the derived cross-frame caches. Regression coverage for the velocity invalidation cleanup: writing the root center-of-mass (or link) velocity must invalidate the derived root link (or com) velocity so the next read re-derives it. Linear velocity differs between the two frames, so - as in the rigid object test - we compare angular velocity, which is frame-independent and therefore only matches when the derived velocity was actually refreshed. + + The center-of-mass offset is set through ``set_coms_index``, which must update the body-frame CoM and + the world-frame CoM derived from it without a sim step. """ sim._app_control_on_stop_handle = None articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) @@ -3311,20 +2246,56 @@ def test_write_root_state_functions_data_consistency( root_idx = articulation.find_bodies("base")[0][0] # change center of mass offset from link frame on the root body - if with_offset: - offset = torch.tensor([1.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - com = wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())) - com[:, 0, root_idx, :] = offset.squeeze(-2) - articulation.root_view.set_attribute("body_com", SimulationManager.get_model(), wp.from_torch(com, dtype=wp.vec3f)) - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) + original_com = articulation.data.body_com_pos_b.torch.clone() + # Populate the derived world-frame cache so a missing invalidation would surface as a stale read. + original_com_w = articulation.data.body_com_pos_w.torch.clone() + new_com = original_com.clone() + new_com[:, root_idx] = torch.tensor([1.0, 0.0, 0.0], device=device) + env_ids = torch.arange(num_articulations, device=device, dtype=torch.int32) + # Full poses are accepted too; Newton uses the position and ignores the orientation. + com_poses = articulation.data.body_com_pose_b.torch.clone() + com_poses[..., :3] = new_com + articulation.set_coms_index(coms=com_poses, env_ids=env_ids) + torch.testing.assert_close(articulation.data.body_com_pos_b.torch, new_com, atol=1e-5, rtol=1e-5) + articulation.set_coms_index(coms=original_com, env_ids=env_ids) + torch.testing.assert_close(articulation.data.body_com_pos_b.torch, original_com, atol=1e-5, rtol=1e-5) + _ = articulation.data.body_com_pos_w.torch + articulation.set_coms_index(coms=new_com, env_ids=env_ids) + + torch.testing.assert_close(articulation.data.body_com_pos_b.torch, new_com, atol=1e-5, rtol=1e-5) + # Without a sim step the links stay put, so the world-frame CoM is the link pose applied to the new offset. + link_pos_w = articulation.data.body_link_pos_w.torch + link_quat_w = articulation.data.body_link_quat_w.torch + expected_com_w = link_pos_w + math_utils.quat_apply(link_quat_w, new_com) + updated_com_w = articulation.data.body_com_pos_w.torch + torch.testing.assert_close(updated_com_w, expected_com_w, atol=1e-5, rtol=1e-5) + assert not torch.allclose(updated_com_w, original_com_w) + + def random_root_state(num_envs: int) -> torch.Tensor: + state = torch.rand(num_envs, 13, device=device) + state[..., :3] += env_pos[:num_envs] + # make quaternion a unit vector + state[..., 3:7] = torch.nn.functional.normalize(state[..., 3:7], dim=-1) + return state + + def write_root_state(state: torch.Tensor, env_ids: torch.Tensor | None = None): + if state_location == "com": + articulation.write_root_com_pose_to_sim_index(root_pose=state[..., :7], env_ids=env_ids) + articulation.write_root_com_velocity_to_sim_index(root_velocity=state[..., 7:], env_ids=env_ids) + elif state_location == "link": + articulation.write_root_link_pose_to_sim_index(root_pose=state[..., :7], env_ids=env_ids) + articulation.write_root_link_velocity_to_sim_index(root_velocity=state[..., 7:], env_ids=env_ids) + elif state_location == "root": + articulation.write_root_pose_to_sim_index(root_pose=state[..., :7], env_ids=env_ids) + articulation.write_root_velocity_to_sim_index(root_velocity=state[..., 7:], env_ids=env_ids) - rand_state = torch.rand(num_articulations, 13, device=device) - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + def read_written_root_state() -> torch.Tensor: + # the root pose is the link pose and the root velocity is the center-of-mass velocity + pose_w = articulation.data.root_com_pose_w if state_location == "com" else articulation.data.root_link_pose_w + vel_w = articulation.data.root_link_vel_w if state_location == "link" else articulation.data.root_com_vel_w + return torch.cat((pose_w.torch, vel_w.torch), dim=-1) + + rand_state = random_root_state(num_articulations) # perform a step then update the buffers sim.step() @@ -3338,15 +2309,7 @@ def test_write_root_state_functions_data_consistency( _ = articulation.data.root_link_vel_w.torch _ = articulation.data.root_com_vel_w.torch - if state_location == "com": - articulation.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "link": - articulation.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "root": - articulation.write_root_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + write_root_state(rand_state) body_com_pose_b = articulation.data.body_com_pose_b.torch if state_location == "com": @@ -3384,6 +2347,16 @@ def test_write_root_state_functions_data_consistency( torch.testing.assert_close(expected_com_pose, root_com_pose_w) torch.testing.assert_close(root_link_vel_w[:, 3:], root_com_vel_w[:, 3:]) + # the written state reads back in the written frame + torch.testing.assert_close(read_written_root_state(), rand_state) + + # a partial write only changes the selected environment + partial_state = random_root_state(1) + write_root_state(partial_state, env_ids=torch.tensor([0], device=device, dtype=torch.int32)) + expected_state = rand_state.clone() + expected_state[0] = partial_state[0] + torch.testing.assert_close(read_written_root_state(), expected_state) + @pytest.mark.parametrize("device", ["cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) @@ -3401,9 +2374,10 @@ def test_setting_articulation_root_prim_path(sim, device, articulation_type, roo if root_prim_path == "/torso": replicate(sim.get_clone_plan()) sim.reset() - assert articulation._is_initialized + assert articulation.is_initialized else: - with pytest.raises((RuntimeError, KeyError)): + replicate(sim.get_clone_plan()) + with pytest.raises(KeyError, match="No articulations matching pattern"): sim.reset() @@ -3412,11 +2386,14 @@ def test_setting_articulation_root_prim_path(sim, device, articulation_type, roo @pytest.mark.parametrize("gravity_enabled", [False]) @pytest.mark.parametrize("articulation_type", ["anymal"]) def test_write_joint_state_data_consistency(sim, num_articulations, device, gravity_enabled, articulation_type): - """Test the setters for root_state using both the link frame and center of mass as reference frame. + """Joint limit and joint state writes update the joint buffers and refresh the body state without a step. + + This test verifies that: + 1. Joint position limits are written and keep in-limit default joint positions + 2. A partial joint state write with unsorted int64 selectors updates only the selected entries + 3. A joint state write moves the bodies and refreshes the derived body poses and velocities + 4. Indexed joint limits that exclude a default joint position clamp it into the new limits - This test verifies that after write_joint_state_to_sim operations: - 1. state, com_state, link_state value consistency - 2. body_pose, link Args: sim: The simulation fixture num_articulations: Number of articulations to test @@ -3431,11 +2408,33 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav replicate(sim.get_clone_plan()) sim.reset() + # Get current default joint pos + default_joint_pos = articulation.data.default_joint_pos.torch.clone() + limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) limits[..., 0] = (torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0) * -1.0 limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 articulation.write_joint_position_limit_to_sim_index(limits=limits) + # Check new limits are in place and the in-limit defaults are preserved + torch.testing.assert_close(articulation.data.joint_pos_limits.torch, limits) + torch.testing.assert_close(articulation.data.default_joint_pos.torch, default_joint_pos) + + # Write joint state with unsorted int64 selectors + env_ids = torch.tensor([1, 0], dtype=torch.int64, device=device) + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int64, device=device) + position = torch.tensor([[0.21, 0.11], [0.22, 0.12]], device=device) + velocity = torch.tensor([[1.21, 1.11], [1.22, 1.12]], device=device) + expected_position = articulation.data.joint_pos.torch.clone() + expected_velocity = articulation.data.joint_vel.torch.clone() + articulation.write_joint_state_to_sim_index( + position=position, velocity=velocity, env_ids=env_ids, joint_ids=joint_ids + ) + expected_position[env_ids[:, None], joint_ids[None, :]] = position + expected_velocity[env_ids[:, None], joint_ids[None, :]] = velocity + torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) + torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + from torch.distributions import Uniform joint_pos_limits = articulation.data.joint_pos_limits.torch @@ -3459,11 +2458,9 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav assert torch.count_nonzero(original_body_states[:, 1:] != body_state_w[:, 1:]) > ( len(original_body_states[:, 1:]) / 2 ) - # validate body - link consistency - body_link_vel_w = articulation.data.body_link_vel_w.torch - torch.testing.assert_close(body_link_pose_w, articulation.data.body_link_pose_w.torch) # skip lin_vel because it differs from link frame, this should be fine because we are only checking # if velocity update is triggered, which can be determined by comparing angular velocity + body_link_vel_w = articulation.data.body_link_vel_w.torch torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) # validate link - com conistency @@ -3475,78 +2472,39 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav body_com_pos_b.view(-1, 3), body_com_quat_b.view(-1, 4), ) - body_com_pos_w = articulation.data.body_com_pos_w.torch - body_com_quat_w = articulation.data.body_com_quat_w.torch - torch.testing.assert_close(expected_com_pos.view(len(env_idx), -1, 3), body_com_pos_w) - torch.testing.assert_close(expected_com_quat.view(len(env_idx), -1, 4), body_com_quat_w) - - # validate body - com consistency - body_com_lin_vel_w = articulation.data.body_com_lin_vel_w.torch - body_com_ang_vel_w = articulation.data.body_com_ang_vel_w.torch - torch.testing.assert_close(body_com_vel_w[..., :3], body_com_lin_vel_w) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_com_ang_vel_w) - - # validate pos_w, quat_w, pos_b, quat_b is consistent with pose_w and pose_b - expected_com_pose_w = torch.cat((body_com_pos_w, body_com_quat_w), dim=2) - expected_com_pose_b = torch.cat((body_com_pos_b, body_com_quat_b), dim=2) - body_pos_w = articulation.data.body_pos_w.torch - body_quat_w = articulation.data.body_quat_w.torch - expected_body_pose_w = torch.cat((body_pos_w, body_quat_w), dim=2) - body_link_pos_w = articulation.data.body_link_pos_w.torch - body_link_quat_w = articulation.data.body_link_quat_w.torch - expected_body_link_pose_w = torch.cat((body_link_pos_w, body_link_quat_w), dim=2) - body_com_pose_w = articulation.data.body_com_pose_w.torch - body_com_pose_b = articulation.data.body_com_pose_b.torch - body_pose_w = articulation.data.body_pose_w.torch - body_link_pose_w_fresh = articulation.data.body_link_pose_w.torch - torch.testing.assert_close(body_com_pose_w, expected_com_pose_w) - torch.testing.assert_close(body_com_pose_b, expected_com_pose_b) - torch.testing.assert_close(body_pose_w, expected_body_pose_w) - torch.testing.assert_close(body_link_pose_w_fresh, expected_body_link_pose_w) - - # validate pose_w is consistent with individual properties - body_vel_w = articulation.data.body_vel_w.torch - body_com_vel_w_fresh = articulation.data.body_com_vel_w.torch - torch.testing.assert_close(body_pose_w, body_link_pose_w) - torch.testing.assert_close(body_vel_w, body_com_vel_w) - torch.testing.assert_close(body_link_pose_w_fresh, body_link_pose_w) - torch.testing.assert_close(body_com_pose_w, articulation.data.body_com_pose_w.torch) - torch.testing.assert_close(body_vel_w, body_com_vel_w_fresh) - - -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test static joint friction writes propagate directly to the Newton model.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device, add_ground_plane=True - ) + torch.testing.assert_close(expected_com_pos.view(len(env_idx), -1, 3), articulation.data.body_com_pos_w.torch) + torch.testing.assert_close(expected_com_quat.view(len(env_idx), -1, 4), articulation.data.body_com_quat_w.torch) - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() + # Set new joint limits with indexing that invalidate the selected default joint positions + env_ids = torch.arange(1, device=device, dtype=torch.int32) + joint_ids = torch.nonzero(default_joint_pos[0].abs() > 0.1).squeeze(-1)[:2].to(torch.int32) + assert len(joint_ids) == 2 + limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) + limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 + limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 + articulation.write_joint_position_limit_to_sim_index(limits=limits, env_ids=env_ids, joint_ids=joint_ids) - friction = torch.rand(num_articulations, articulation.num_joints, device=device) - articulation.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=friction, + # Check new limits are in place and the defaults are clamped into them + torch.testing.assert_close(articulation.data.joint_pos_limits.torch[env_ids][:, joint_ids], limits) + default_joint_pos_torch = articulation.data.default_joint_pos.torch + within_bounds = (default_joint_pos_torch[env_ids][:, joint_ids] >= limits[..., 0]) & ( + default_joint_pos_torch[env_ids][:, joint_ids] <= limits[..., 1] ) - joint_friction_coeff_sim = wp.to_torch( - articulation.root_view.get_attribute("joint_friction", SimulationManager.get_model()) - )[:, 0, :] - torch.testing.assert_close(joint_friction_coeff_sim, friction) + assert torch.all(within_bounds) @pytest.mark.parametrize("selector_kind", ["index", "mask"]) +@pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("articulation_type", ["panda"]) @pytest.mark.parametrize("device", test_devices()) -def test_write_joint_viscous_friction_to_sim(sim, device, articulation_type, selector_kind): - """Test passive viscous joint damping is distinct from actuator derivative gains.""" +def test_write_joint_viscous_friction_to_sim(sim, num_articulations, device, articulation_type, selector_kind): + """Test passive viscous joint damping is distinct from actuator derivative gains. + + Static joint friction writes also propagate directly to the Newton model. + """ articulation_cfg = generate_articulation_cfg(articulation_type) articulation_cfg.actuators["panda_shoulder"].viscous_friction = 0.25 - articulation, _ = generate_articulation(articulation_cfg, 1, device) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) replicate(sim.get_clone_plan()) sim.reset() @@ -3587,6 +2545,15 @@ def test_write_joint_viscous_friction_to_sim(sim, device, articulation_type, sel values, ) + # Distinct per-env rows catch writers that ignore the env index + friction = torch.rand(articulation.num_instances, articulation.num_joints, device=device) + assert not torch.allclose(friction[0], friction[1]) + articulation.write_joint_friction_coefficient_to_sim_index(joint_friction_coeff=friction) + joint_friction_coeff_sim = wp.to_torch( + articulation.root_view.get_attribute("joint_friction", SimulationManager.get_model()) + )[:, 0, :] + torch.testing.assert_close(joint_friction_coeff_sim, friction) + @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @@ -3670,135 +2637,106 @@ def _patched_simulate(cls): ) -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_randomize_rigid_body_com(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that randomize_rigid_body_com modifies CoM and affects simulation dynamics.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - original_com = articulation.data.body_com_pos_b.torch.clone() - - com_offset = torch.zeros(num_articulations, articulation.num_bodies, 3, device=device) - com_offset[..., 0] = 0.5 - new_com = original_com + com_offset - env_ids = torch.arange(num_articulations, device=device, dtype=torch.int32) - articulation.set_coms_index(coms=new_com, env_ids=env_ids) - - updated_com = articulation.data.body_com_pos_b.torch - torch.testing.assert_close(updated_com, new_com, atol=1e-5, rtol=1e-5) - - # poses (position and quaternion) are accepted too, like on the other backends; the orientation is ignored - com_poses = articulation.data.body_com_pose_b.torch.clone() - com_poses[..., :3] = original_com - articulation.set_coms_index(coms=com_poses, env_ids=env_ids) - torch.testing.assert_close(articulation.data.body_com_pos_b.torch, original_com, atol=1e-5, rtol=1e-5) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that Newton collider offset randomization (shape_margin, shape_gap) takes effect.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - model = SimulationManager.get_model() - original_margin = wp.to_torch(articulation.root_view.get_attribute("shape_margin", model)).clone() - original_gap = wp.to_torch(articulation.root_view.get_attribute("shape_gap", model)).clone() - - new_margin = original_margin.clone() - new_margin[:, 0] += 0.01 - articulation.root_view.set_attribute("shape_margin", model, wp.from_torch(new_margin, dtype=wp.float32)) - - new_gap = original_gap.clone() - new_gap[:, 0] += 0.005 - articulation.root_view.set_attribute("shape_gap", model, wp.from_torch(new_gap, dtype=wp.float32)) - - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.SHAPE_PROPERTIES) - - updated_margin = wp.to_torch(articulation.root_view.get_attribute("shape_margin", model)) - updated_gap = wp.to_torch(articulation.root_view.get_attribute("shape_gap", model)) - torch.testing.assert_close(updated_margin, new_margin) - torch.testing.assert_close(updated_gap, new_gap) - - -## -# Shape-contract regression tests for the new BaseArticulation accessors. -# These pin the public shape contract so future regressions (e.g., reverting -# to model-wide max sizing or to the wrong fixed-base row offset) fail fast. -## - - -@pytest.mark.parametrize("num_articulations", [4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) -@pytest.mark.isaacsim_ci -def test_dynamics_accessor_shapes(sim, num_articulations, device, add_ground_plane, articulation_type): - """Pin the per-articulation shapes of the Jacobian, mass matrix and gravity compensation accessors. - - Also checks that the mass matrix is symmetric and positive-definite. - - Fixed-base (panda): ``body_link_jacobian_w`` drops the fixed-root row, so its shape is - ``(N, num_bodies - 1, 6, num_joints)``; ``mass_matrix`` is ``(N, num_joints, num_joints)`` and - ``gravity_compensation_forces`` is ``(N, num_joints)``. - - Floating-base (anymal): every body row is kept and ``num_base_dofs`` floating-base columns/entries - are prepended on the DoF axis, matching the cross-library convention (Pinocchio, Drake, MuJoCo, - RBDL, OCS2, iDynTree). +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("body_subset", [False, True]) +def test_set_material_properties(sim, num_articulations, device, add_ground_plane, articulation_type, body_subset): + """Material and collider-offset randomization write through to the robot's shapes in the Newton model. - Both catch (a) the link_offset fix that drops Newton's row 0 for fixed-base and (b) per-articulation - output sizing: model-wide ``max_links``/``max_dofs`` would over-allocate in heterogeneous scenes and - surface as zero-padded mass-matrix diagonals, which is why the diagonal is asserted strictly positive - rather than checking a determinant (a well-formed 9x9 Franka mass matrix has det ~1e-13). + The event terms write the asset's view-level shape bindings; the assertions read the flat Newton model + arrays at the collision shapes of the selected bodies and environments. Visual shapes are ignored because + their materials and offsets have no physical effect. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device, add_ground_plane=True + ) + + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base == (articulation_type == "panda") - - num_dofs = articulation.num_joints + articulation.num_base_dofs - num_jacobian_bodies = articulation.num_bodies - 1 if articulation.is_fixed_base else articulation.num_bodies - - J = articulation.data.body_link_jacobian_w.torch - assert J.shape == torch.Size((num_articulations, num_jacobian_bodies, 6, num_dofs)), tuple(J.shape) - assert J.dtype == torch.float32 - g = articulation.data.gravity_compensation_forces.torch - assert g.shape == torch.Size((num_articulations, num_dofs)), tuple(g.shape) - assert g.dtype == torch.float32 + # Resolve the robot's shapes per environment and body from the flat Newton model. + model = SimulationManager.get_model() + body_world = model.body_world.numpy() + body_names = [label.rsplit("/", 1)[-1] for label in model.body_label] + shape_body = model.shape_body.numpy() + is_collision_shape = (model.shape_flags.numpy() & int(ShapeFlags.COLLIDE_SHAPES)) != 0 + + def robot_shapes(env_index: int, selected_body_names: list[str] | None = None) -> np.ndarray: + bodies = [ + body + for body in np.flatnonzero(body_world == env_index) + if selected_body_names is None or body_names[body] in selected_body_names + ] + return np.flatnonzero(np.isin(shape_body, bodies) & is_collision_shape) + + env = SimpleNamespace(scene={"robot": articulation}, sim=sim, device=device, num_envs=num_articulations) + env_ids = torch.tensor([num_articulations - 1], device=device) + + # Randomize the materials in the last environment, with degenerate ranges. + asset_cfg = SceneEntityCfg("robot") + selected_body_names = None + if body_subset: + asset_cfg.body_ids, selected_body_names = articulation.find_bodies(["panda_link3", "panda_hand"]) + selected_shapes = robot_shapes(num_articulations - 1, selected_body_names) + unselected_shapes = np.setdiff1d(robot_shapes(num_articulations - 1), selected_shapes) + other_env_shapes = robot_shapes(0) + assert len(selected_shapes) > 0 and len(other_env_shapes) > 0 + original_mu = model.shape_material_mu.numpy().copy() + original_restitution = model.shape_material_restitution.numpy().copy() + params = { + "static_friction_range": (0.55, 0.55), + "dynamic_friction_range": (0.55, 0.55), + "restitution_range": (0.15, 0.15), + "num_buckets": 1, + "asset_cfg": asset_cfg, + } + material_term = randomize_rigid_body_material( + EventTermCfg(func=randomize_rigid_body_material, mode="startup", params=params), env + ) + material_term(env, env_ids, **params) + + # Randomize the collider offsets of every shape in the last environment. + original_margin = model.shape_margin.numpy().copy() + original_gap = model.shape_gap.numpy().copy() + offset_params = { + "asset_cfg": SceneEntityCfg("robot"), + "rest_offset_distribution_params": (0.01, 0.01), + "contact_offset_distribution_params": (0.03, 0.03), + } + offset_term = randomize_rigid_body_collider_offsets( + EventTermCfg(func=randomize_rigid_body_collider_offsets, mode="startup", params=offset_params), env + ) + offset_term(env, env_ids, **offset_params) + # Simulate physics sim.step() articulation.update(sim.cfg.dt) - M = articulation.data.mass_matrix.torch - assert M.shape == torch.Size((num_articulations, num_dofs, num_dofs)), tuple(M.shape) - assert M.dtype == torch.float32 - diag = M.diagonal(dim1=-2, dim2=-1) - assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" + mu = model.shape_material_mu.numpy() + restitution = model.shape_material_restitution.numpy() + np.testing.assert_allclose(mu[selected_shapes], 0.55) + np.testing.assert_allclose(restitution[selected_shapes], 0.15) + for shapes in (unselected_shapes, other_env_shapes): + np.testing.assert_array_equal(mu[shapes], original_mu[shapes]) + np.testing.assert_array_equal(restitution[shapes], original_restitution[shapes]) - # The joint-space inertia is symmetric by construction; asymmetry means a wrong-axis gather or a - # half-populated buffer. OSC inverts ``J M^-1 J^T`` every step, so ``M`` must also be positive-definite. - asym = (M - M.transpose(-1, -2)).abs().max().item() - assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric" - # A tiny jitter tolerates the float32 eigenvalue floor without masking real non-PD bugs. - eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) - torch.linalg.cholesky(M + 1e-6 * eye) + # Newton maps the rest offset to the shape margin and the contact offset to margin + gap. + randomized_shapes = robot_shapes(num_articulations - 1) + np.testing.assert_allclose(model.shape_margin.numpy()[randomized_shapes], 0.01) + np.testing.assert_allclose(model.shape_gap.numpy()[randomized_shapes], 0.02, atol=1e-6) + np.testing.assert_array_equal(model.shape_margin.numpy()[other_env_shapes], original_margin[other_env_shapes]) + np.testing.assert_array_equal(model.shape_gap.numpy()[other_env_shapes], original_gap[other_env_shapes]) + + +## +# Shape-contract regression tests for the new BaseArticulation accessors. +# These pin the public shape contract so future regressions (e.g., reverting +# to model-wide max sizing or to the wrong fixed-base row offset) fail fast. +## @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @@ -3905,88 +2843,6 @@ def test_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, arti assert anymal_g.abs().max() > 1e-3, "Anymal gravity compensation is all-zero under heterogeneous scene" -@pytest.mark.parametrize("num_articulations", [4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci -def test_get_jacobians_link_origin_contract(sim, num_articulations, device, articulation_type, gravity_enabled): - """``J · q_dot`` must encode the link-origin twist (after the COM->origin shift). - - The IsaacLab task-space controllers (IK / OSC / RMPFlow) silently - rely on :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` - returning a Jacobian whose linear rows reference each link's origin - (the body's USD prim transform), not its COM. Newton's ``eval_jacobian`` - natively produces COM-referenced rows; the wrapper applies a per-column - shift ``v_origin = v_com - omega x (R · body_com_pos_b)`` to honor the - contract. This test asserts the identity by computing both sides - independently: - - * Predicted by ``J · q_dot``: takes the (already-shifted) Jacobian - and the same ``q_dot`` Newton has after the kinematics refresh. Linear rows should - equal v_origin. - * Ground truth from ``state.body_qd``: read Newton's per-body spatial - twist directly via ``ArticulationView.get_link_velocities`` (which - returns ``(v_com_world, omega_world)``), then apply the same shift - in python and compare. - - Reading the velocity from the ArticulationView state rather than - ``data.body_com_lin_vel_w`` bypasses the IsaacLab lazy-buffer chain, - which is irrelevant to the contract being tested. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - # Reproducible non-trivial q_dot — large enough to drive omega well above - # the floor where COM offset effects would round into noise. - torch.manual_seed(0) - qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 - articulation.write_joint_velocity_to_sim_index(velocity=qdot) - # Refresh kinematics without integrating: for floating bases, an actuator step can - # introduce root motion that is intentionally absent from the actuated-only J slice. - sim.forward() - articulation.update(sim.cfg.dt) - - # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past - # them so the joint axis aligns with joint_vel (actuated-only). - J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :] - qdot_view = articulation.data.joint_vel.torch - v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) # (N, B_jac, 6) - v_pred_lin = v_pred[..., 0:3] - v_pred_ang = v_pred[..., 3:6] - - # Ground truth from Newton state. ``get_link_velocities`` returns shape - # (num_instances, 1, num_bodies, 6) — per-articulation grouping with - # one articulation per instance — so we squeeze the inner dim. - state = SimulationManager.get_state_0() - body_qd_view = wp.to_torch(articulation.root_view.get_link_velocities(state)).squeeze(1) - body_v_com = body_qd_view[..., :3] - body_omega = body_qd_view[..., 3:] - - # World-frame COM-to-origin offset, derived from already-computed - # data layer outputs (avoids quaternion-convention pitfalls). - body_com_pos_w = articulation.data.body_com_pos_w.torch # (N, num_bodies, 3) - body_link_pos_w = articulation.data.body_link_pos_w.torch # (N, num_bodies, 3) - c_world = body_com_pos_w - body_link_pos_w - - if articulation.is_fixed_base: - body_v_com = body_v_com[:, 1:] - body_omega = body_omega[:, 1:] - c_world = c_world[:, 1:] - - # Expected v_origin = v_com - omega x c_world. - v_origin_expected = body_v_com - torch.cross(body_omega, c_world, dim=-1) - - # Tolerance: 5 mm absolute. The COM-offset bug produces a ~3 cm bias - # on the panda hand under the 0.5-rad/s injected qdot, well above - # this floor; numerical noise from kernel ordering stays under 1 mm. - torch.testing.assert_close(v_pred_ang, body_omega, atol=5e-3, rtol=1e-2) - torch.testing.assert_close(v_pred_lin, v_origin_expected, atol=5e-3, rtol=1e-2) - - @pytest.mark.parametrize("num_articulations", [4]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -4000,12 +2856,11 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( Newton computes the gravity compensation force through an RNEA pass (``eval_inverse_dynamics_passive``); the static identity above derives the same - quantity independently from the (already contract-validated) COM-referenced - Jacobian and the per-body masses, pinning the sign convention, the DoF - ordering (including the 6 floating-base entries), and the flat-buffer view - gather in one assertion. Non-default joint positions and — for - floating-base — a rotated, lifted root pose guard the corner fixed - upstream in newton#2625 (wrong gravity compensation under non-identity + quantity independently from the COM-referenced Jacobian and the per-body masses, + pinning the sign convention, the DoF ordering (including the 6 floating-base + entries), and the flat-buffer view gather in one assertion. Non-default joint + positions and — for floating-base — a rotated, lifted root pose guard the corner + fixed upstream in newton#2625 (wrong gravity compensation under non-identity root pose). With ``ordering_mode="reversed"`` a nonidentity joint ordering is active and @@ -4013,15 +2868,82 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( Jacobian gather applies the user->backend permutation, so a ``gather_dof_force_rows`` that skips it returns backend-ordered forces and breaks the identity row-wise. + + The same fixture also pins: + + * the per-articulation shapes of the Jacobian, mass matrix and gravity compensation + accessors, and a symmetric, positive-definite mass matrix. Fixed-base (panda): + ``body_link_jacobian_w`` drops the fixed-root row, so its shape is + ``(N, num_bodies - 1, 6, num_joints)``. Floating-base (anymal): every body row is kept and + ``num_base_dofs`` floating-base columns/entries are prepended on the DoF axis, matching the + cross-library convention (Pinocchio, Drake, MuJoCo, RBDL, OCS2, iDynTree). A zero-padded + model-wide sizing would surface as non-positive mass-matrix diagonals; + * that every dynamics accessor reflects a manual joint write without a sim step (the FK + trigger before ``eval_jacobian``, ``eval_mass_matrix`` and the RNEA pass); + * the link-origin Jacobian contract: ``J · q_dot`` must encode the link-origin twist + ``v_origin = v_com - omega x (R · body_com_pos_b)``, which the IsaacLab task-space + controllers (IK / OSC / RMPFlow) rely on. Newton's ``eval_jacobian`` natively produces + COM-referenced rows, so the ground truth reads Newton's per-body twist directly from the + ArticulationView state. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) if ordering_mode == "reversed": joint_names = PANDA_JOINT_NAMES if articulation_type == "panda" else ANYMAL_C_PHYSX_JOINT_NAMES articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(joint_names))) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) + + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(articulation) < 10 + replicate(sim.get_clone_plan()) sim.reset() assert articulation.is_initialized + assert articulation.is_fixed_base == (articulation_type == "panda") + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) + assert articulation.data.joint_pos.torch.shape == (num_articulations, 9 if articulation_type == "panda" else 12) + assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) + + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg + + num_dofs = articulation.num_joints + articulation.num_base_dofs + num_jacobian_bodies = articulation.num_bodies - 1 if articulation.is_fixed_base else articulation.num_bodies + + J = articulation.data.body_link_jacobian_w.torch + assert J.shape == torch.Size((num_articulations, num_jacobian_bodies, 6, num_dofs)), tuple(J.shape) + assert J.dtype == torch.float32 + + g = articulation.data.gravity_compensation_forces.torch + assert g.shape == torch.Size((num_articulations, num_dofs)), tuple(g.shape) + assert g.dtype == torch.float32 + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch + assert M.shape == torch.Size((num_articulations, num_dofs, num_dofs)), tuple(M.shape) + assert M.dtype == torch.float32 + diag = M.diagonal(dim1=-2, dim2=-1) + assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" + + # The joint-space inertia is symmetric by construction; asymmetry means a wrong-axis gather or a + # half-populated buffer. OSC inverts ``J M^-1 J^T`` every step, so ``M`` must also be positive-definite. + asym = (M - M.transpose(-1, -2)).abs().max().item() + assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric" + # A tiny jitter tolerates the float32 eigenvalue floor without masking real non-PD bugs. + eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) + torch.linalg.cholesky(M + 1e-6 * eye) + + # Read every accessor at the stepped joint state. + J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() + J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() + M_0 = articulation.data.mass_matrix.torch.clone() + g_0 = articulation.data.gravity_compensation_forces.torch.clone() # Non-trivial configuration via manual writes (no sim step, so the assert # compares both quantities at exactly this state): random joint offsets, @@ -4042,6 +2964,21 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( # agree trivially, voiding the newton#2625 rotated-root coverage. torch.testing.assert_close(articulation.data.root_link_pose_w.torch, root_pose, atol=1e-5, rtol=0.0) + # With the FK trigger, forward() refreshes body_q to the written state before each accessor evaluates. + # Without it, body_q stays at the previous state and every accessor returns its previous value. + assert not torch.allclose(J_link_0, articulation.data.body_link_jacobian_w.torch, atol=1e-3), ( + "body_link_jacobian_w did not change after manual joint write; FK trigger likely missing" + ) + assert not torch.allclose(J_com_0, articulation.data.body_com_jacobian_w.torch, atol=1e-3), ( + "body_com_jacobian_w did not change after manual joint write; FK trigger likely missing" + ) + assert not torch.allclose(M_0, articulation.data.mass_matrix.torch, atol=1e-3), ( + "mass_matrix did not change after manual joint write; FK trigger likely missing" + ) + assert not torch.allclose(g_0, articulation.data.gravity_compensation_forces.torch, atol=1e-3), ( + "gravity_compensation_forces did not change after manual joint write; FK trigger likely missing" + ) + g_meas = articulation.data.gravity_compensation_forces.torch # Independent derivation: generalized gravity load tau_g = sum_b J_lin_b^T (m_b g_w); @@ -4060,55 +2997,53 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( torch.testing.assert_close(g_meas, g_expected, atol=1e-2, rtol=1e-3) - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) -@pytest.mark.isaacsim_ci -def test_dynamics_accessors_refresh_after_manual_joint_write(sim, num_articulations, device, articulation_type): - """After ``write_joint_position_to_sim_index`` (no sim step), every dynamics accessor must reflect the - new joint state, not the previous one. - - Catches a missing FK trigger before ``eval_jacobian`` / the COM->origin shift kernel - (:attr:`body_com_jacobian_w`, :attr:`body_link_jacobian_w`), before ``eval_mass_matrix`` - (``compute_body_spatial_inertia`` reads ``state.body_q``) and before the RNEA pass in - ``eval_inverse_dynamics_passive`` (:attr:`gravity_compensation_forces`). Gravity stays enabled - (the default): with gravity off ``g(q)`` is identically zero and its assertion would be vacuous. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - sim.step() + # Link-origin Jacobian contract. Reproducible non-trivial q_dot — large enough to drive omega + # well above the floor where COM offset effects would round into noise. The root is at rest so + # the actuated-only J slice predicts the whole body twist. + qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 + articulation.write_joint_velocity_to_sim_index(velocity=qdot) + if not articulation.is_fixed_base: + articulation.write_root_velocity_to_sim_index(root_velocity=torch.zeros(num_articulations, 6, device=device)) + # Refresh kinematics without integrating: for floating bases, an actuator step can + # introduce root motion that is intentionally absent from the actuated-only J slice. + sim.forward() articulation.update(sim.cfg.dt) - # Read every accessor at the baseline joint state. - q_baseline = articulation.data.joint_pos.torch.clone() - J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() - J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() - M_0 = articulation.data.mass_matrix.torch.clone() - g_0 = articulation.data.gravity_compensation_forces.torch.clone() + # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past + # them so the joint axis aligns with joint_vel (actuated-only). + J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :] + qdot_view = articulation.data.joint_vel.torch + v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) # (N, B_jac, 6) + v_pred_lin = v_pred[..., 0:3] + v_pred_ang = v_pred[..., 3:6] - # Manually write a different joint state (large delta so the change is visible) without a sim step - # or update, which marks FK stale (write_joint_position_to_sim sets _fk_timestamp = -1). - q_target = q_baseline + 0.5 - env_ids = wp.array([0], dtype=wp.int32, device=device) - articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + # Ground truth from Newton state. ``get_link_velocities`` returns shape + # (num_instances, 1, num_bodies, 6) — per-articulation grouping with + # one articulation per instance — so we squeeze the inner dim. + state = SimulationManager.get_state_0() + body_qd_view = wp.to_torch(articulation.root_view.get_link_velocities(state)).squeeze(1) + body_v_com = body_qd_view[..., :3] + body_omega = body_qd_view[..., 3:] - # With the FK trigger, forward() refreshes body_q to q_target before each accessor evaluates. - # Without it, body_q stays at the baseline and every accessor returns its previous value. - assert not torch.allclose(J_link_0, articulation.data.body_link_jacobian_w.torch, atol=1e-3), ( - "body_link_jacobian_w did not change after manual joint write; FK trigger likely missing" - ) - assert not torch.allclose(J_com_0, articulation.data.body_com_jacobian_w.torch, atol=1e-3), ( - "body_com_jacobian_w did not change after manual joint write; FK trigger likely missing" - ) - assert not torch.allclose(M_0, articulation.data.mass_matrix.torch, atol=1e-3), ( - "mass_matrix did not change after manual joint write; FK trigger likely missing" - ) - assert not torch.allclose(g_0, articulation.data.gravity_compensation_forces.torch, atol=1e-3), ( - "gravity_compensation_forces did not change after manual joint write; FK trigger likely missing" - ) + # World-frame COM-to-origin offset, derived from already-computed + # data layer outputs (avoids quaternion-convention pitfalls). + body_com_pos_w = articulation.data.body_com_pos_w.torch # (N, num_bodies, 3) + body_link_pos_w = articulation.data.body_link_pos_w.torch # (N, num_bodies, 3) + c_world = body_com_pos_w - body_link_pos_w + + if articulation.is_fixed_base: + body_v_com = body_v_com[:, 1:] + body_omega = body_omega[:, 1:] + c_world = c_world[:, 1:] + + # Expected v_origin = v_com - omega x c_world. + v_origin_expected = body_v_com - torch.cross(body_omega, c_world, dim=-1) + + # Tolerance: 5 mm absolute. The COM-offset bug produces a ~3 cm bias + # on the panda hand under the 0.5-rad/s injected qdot, well above + # this floor; numerical noise from kernel ordering stays under 1 mm. + torch.testing.assert_close(v_pred_ang, body_omega, atol=5e-3, rtol=1e-2) + torch.testing.assert_close(v_pred_lin, v_origin_expected, atol=5e-3, rtol=1e-2) @pytest.mark.parametrize("num_articulations", [1]) @@ -4197,83 +3132,6 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio ) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci -def test_franka_ik_tracking_accuracy(sim, device, articulation_type, gravity_enabled): - """Newton-side IK convergence sentinel. - - Runs a full IK tracking loop end-to-end through the new - ``robot.data.body_link_jacobian_w`` accessor and records the steady-state EE - pose error. With the robot teleported to its configured init_state - home pose and scene gravity off, Newton's IK converges to - machine-precision tracking (sub-mm). A bridge regression - (wrong-reference-frame Jacobian, missing COM->origin shift, DoF - mis-ordering) would push the steady-state error well above the - threshold below. - - The pose teleport is deliberate: the standalone test path does not - invoke a manager-based env reset (which is what normally pushes - :attr:`~isaaclab.assets.ArticulationData.default_joint_pos` to sim). - Without it, the robot starts at the URDF-neutral pose where the - Franka wrist axes nearly align (rank-deficient Jacobian) and DLS - plateaus at multi-cm error -- a kinematic-singularity artifact, not - a bridge or Newton issue. - - See ``test_get_jacobians_link_origin_contract`` (above) for the - sharper unit-level pin on the Jacobian's reference-point contract. - """ - robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim) - - sim.step() - robot.update(sim.cfg.dt) - target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device) - - ik = DifferentialIKController( - DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), - num_envs=1, - device=device, - ) - ik.set_command(target_pose_b) - - pos_history: list[float] = [] - rot_history: list[float] = [] - for _ in range(800): - jacobian = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) - ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids] - - joint_pos_des = ik.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) - - robot.set_joint_position_target(joint_pos_des, joint_ids=arm_joint_ids) - robot.write_data_to_sim() - sim.step() - robot.update(sim.cfg.dt) - - pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) - pos_history.append(pos_error.norm(dim=-1).max().item()) - rot_history.append(rot_error.norm(dim=-1).max().item()) - - pos_min, pos_mean = _summarize_history(pos_history) - rot_min, rot_mean = _summarize_history(rot_history) - - # Print metrics every run for stress-test capture. - print(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") - - # Regression sentinel: assert on tail mean rather than min. Tail - # min is the bottom of any oscillation envelope and can be tiny - # while the actual tracking error is much larger. With the - # configured home pose and scene gravity off, Newton converges to - # machine precision (sub-mm). The 5 mm bound absorbs any CUDA- - # kernel-ordering noise while remaining well below the "totally - # broken" regime: a bridge regression (wrong-frame Jacobian, - # missing COM->origin shift, DoF mis-ordering) would push the - # steady-state error well past this bound. - assert pos_mean < 5e-3, f"IK pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" - assert rot_mean < 5e-2, f"IK rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" - - @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["panda"]) @pytest.mark.parametrize("gravity_enabled", [False]) @@ -4283,8 +3141,7 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en Mirror of the existing PhysX-side OSC tests in :mod:`isaaclab.test.controllers.test_operational_space`, scoped to - Franka pose-abs tracking on Newton. Like the IK sentinel above, this - test exercises the full controller-bridge pipeline + Franka pose-abs tracking on Newton. This test exercises the full controller-bridge pipeline (:attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`) end-to-end and asserts a loose regression bound rather than a tight correctness @@ -4346,15 +3203,13 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en pos_history.append(pos_error.norm(dim=-1).max().item()) rot_history.append(rot_error.norm(dim=-1).max().item()) - pos_min, pos_mean = _summarize_history(pos_history) - rot_min, rot_mean = _summarize_history(rot_history) - - print(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + pos_mean = _tail_mean(pos_history) + rot_mean = _tail_mean(rot_history) # Regression sentinel: assert on tail mean rather than min. With # ``current_ee_vel_b = J · q_dot`` providing OSC's damping term and # the actuator PD zeroed, the impedance settles to machine - # precision -- same ballpark as the IK test. The 5 mm bound is a + # precision. The 5 mm bound is a # bridge regression sentinel: a wrong J, wrong mass matrix, or # DoF mis-ordering pushes the steady-state error well past it # because OSC consumes both ``body_link_jacobian_w`` and @@ -4472,8 +3327,6 @@ def _stationary_tail_mean(history, label): pos_off = _stationary_tail_mean(hist_off, "phase-1 sag") pos_on = _stationary_tail_mean(hist_on, "phase-2 hold") - print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.6f}") - # Re-validated on newton 81cdcfc2 / mujoco-warp 3.10.0.2 with 4 substeps: # pos_off ~= 0.024, pos_on ~= 1e-6. assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" diff --git a/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py b/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py index 06d516c1213..fcb4e7759de 100644 --- a/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py +++ b/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py @@ -16,8 +16,7 @@ def _selector(values: list[int], dtype: type) -> wp.array: return wp.array(values, dtype=dtype, device="cpu") -@pytest.mark.parametrize("env_dtype", [wp.int32, wp.int64]) -@pytest.mark.parametrize("joint_dtype", [wp.int32, wp.int64]) +@pytest.mark.parametrize(("env_dtype", "joint_dtype"), [(wp.int32, wp.int32), (wp.int64, wp.int64)]) def test_write_joint_limit_data_to_user_and_backend_index_accepts_index_dtypes( env_dtype: type, joint_dtype: type ) -> None: diff --git a/source/isaaclab_newton/test/assets/test_joint_coordinates.py b/source/isaaclab_newton/test/assets/test_joint_coordinates.py index 216ca4b7c5e..74d437408ed 100644 --- a/source/isaaclab_newton/test/assets/test_joint_coordinates.py +++ b/source/isaaclab_newton/test/assets/test_joint_coordinates.py @@ -33,16 +33,6 @@ def _map() -> BallJointCoordinateMap: return build_ball_joint_coordinate_map(COORD_COUNTS, DOF_COUNTS, "cpu") -def test_tables_cover_every_dof() -> None: - """Every DOF of every joint must be tabulated exactly once.""" - m = _map() - covered = list(m.single_dof.numpy()) + [b + k for b in m.ball_dof.numpy() for k in range(3)] - assert sorted(covered) == list(range(sum(DOF_COUNTS))) - assert sorted(list(m.single_coord.numpy()) + [b + k for b in m.ball_coord.numpy() for k in range(4)]) == list( - range(sum(COORD_COUNTS)) - ) - - def test_two_ball_tables_cover_every_dof() -> None: """A second ball joint's offsets are not a simple repeat of the first's.""" m = build_ball_joint_coordinate_map(TWO_BALL_COORD_COUNTS, TWO_BALL_DOF_COUNTS, "cpu") @@ -155,9 +145,9 @@ def test_unsupported_layout_is_rejected() -> None: build_ball_joint_coordinate_map([7], [6], "cpu") -@pytest.mark.parametrize("num_envs", [1, 2]) -def test_scatter_then_gather_round_trips(num_envs: int) -> None: - """DOF values survive a trip through coordinate space, at one environment and at two.""" +def test_scatter_then_gather_round_trips() -> None: + """DOF values survive a trip through coordinate space.""" + num_envs = 2 m = _map() n_dofs, n_coords = sum(DOF_COUNTS), sum(COORD_COUNTS) rng = np.random.default_rng(0) diff --git a/source/isaaclab_newton/test/assets/test_mpm_object.py b/source/isaaclab_newton/test/assets/test_mpm_object.py index bc54cd73ccf..57d0551989f 100644 --- a/source/isaaclab_newton/test/assets/test_mpm_object.py +++ b/source/isaaclab_newton/test/assets/test_mpm_object.py @@ -13,7 +13,7 @@ newton = pytest.importorskip("newton") -from isaaclab_newton.assets.mpm_object import MPMObjectCfg +from isaaclab_newton.assets.mpm_object import MPMObject, MPMObjectCfg from isaaclab_newton.physics import MPMSolverCfg, NewtonCfg, NewtonMPMManager from isaaclab_newton.sim.spawners.mpm import MPMGridCfg @@ -50,6 +50,7 @@ class MPMSceneCfg(InteractiveSceneCfg): sim.reset() media = scene["media"] + assert isinstance(media, MPMObject) assert media.num_instances == 2 assert media.particles_per_object == 1 assert media.data.particle_pos_w.torch.shape == (2, 1, 3) diff --git a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py index 264ce94b449..f338a342550 100644 --- a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py +++ b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py @@ -34,7 +34,7 @@ from isaaclab_newton.physics import NewtonManager as SimulationManager import isaaclab.sim as sim_utils -from isaaclab.actuators import IdealPDActuatorCfg +from isaaclab.actuators import DCMotorCfg, DelayedPDActuatorCfg, IdealPDActuatorCfg from isaaclab.actuators.newton import read_group_parameter from isaaclab.actuators.newton.kernels import sync_torque_telemetry from isaaclab.assets import AssetBaseCfg @@ -42,7 +42,6 @@ from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.test.utils.actuator_equivalence import ( CARTPOLE_EXPLICIT_ACTUATORS, - DC_MOTOR_ACTUATORS, DELAYED_PD_ACTUATORS, IDEAL_PD_ACTUATORS, IMPLICIT_ONLY_ACTUATORS, @@ -82,6 +81,32 @@ use_cuda_graph=False, ) +# The PD demand (~kp * TARGET_OFFSET = 4 N·m) exceeds the 2 N·m saturation effort, so the DC-motor +# torque-speed clamp binds and Newton must author it for the paths to match. +SATURATING_DC_MOTOR_ACTUATORS = { + "legs": DCMotorCfg( + joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], + saturation_effort=2.0, + actuator_effort_limit=80.0, + actuator_velocity_limit=7.5, + stiffness=40.0, + damping=5.0, + ), +} + +# Newton authors ``max_delay`` as a fixed delay, while the Lab path samples a lag in +# ``[min_delay, max_delay]`` on reset; a fixed delay makes both paths apply the same lag. +FIXED_DELAYED_PD_ACTUATORS = { + "legs": DelayedPDActuatorCfg( + joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], + stiffness=40.0, + damping=5.0, + actuator_effort_limit=80.0, + min_delay=2, + max_delay=2, + ), +} + # --------------------------------------------------------------------------- # Simulation runner # --------------------------------------------------------------------------- @@ -98,6 +123,7 @@ def _run_simulation( feedforward: float | None = None, joint_ordering: tuple[str, ...] | None = None, permutation_sensitive_commands: bool = False, + ramp_targets: bool = False, ) -> dict: """Run ANYmal-C and return recorded trajectories + telemetry. @@ -119,10 +145,11 @@ def _run_simulation( joint_ordering: Optional explicit public joint-name order. permutation_sensitive_commands: Whether to command distinct position, velocity, and effort values by physical joint name. + ramp_targets: Whether to ramp the position target over the rollout so command delays are observable. Returns: - Recorded joint-name metadata, commands, public trajectories and torque telemetry, and backend-order - adapter effort traces. + Recorded joint-name metadata, commands, public trajectories and torque telemetry, backend-order + adapter effort traces, and the Newton actuators built from the model. """ sim_cfg = SimulationCfg(dt=dt, physics=newton_cfg, use_newton_actuators=use_newton_actuators) with build_simulation_context( @@ -149,6 +176,8 @@ def _run_simulation( replicate(sim.get_clone_plan()) sim.reset() assert articulation.is_initialized + # Reset samples the Lab-path actuator delay; without it the Lab lag stays at zero. + articulation.reset() if use_newton_actuators and decimation > 1: SimulationManager.set_decimation(decimation) @@ -199,7 +228,10 @@ def _run_simulation( recorded_pos, recorded_vel = [], [] recorded_computed_effort, recorded_applied_effort = [], [] recorded_adapter_applied = [] - for _ in range(num_steps): + for step in range(num_steps): + if ramp_targets: + target_pos = init_pos + TARGET_OFFSET * (step + 1) / num_steps + articulation.set_joint_position_target_index(target=target_pos) if handles_dec: articulation.write_data_to_sim() sim.step() @@ -216,6 +248,17 @@ def _run_simulation( if use_newton_actuators: recorded_adapter_applied.append(wp.to_torch(articulation.data._sim_bind_joint_effort).clone()) + actuator_info = [] + if use_newton_actuators: + for act in SimulationManager.get_model().actuators: + actuator_info.append( + { + "controller_type": type(act.controller).__name__, + "clamping_types": sorted(type(c).__name__ for c in (act.clamping or [])), + "has_delay": act.delay is not None, + } + ) + return { "joint_names": joint_names, "backend_joint_names": backend_joint_names, @@ -229,19 +272,24 @@ def _run_simulation( "target_pos": target_pos.clone(), "target_vel": target_vel.clone(), "effort_target": None if effort_target is None else effort_target.clone(), + "actuator_info": actuator_info, } def test_newton_actuator_rollout_matches_reversed_joint_ordering() -> None: - """Match Newton-backend actuator traces under reversed public joint ordering.""" + """Match Newton-backend actuator traces under reversed public joint ordering. + + The implicit hip group reads its torque telemetry from backend-order effort buffers, so the + permutation-sensitive feedforward effort also checks that gather. + """ identity_result = _run_simulation( - IDEAL_PD_ACTUATORS, + MIXED_WITH_IMPLICIT_ACTUATORS, use_newton_actuators=True, permutation_sensitive_commands=True, ) requested_joint_names = tuple(reversed(identity_result["joint_names"])) reversed_result = _run_simulation( - IDEAL_PD_ACTUATORS, + MIXED_WITH_IMPLICIT_ACTUATORS, use_newton_actuators=True, joint_ordering=requested_joint_names, permutation_sensitive_commands=True, @@ -274,6 +322,7 @@ class _EquivalenceTestBase(EquivalenceAssertionsMixin, unittest.TestCase): newton_cfg: NewtonCfg = NEWTON_CFG num_steps: int = NUM_STEPS decimation: int = 1 + ramp_targets: bool = False @classmethod def setUpClass(cls): @@ -283,6 +332,7 @@ def setUpClass(cls): newton_cfg=cls.newton_cfg, num_steps=cls.num_steps, decimation=cls.decimation, + ramp_targets=cls.ramp_targets, ) cls.lab_result = _run_simulation(cls.actuators, use_newton_actuators=False, **kwargs) cls.newton_result = _run_simulation(cls.actuators, use_newton_actuators=True, **kwargs) @@ -293,18 +343,18 @@ def setUpClass(cls): # --------------------------------------------------------------------------- -class TestIdealPDEquivalence(_EquivalenceTestBase): - """IdealPDActuator on all 12 joints: Lab vs Newton.""" - - __test__ = True - actuators = IDEAL_PD_ACTUATORS - - class TestDCMotorEquivalence(_EquivalenceTestBase): - """DCMotor actuator on all 12 joints: Lab vs Newton.""" + """Saturating DCMotor actuator on all 12 joints: Lab vs Newton.""" __test__ = True - actuators = DC_MOTOR_ACTUATORS + actuators = SATURATING_DC_MOTOR_ACTUATORS + + def test_dc_motor_clamp_binds(self): + for result in (self.lab_result, self.newton_result): + assert any( + not torch.allclose(applied, computed) + for applied, computed in zip(result["applied_effort"], result["computed_effort"]) + ), "the DC-motor clamp never bound, so the equivalence cannot detect missing clamping" class TestMixedWithImplicitEquivalence(_EquivalenceTestBase): @@ -457,72 +507,6 @@ class TestRandomizeActuatorGainsViaEventsNewton(unittest.TestCase): ``K`` — so the assertions are deterministic. """ - def test_single_articulation(self): - sim_cfg = SimulationCfg(dt=DT, physics=NEWTON_CFG, use_newton_actuators=True) - with build_simulation_context( - device="cuda:0", - gravity_enabled=True, - add_ground_plane=True, - sim_cfg=sim_cfg, - ) as sim: - sim._app_control_on_stop_handle = None - sim_utils.create_prim("/World/Env_0", "Xform") - art_cfg = ANYMAL_C_CFG.replace( - actuators=IDEAL_PD_ACTUATORS, - prim_path="/World/Env_[^/]*/Robot", - ) - clone_plan_from_env_0( - CloneCfg(clone_template="/World/Env_{}"), - (art_cfg, AssetBaseCfg(prim_path="/World/defaultGroundPlane")), - NUM_ENVS, - 3.0, - positions=np.asarray([(i * 3.0, 0.0, 0.0) for i in range(NUM_ENVS)]), - ) - anymal = Articulation(art_cfg) - replicate(sim.get_clone_plan()) - sim.reset() - - adapter = SimulationManager._adapter - self.assertIsNotNone(adapter, "Newton adapter should exist with use_newton_actuators=True") - read = functools.partial(read_group_parameter, anymal.actuators) - n = anymal.num_joints - # Before DR, native gain reads must return the configured values for - # *every* env. IDEAL_PD_ACTUATORS covers all 12 joints with constant - # gains, so every cell of both env rows must equal the configured - # value. This is also the regression check for the env-major DOF - # stride decoding on floating-base articulations (ANYmal-C has 6 - # free-root DOFs + 12 joints -> a per-env stride of 18 vs. - # ``num_joints == 12``): a wrong stride corrupts every env past the - # first. - legs_stiffness_before = read("legs", "controller", "kp").clone() - legs_damping_before = read("legs", "controller", "kd").clone() - torch.testing.assert_close(legs_stiffness_before, torch.full((NUM_ENVS, n), 40.0, device=anymal.device)) - torch.testing.assert_close(legs_damping_before, torch.full((NUM_ENVS, n), 5.0, device=anymal.device)) - - env = MockEnv({"robot": anymal}, NUM_ENVS, anymal.device) - term, asset_cfg = build_dr_term(env, "robot") - env_ids = torch.tensor([0], device=anymal.device, dtype=torch.long) - - term( - env, - env_ids=env_ids, - asset_cfg=asset_cfg, - stiffness_distribution_params=(100.0, 100.0), - damping_distribution_params=(5.0, 5.0), - operation="abs", - distribution="uniform", - ) - - # Named native-group reads project the controller values immediately. - torch.testing.assert_close( - read("legs", "controller", "kp")[0], torch.full((n,), 100.0, device=anymal.device) - ) - torch.testing.assert_close(read("legs", "controller", "kd")[0], torch.full((n,), 5.0, device=anymal.device)) - # Other envs untouched. - for env_idx in range(1, NUM_ENVS): - torch.testing.assert_close(read("legs", "controller", "kp")[env_idx], legs_stiffness_before[env_idx]) - torch.testing.assert_close(read("legs", "controller", "kd")[env_idx], legs_damping_before[env_idx]) - def test_two_articulations(self): from isaaclab_assets import CARTPOLE_CFG # noqa: PLC0415 @@ -560,6 +544,13 @@ def test_two_articulations(self): cartpole_read = functools.partial(read_group_parameter, cartpole.actuators) anymal_stiffness_before = anymal_read("legs", "controller", "kp").clone() anymal_damping_before = anymal_read("legs", "controller", "kd").clone() + # Before DR, native gain reads must return the configured values for *every* env. This is + # also the regression check for the env-major DOF stride decoding on floating-base + # articulations (ANYmal-C has 6 free-root DOFs + 12 joints): a wrong stride corrupts every + # env past the first. + n = anymal.num_joints + torch.testing.assert_close(anymal_stiffness_before, torch.full((NUM_ENVS, n), 40.0, device=anymal.device)) + torch.testing.assert_close(anymal_damping_before, torch.full((NUM_ENVS, n), 5.0, device=anymal.device)) cartpole_stiffness_before = cartpole_read("all_joints", "controller", "kp").clone() cartpole_damping_before = cartpole_read("all_joints", "controller", "kd").clone() @@ -598,6 +589,31 @@ def test_two_articulations(self): cartpole_read("all_joints", "controller", "kd")[env_idx], cartpole_damping_before[env_idx] ) + # DR scoped to the floating-base ANYmal updates only its selected env. + anymal_term, anymal_asset_cfg = build_dr_term(env, "anymal") + anymal_term( + env, + env_ids=env_ids, + asset_cfg=anymal_asset_cfg, + stiffness_distribution_params=(100.0, 100.0), + damping_distribution_params=(5.0, 5.0), + operation="abs", + distribution="uniform", + ) + torch.testing.assert_close( + anymal_read("legs", "controller", "kp")[0], torch.full((n,), 100.0, device=anymal.device) + ) + torch.testing.assert_close( + anymal_read("legs", "controller", "kd")[0], torch.full((n,), 5.0, device=anymal.device) + ) + for env_idx in range(1, NUM_ENVS): + torch.testing.assert_close( + anymal_read("legs", "controller", "kp")[env_idx], anymal_stiffness_before[env_idx] + ) + torch.testing.assert_close( + anymal_read("legs", "controller", "kd")[env_idx], anymal_damping_before[env_idx] + ) + # --------------------------------------------------------------------------- # DelayedPD equivalence: PD with actuator command delay @@ -608,11 +624,19 @@ class TestDelayedPDEquivalence(_EquivalenceTestBase): """DelayedPDActuator on all 12 joints: Lab vs Newton. Verifies that actuator command delays are correctly authored as - ``NewtonActuatorDelayAPI`` and produce matching trajectories. + ``NewtonActuatorDelayAPI`` and produce matching trajectories. The ramped + position target makes a missing delay change the trajectory. """ __test__ = True - actuators = DELAYED_PD_ACTUATORS + actuators = FIXED_DELAYED_PD_ACTUATORS + ramp_targets = True + + def test_newton_actuators_are_delayed_pd(self): + self.assertTrue(self.newton_result["actuator_info"], "No Newton actuators were created") + for a in self.newton_result["actuator_info"]: + self.assertTrue(a["has_delay"], "Delay not found on delayed PD actuator") + self.assertEqual(a["controller_type"], "DrivePD") # --------------------------------------------------------------------------- @@ -644,14 +668,6 @@ class _DecimationMixin: decimation = 2 -class TestDecimationDCMotor(_DecimationMixin, TestDCMotorEquivalence): - """DCMotor — same equivalence checks, with decimation=2 + CUDA graph.""" - - -class TestDecimationDelayedPD(_DecimationMixin, TestDelayedPDEquivalence): - """DelayedPD — decimation=2 + CUDA graph (delay queue stepped inside the captured graph).""" - - # --------------------------------------------------------------------------- # Per-env reset: actuator state isolation # --------------------------------------------------------------------------- @@ -705,126 +721,31 @@ def _remotized_pd_actuators() -> dict: stiffness=60.0, damping=1.5, actuator_effort_limit=80.0, + min_delay=3, max_delay=3, joint_parameter_lookup=SPOT_KNEE_LOOKUP, ), } -def _run_authoring_introspection(actuator_cfgs: dict) -> dict: - """Instantiate Newton simulation, return Newton actuator introspection. - - Verifies that Lab configs are correctly authored to Newton USD schemas - and that Newton creates the expected controller/clamping/delay objects. - - Returns: - Dict with ``num_actuators``, ``actuator_info`` (list of per-actuator - dicts), and ``joint_pos`` (recorded trajectories). - """ - sim_cfg = SimulationCfg(dt=DT, physics=NEWTON_CFG, use_newton_actuators=True) - - with build_simulation_context( - device="cuda:0", - gravity_enabled=True, - add_ground_plane=True, - sim_cfg=sim_cfg, - ) as sim: - sim._app_control_on_stop_handle = None - - sim_utils.create_prim("/World/Env_0", "Xform") - - art_cfg = ANYMAL_C_CFG.replace( - actuators=actuator_cfgs, - prim_path="/World/Env_[^/]*/Robot", - ) - clone_plan_from_env_0( - CloneCfg(clone_template="/World/Env_{}"), - (art_cfg, AssetBaseCfg(prim_path="/World/defaultGroundPlane")), - NUM_ENVS, - 3.0, - positions=np.asarray([(i * 3.0, 0.0, 0.0) for i in range(NUM_ENVS)]), - ) - articulation = Articulation(art_cfg) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - model = SimulationManager.get_model() - - actuator_info = [] - for act in model.actuators: - ctrl_type = type(act.controller).__name__ - clamp_types = sorted(type(c).__name__ for c in (act.clamping or [])) - actuator_info.append( - { - "controller_type": ctrl_type, - "clamping_types": clamp_types, - "has_delay": act.delay is not None, - "num_indices": len(act.indices), - } - ) - - init_pos = wp.to_torch(articulation.data.joint_pos).clone() - target_pos = init_pos + TARGET_OFFSET - target_vel = torch.zeros_like(init_pos) - articulation.set_joint_position_target_index(target=target_pos) - articulation.set_joint_velocity_target_index(target=target_vel) - - recorded_pos = [] - for _ in range(NUM_STEPS): - articulation.write_data_to_sim() - sim.step() - articulation.update(DT) - recorded_pos.append(wp.to_torch(articulation.data.joint_pos).clone()) - - return { - "num_actuators": len(model.actuators), - "actuator_info": actuator_info, - "joint_pos": recorded_pos, - } - - -class TestRemotizedPDAuthoring(unittest.TestCase): - """Verify RemotizedPDActuatorCfg is authored as Newton PD + delay + - position-based clamping. - - Uses the Spot knee lookup table on ANYmal's KFE joints, with IdealPD - on HAA and HFE joints. - """ - - @classmethod - def setUpClass(cls): - cls.result = _run_authoring_introspection(_remotized_pd_actuators()) - - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_kfe_controller_is_pd(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - self.assertTrue(len(kfe_acts) > 0, "No actuator with position-based clamping found") - for a in kfe_acts: - self.assertEqual(a["controller_type"], "DrivePD") - - def test_kfe_has_position_based_clamping(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - self.assertTrue(len(kfe_acts) > 0, "Position-based clamping not found") - - def test_kfe_has_delay(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - for a in kfe_acts: - self.assertTrue(a["has_delay"], "Delay not found on remotized KFE actuator") - - class TestRemotizedPDEquivalence(_EquivalenceTestBase): """RemotizedPD (PD + delay + position-based clamping): Lab vs Newton.""" __test__ = True + ramp_targets = True @classmethod def setUpClass(cls): cls.actuators = _remotized_pd_actuators() super().setUpClass() + def test_newton_knee_actuators_are_delayed_position_clamped_pd(self): + kfe_acts = [a for a in self.newton_result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] + self.assertTrue(len(kfe_acts) > 0, "No actuator with position-based clamping found") + for a in kfe_acts: + self.assertEqual(a["controller_type"], "DrivePD") + self.assertTrue(a["has_delay"], "Delay not found on remotized KFE actuator") + class TestDecimationRemotizedPD(_DecimationMixin, TestRemotizedPDEquivalence): """RemotizedPD — decimation=2 + CUDA graph.""" @@ -835,17 +756,18 @@ class TestDecimationRemotizedPD(_DecimationMixin, TestRemotizedPDEquivalence): # --------------------------------------------------------------------------- -class TestNeuralMLPAuthoring(unittest.TestCase): - """Verify ActuatorNetMLPCfg is authored as Newton NeuralMLP controller - with DC motor clamping. +class TestNeuralActuatorAuthoring(unittest.TestCase): + """Verify ActuatorNetMLPCfg and ActuatorNetLSTMCfg are authored as Newton neural + controllers with DC motor clamping and run on the Newton backend. """ @classmethod def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetMLPCfg # noqa: PLC0415 + from isaaclab.actuators.actuator_net_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg # noqa: PLC0415 cls.mlp_path = make_dummy_mlp_checkpoint() - cls.result = _run_authoring_introspection( + cls.lstm_path = make_dummy_lstm_checkpoint() + cls.result = _run_simulation( { "mlp_legs": ActuatorNetMLPCfg( joint_names_expr=[".*HAA"], @@ -859,124 +781,43 @@ def setUpClass(cls): input_order="pos_vel", input_idx=[0, 1, 2], ), - "pd_legs": IdealPDActuatorCfg( - joint_names_expr=[".*HFE", ".*KFE"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ), - } - ) - - @classmethod - def tearDownClass(cls): - os.unlink(cls.mlp_path) - - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_has_neural_mlp_controller(self): - mlp_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralMLP"] - self.assertTrue(len(mlp_acts) > 0, "No NeuralMLP controller found") - - def test_mlp_has_dc_motor_clamping(self): - mlp_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralMLP"] - for a in mlp_acts: - self.assertIn("ClampingDCMotor", a["clamping_types"]) - - -class TestNeuralLSTMAuthoring(unittest.TestCase): - """Verify ActuatorNetLSTMCfg is authored as Newton NeuralLSTM controller - with DC motor clamping. - """ - - @classmethod - def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetLSTMCfg # noqa: PLC0415 - - cls.lstm_path = make_dummy_lstm_checkpoint() - cls.result = _run_authoring_introspection( - { "lstm_legs": ActuatorNetLSTMCfg( - joint_names_expr=[".*HAA"], + joint_names_expr=[".*HFE"], network_file=cls.lstm_path, saturation_effort=120.0, actuator_effort_limit=80.0, actuator_velocity_limit=7.5, ), "pd_legs": IdealPDActuatorCfg( - joint_names_expr=[".*HFE", ".*KFE"], + joint_names_expr=[".*KFE"], stiffness=40.0, damping=5.0, actuator_effort_limit=80.0, ), - } + }, + use_newton_actuators=True, ) @classmethod def tearDownClass(cls): + os.unlink(cls.mlp_path) os.unlink(cls.lstm_path) - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_has_neural_lstm_controller(self): - lstm_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralLSTM"] - self.assertTrue(len(lstm_acts) > 0, "No NeuralLSTM controller found") + def test_mlp_has_dc_motor_clamping(self): + mlp_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralMLP"] + self.assertTrue(len(mlp_acts) > 0, "No NeuralMLP controller found") + for a in mlp_acts: + self.assertIn("ClampingDCMotor", a["clamping_types"]) def test_lstm_has_dc_motor_clamping(self): lstm_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralLSTM"] + self.assertTrue(len(lstm_acts) > 0, "No NeuralLSTM controller found") for a in lstm_acts: self.assertIn("ClampingDCMotor", a["clamping_types"]) - -def test_sync_torque_telemetry_reads_backend_effort_buffers_in_user_order() -> None: - """Report torque telemetry in public joint order from backend-order effort buffers.""" - joint_pos = wp.zeros((1, 3), dtype=wp.float32, device="cpu") - joint_vel = wp.zeros_like(joint_pos) - joint_pos_target = wp.zeros_like(joint_pos) - joint_vel_target = wp.zeros_like(joint_pos) - joint_stiffness = wp.zeros_like(joint_pos) - joint_damping = wp.zeros_like(joint_pos) - effort_limit = wp.full((1, 3), 1000.0, dtype=wp.float32, device="cpu") - joint_modes = wp.array(np.asarray([0, 1, 0], dtype=np.int32), dtype=wp.int32, device="cpu") - user_to_backend = wp.array(np.asarray([2, 0, 1], dtype=np.int32), dtype=wp.int32, device="cpu") - sim_bind_joint_effort = wp.array( - np.asarray([[100.0, 200.0, 300.0]], dtype=np.float32), - dtype=wp.float32, - device="cpu", - ) - actuator_computed_effort = wp.array( - np.asarray([[10.0, 20.0, 30.0]], dtype=np.float32), - dtype=wp.float32, - device="cpu", - ) - computed = wp.zeros_like(joint_pos) - applied = wp.zeros_like(joint_pos) - - wp.launch( - sync_torque_telemetry, - dim=joint_pos.shape, - inputs=[ - joint_pos, - joint_vel, - joint_pos_target, - joint_vel_target, - joint_stiffness, - joint_damping, - effort_limit, - joint_modes, - sim_bind_joint_effort, - actuator_computed_effort, - user_to_backend, - True, - ], - outputs=[computed, applied], - device="cpu", - ) - - np.testing.assert_allclose(computed.numpy(), np.asarray([[30.0, 100.0, 20.0]], dtype=np.float32)) - np.testing.assert_allclose(applied.numpy(), np.asarray([[300.0, 100.0, 200.0]], dtype=np.float32)) + def test_positions_finite(self): + for step_i, pos in enumerate(self.result["joint_pos"]): + self.assertTrue(torch.isfinite(pos).all(), f"Non-finite positions at step {step_i}") def test_sync_torque_telemetry_keeps_user_order_effort_buffers_unmapped() -> None: diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 01a4de48860..4bd2d9aacad 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -10,7 +10,7 @@ """Launch Isaac Sim Simulator first.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices +from isaaclab.test.utils import DeviceScope, resolve_test_sim_device, test_devices # launch omniverse app simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app @@ -18,6 +18,7 @@ """Rest everything follows.""" import sys +from types import SimpleNamespace from typing import Literal import numpy as np @@ -34,11 +35,12 @@ import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg, RigidObjectCfg from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate +from isaaclab.envs.mdp.events import randomize_rigid_body_material +from isaaclab.managers import EventTermCfg, SceneEntityCfg from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab.utils.math import ( combine_frame_transforms, - default_orientation, quat_apply_inverse, quat_inv, quat_mul, @@ -127,41 +129,6 @@ def generate_cubes_scene( return cube_object, torch.as_tensor(origins, device=device) -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization(num_cubes, device): - """Test initialization for prim with rigid body API at the provided prim path.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Check if object is initialized - assert cube_object.is_initialized - assert len(cube_object.body_names) == 1 - - # Check buffers that exists and have correct shapes - assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) - assert cube_object.data.root_quat_w.torch.shape == (num_cubes, 4) - assert cube_object.data.body_mass.torch.shape == (num_cubes, 1) - assert cube_object.data.body_inertia.torch.shape == (num_cubes, 1, 9) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("api", ["none", "articulation_root"]) def test_initialization_rejects_non_rigid_body_prims(api): @@ -174,121 +141,78 @@ def test_initialization_rejects_non_rigid_body_prims(api): assert sys.getrefcount(cube_object) < 10 replicate(sim.get_clone_plan()) - with pytest.raises(RuntimeError): + with pytest.raises(RuntimeError, match="Expected 1 prims at"): sim.reset() -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(device): - """Test if external force buffer correctly updates in the force value is zero case. - - In this test, we apply a non-zero force, then a zero force, then finally a non-zero force - to an object. We check if the force buffer is properly updated at each step. - """ - - # Generate cubes scene - with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=1, device=device, add_ground_plane=True) - - # play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # find bodies to apply the force - body_ids, body_names = cube_object.find_bodies(".*") - - # reset object - cube_object.reset() - - # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - - # check if the cube's force and torque buffers are correctly updated - for i in range(cube_object.num_instances): - assert cube_object._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force - assert cube_object._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force - - # Check if the instantaneous wrench is correctly added to the permanent wrench - cube_object.permanent_wrench_composer.add_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [4]) @pytest.mark.parametrize("device", test_devices()) def test_external_force_on_single_body(num_cubes, device): - """Test application of external force on the base of the object. + """Test initialization and external forces on the base of the object. - In this test, we apply a force equal to the weight of an object on the base of + In the first phase, we apply a force equal to the weight of an object on the base of one of the objects. We check that the object does not move. For the other object, - we do not apply any force and check that it falls down. + we do not apply any force and check that it falls down. In the second phase, the force + is applied at 1m in the Y direction and the object must rotate around its X axis. - We validate that this works when we apply the force in the global frame and in the local frame. + We validate that this works when we apply the force in the global frame and in the local frame, + and that resetting the object clears the wrench applied in the previous iteration. """ # Generate cubes scene with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device, add_ground_plane=True) + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(cube_object) < 10 + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() + # Check if object is initialized + assert cube_object.is_initialized + assert len(cube_object.body_names) == 1 + + # Check buffers that exists and have correct shapes + assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) + assert cube_object.data.root_quat_w.torch.shape == (num_cubes, 4) + assert cube_object.data.body_mass.torch.shape == (num_cubes, 1) + assert cube_object.data.body_inertia.torch.shape == (num_cubes, 1, 9) + # Find bodies to apply the force body_ids, body_names = cube_object.find_bodies(".*") + def reset_cubes(): + # reset root state; shift the cubes to their origins so they are not on top of each other + root_pose = cube_object.data.default_root_pose.torch.clone() + root_pose[:, :3] = origins + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + cube_object.write_root_velocity_to_sim_index(root_velocity=cube_object.data.default_root_vel.torch.clone()) + cube_object.reset() + + # Reset should zero external forces and torques + assert not cube_object.instantaneous_wrench_composer.active + assert not cube_object.permanent_wrench_composer.active + assert torch.count_nonzero(cube_object.instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object.instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(cube_object.permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object.permanent_wrench_composer.out_torque_b.torch) == 0 + + def simulate(): + for _ in range(5): + cube_object.write_data_to_sim() + sim.step() + cube_object.update(sim.cfg.dt) + # Sample a force equal to the weight of the object external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) # Every 2nd cube should have a force applied to it external_wrench_b[0::2, :, 2] = 9.81 * cube_object.data.body_mass.torch[0] - # Now we are ready! - for i in range(5): - # reset root state - root_pose = cube_object.data.default_root_pose.torch.clone() - root_vel = cube_object.data.default_root_vel.torch.clone() - - # need to shift the position of the cubes otherwise they will be on top of each other - root_pose[:, :3] = origins - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # reset object - cube_object.reset() + for i in range(2): + reset_cubes() is_global = False if i % 2 == 0: @@ -305,16 +229,7 @@ def test_external_force_on_single_body(num_cubes, device): body_ids=body_ids, is_global=is_global, ) - # perform simulation - for _ in range(5): - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) + simulate() # First object should still be at the same Z position (1.0) torch.testing.assert_close( @@ -323,50 +238,14 @@ def test_external_force_on_single_body(num_cubes, device): # Second object should have fallen, so it's Z height should be less than initial height of 1.0 assert torch.all(cube_object.data.root_pos_w.torch[1::2, 2] < 1.0) - -@pytest.mark.parametrize("num_cubes", [4]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(num_cubes, device): - """Test application of external force on the base of the object at a specific position. - - In this test, we apply a force equal to the weight of an object on the base of - one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. - For the other object, we do not apply any force and check that it falls down. - - We validate that this works when we apply the force in the global frame and in the local frame. - """ - # Generate cubes scene - with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device, add_ground_plane=True) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, body_names = cube_object.find_bodies(".*") - - # Sample a force equal to the weight of the object + # Apply a force at 1m in the Y direction on every 2nd cube external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) external_wrench_positions_b = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) - # Every 2nd cube should have a force applied to it external_wrench_b[0::2, :, 2] = 50.0 external_wrench_positions_b[0::2, :, 1] = 1.0 - # Now we are ready! - for i in range(5): - # reset root state - root_pose = cube_object.data.default_root_pose.torch.clone() - root_vel = cube_object.data.default_root_vel.torch.clone() - - # need to shift the position of the cubes otherwise they will be on top of each other - root_pose[:, :3] = origins - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # reset object - cube_object.reset() + for i in range(2): + reset_cubes() is_global = False if i % 2 == 0: @@ -396,16 +275,7 @@ def test_external_force_on_single_body_at_position(num_cubes, device): body_ids=body_ids, is_global=is_global, ) - # perform simulation - for _ in range(5): - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) + simulate() # The first object should be rotating around it's X axis assert torch.all(torch.abs(cube_object.data.root_ang_vel_b.torch[0::2, 0]) > 0.1) @@ -416,114 +286,54 @@ def test_external_force_on_single_body_at_position(num_cubes, device): @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -def test_set_rigid_object_state(num_cubes, device): - """Test setting the state of the rigid object. - - In this test, we set the state of the rigid object to a random state and check - that the object is in that state after simulation. We set gravity to zero as - we don't want any external forces acting on the object to ensure state remains static. - """ - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: +def test_rigid_body_set_material_properties(num_cubes, device): + """Material randomization writes friction and restitution into the Newton model shapes of the selected envs.""" + with _newton_sim_context(device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - state_types = ["root_pos_w", "root_quat_w", "root_lin_vel_w", "root_ang_vel_w"] - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in state_types: - state_dict = { - "root_pos_w": torch.zeros_like(cube_object.data.root_pos_w.torch, device=sim.device), - "root_quat_w": default_orientation(num=num_cubes, device=sim.device), - "root_lin_vel_w": torch.zeros_like(cube_object.data.root_lin_vel_w.torch, device=sim.device), - "root_ang_vel_w": torch.zeros_like(cube_object.data.root_ang_vel_w.torch, device=sim.device), - } + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device, add_ground_plane=True) - # Now we are ready! - for _ in range(5): - # reset object - cube_object.reset() - - # Set random state - if state_type_to_randomize == "root_quat_w": - state_dict[state_type_to_randomize] = random_orientation(num=num_cubes, device=sim.device) - else: - state_dict[state_type_to_randomize] = torch.randn(num_cubes, 3, device=sim.device) - - # perform simulation - for _ in range(5): - root_pose = torch.cat( - [state_dict["root_pos_w"], state_dict["root_quat_w"]], - dim=-1, - ) - root_vel = torch.cat( - [state_dict["root_lin_vel_w"], state_dict["root_ang_vel_w"]], - dim=-1, - ) - # reset root state - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - sim.step() - - # assert that set root quantities are equal to the ones set in the state_dict - for key, expected_value in state_dict.items(): - value = getattr(cube_object.data, key).torch - # Newton reads state directly from sim (not cached), so post-step drift - # from velocity integration causes larger differences than PhysX - torch.testing.assert_close(value, expected_value, rtol=1e-1, atol=1e-1) - - cube_object.update(sim.cfg.dt) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset_rigid_object(num_cubes, device): - """Test resetting the state of the rigid object.""" - with _newton_sim_context(device, gravity_enabled=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator + # Play sim replicate(sim.get_clone_plan()) sim.reset() - for i in range(5): - # perform rendering - sim.step() - - # update object - cube_object.update(sim.cfg.dt) - - # Move the object to a random position - root_pose = cube_object.data.default_root_pose.torch.clone() - root_pose[:, :3] = torch.randn(num_cubes, 3, device=sim.device) - - # Random orientation - root_pose[:, 3:7] = random_orientation(num=num_cubes, device=sim.device) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - root_vel = cube_object.data.default_root_vel.torch.clone() - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) + # Resolve each cube's shapes from the flat Newton model, independent of the asset's view binding. + model = SimulationManager.get_model() + body_world = model.body_world.numpy() + shape_body = model.shape_body.numpy() + cube_shapes = [ + np.flatnonzero(np.isin(shape_body, np.flatnonzero(body_world == index))) for index in range(num_cubes) + ] + assert all(len(shapes) > 0 for shapes in cube_shapes) + original_mu = model.shape_material_mu.numpy().copy() + original_restitution = model.shape_material_restitution.numpy().copy() + + # Randomize the materials of the last cube through the event term, with degenerate ranges. + env = SimpleNamespace(scene={"cube": cube_object}, sim=sim, device=device, num_envs=num_cubes) + params = { + "static_friction_range": (0.55, 0.55), + "dynamic_friction_range": (0.55, 0.55), + "restitution_range": (0.15, 0.15), + "num_buckets": 1, + "asset_cfg": SceneEntityCfg("cube"), + } + term = randomize_rigid_body_material( + EventTermCfg(func=randomize_rigid_body_material, mode="startup", params=params), env + ) + term(env, torch.tensor([num_cubes - 1], device=device), **params) - if i % 2 == 0: - # reset object - cube_object.reset() + # Simulate physics + sim.step() + cube_object.update(sim.cfg.dt) - # Reset should zero external forces and torques - assert not cube_object._instantaneous_wrench_composer.active - assert not cube_object._permanent_wrench_composer.active - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_torque_b.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_torque_b.torch) == 0 + mu = model.shape_material_mu.numpy() + restitution = model.shape_material_restitution.numpy() + np.testing.assert_allclose(mu[cube_shapes[-1]], 0.55) + np.testing.assert_allclose(restitution[cube_shapes[-1]], 0.15) + # Shapes of the other cubes are untouched. + for shapes in cube_shapes[:-1]: + np.testing.assert_array_equal(mu[shapes], original_mu[shapes]) + np.testing.assert_array_equal(restitution[shapes], original_restitution[shapes]) @pytest.mark.isaacsim_ci @@ -607,48 +417,9 @@ def test_rigid_body_set_mass(num_cubes, device): torch.testing.assert_close(masses, masses_to_check) -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [True, False]) -def test_gravity_vec_w(num_cubes, device, gravity_enabled): - """Test that gravity vector direction is set correctly for the rigid object.""" - with _newton_sim_context(device, gravity_enabled=gravity_enabled) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Obtain gravity direction - if gravity_enabled: - expected_g = (0.0, 0.0, -9.81) - else: - expected_g = (0.0, 0.0, 0.0) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Check that gravity is set correctly - torch.testing.assert_close(cube_object.data.GRAVITY_VEC_W.torch[0], torch.tensor(expected_g, device=device)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Expected gravity value is the acceleration of the body - gravity = torch.zeros(num_cubes, 1, 6, device=device) - if gravity_enabled: - gravity[:, :, 2] = -9.81 - # Check the body accelerations are correct - torch.testing.assert_close(cube_object.data.body_acc_w.torch, gravity) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [3]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_gravity_vec_w_tracks_model_gravity(num_cubes, device): """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. @@ -662,6 +433,19 @@ def test_gravity_vec_w_tracks_model_gravity(num_cubes, device): replicate(sim.get_clone_plan()) sim.reset() + # Check that gravity is set correctly + torch.testing.assert_close( + cube_object.data.GRAVITY_VEC_W.torch[0], torch.tensor((0.0, 0.0, -9.81), device=device) + ) + + # The free-falling cubes accelerate with gravity and keep their identity orientation. + for _ in range(2): + sim.step() + cube_object.update(sim.cfg.dt) + gravity = torch.zeros(num_cubes, 1, 6, device=device) + gravity[:, :, 2] = -9.81 + torch.testing.assert_close(cube_object.data.body_acc_w.torch, gravity) + # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. model = SimulationManager.get_model() model_gravity_arr = model.gravity[: model.world_count] @@ -692,9 +476,8 @@ def test_gravity_vec_w_tracks_model_gravity(num_cubes, device): @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) @flaky(max_runs=3, min_passes=1) -def test_body_root_state_properties(num_cubes, device, with_offset): +def test_body_root_state_properties(num_cubes, device): """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -709,10 +492,7 @@ def test_body_root_state_properties(num_cubes, device, with_offset): assert cube_object.is_initialized # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) # Set center of mass offset via Newton API (position only, no quaternion) com_pos = offset.unsqueeze(1) # (N, 1, 3) @@ -746,68 +526,61 @@ def test_body_root_state_properties(num_cubes, device, with_offset): body_com_pose_w = cube_object.data.body_com_pose_w.torch body_com_vel_w = cube_object.data.body_com_vel_w.torch - # if offset is [0,0,0] all root_state_%_w will match and all body_%_w will match - if not with_offset: - torch.testing.assert_close(root_link_pose_w, root_com_pose_w) - torch.testing.assert_close(root_com_vel_w, root_link_vel_w) - torch.testing.assert_close(root_link_pose_w, body_link_pose_w.squeeze(-2)) - torch.testing.assert_close(root_com_vel_w, root_link_vel_w) - torch.testing.assert_close(body_link_pose_w, body_com_pose_w) - torch.testing.assert_close(body_com_vel_w, body_link_vel_w) - torch.testing.assert_close(root_com_pose_w, body_com_pose_w.squeeze(-2)) - torch.testing.assert_close(body_com_vel_w, body_link_vel_w) - else: - # cubes are spinning around center of mass - # position will not match - # center of mass position will be constant (i.e. spinning around com) - _tol = dict(atol=2e-3, rtol=2e-3) - torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3], **_tol) - torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2), **_tol) - # link position will be moving but should stay constant away from center of mass - root_link_state_pos_rel_com = quat_apply_inverse( - root_link_pose_w[..., 3:], - root_link_pose_w[..., :3] - root_com_pose_w[..., :3], - ) - torch.testing.assert_close(-offset, root_link_state_pos_rel_com, **_tol) - body_link_state_pos_rel_com = quat_apply_inverse( - body_link_pose_w[..., 3:], - body_link_pose_w[..., :3] - body_com_pose_w[..., :3], - ) - torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2), **_tol) - - # orientation of com will be a constant rotation from link orientation - com_quat_b = cube_object.data.body_com_quat_b.torch - com_quat_w = quat_mul(body_link_pose_w[..., 3:], com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:], **_tol) - torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:], **_tol) - - # root and body link orientations describe the same rigid body - torch.testing.assert_close(root_link_pose_w[..., 3:], body_link_pose_w[..., 3:].squeeze(-2), **_tol) - - # lin_vel will not match - # center of mass vel will be constant (i.e. spinning around com) - torch.testing.assert_close(torch.zeros_like(root_com_vel_w[..., :3]), root_com_vel_w[..., :3], **_tol) - torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3], **_tol) - # link frame will be moving, and should be equal to input angular velocity cross offset - lin_vel_rel_root_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_link_vel_w[..., :3]) - lin_vel_rel_body_gt = quat_apply_inverse(body_link_pose_w[..., 3:], body_link_vel_w[..., :3]) - lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_cubes, 1)[..., 3:], -offset) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, **_tol) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), **_tol) - - # ang_vel will always match - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(root_com_vel_w[..., 3:], body_com_vel_w[..., 3:].squeeze(-2)) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + _tol = dict(atol=2e-3, rtol=2e-3) + torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3], **_tol) + torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2), **_tol) + # link position will be moving but should stay constant away from center of mass + root_link_state_pos_rel_com = quat_apply_inverse( + root_link_pose_w[..., 3:], + root_link_pose_w[..., :3] - root_com_pose_w[..., :3], + ) + torch.testing.assert_close(-offset, root_link_state_pos_rel_com, **_tol) + body_link_state_pos_rel_com = quat_apply_inverse( + body_link_pose_w[..., 3:], + body_link_pose_w[..., :3] - body_com_pose_w[..., :3], + ) + torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2), **_tol) + + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.body_com_quat_b.torch + com_quat_w = quat_mul(body_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:], **_tol) + torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:], **_tol) + + # root and body link orientations describe the same rigid body + torch.testing.assert_close(root_link_pose_w[..., 3:], body_link_pose_w[..., 3:].squeeze(-2), **_tol) + + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close(torch.zeros_like(root_com_vel_w[..., :3]), root_com_vel_w[..., :3], **_tol) + torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3], **_tol) + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_root_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_link_vel_w[..., :3]) + lin_vel_rel_body_gt = quat_apply_inverse(body_link_pose_w[..., 3:], body_link_vel_w[..., :3]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, **_tol) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), **_tol) + + # ang_vel will always match + torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) + torch.testing.assert_close(root_com_vel_w[..., 3:], body_com_vel_w[..., 3:].squeeze(-2)) + torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) @pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) +@pytest.mark.parametrize( + ("num_cubes", "state_location"), [(2, "com"), (2, "link"), (2, "root"), (1, "root")] +) # num_cubes=1 covers single-instance initialization @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -def test_write_root_state(num_cubes, device, with_offset, state_location): - """Test the setters for root_state using both the link frame and center of mass as reference frame.""" +def test_write_root_state(num_cubes, device, state_location): + """Test the root state setters in the center-of-mass frame, the link frame, and the default root frames. + + A write must be readable in the written frame and refresh the derived frame and the body-frame caches + without a sim step, and the written state must persist into the solver across a step. + """ with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Create a scene with random cubes @@ -822,10 +595,7 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): assert cube_object.is_initialized # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) # Set center of mass offset via Newton API (position only) com_pos = offset.unsqueeze(1) # (N, 1, 3) @@ -836,34 +606,38 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): # check center of mass has been set torch.testing.assert_close(cube_object.data.body_com_pos_b.torch.squeeze(1), offset) - rand_state = torch.zeros(num_cubes, 13, device=device) - rand_state[..., :7] = cube_object.data.default_root_pose.torch - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - for i in range(10): + for i in range(2): # perform step sim.step() # update buffers cube_object.update(sim.cfg.dt) + # A target state distinct from the current one in position, orientation, and velocity. + target_pose = torch.cat( + [env_pos + 0.3 * torch.rand(num_cubes, 3, device=device), random_orientation(num_cubes, device)], + dim=-1, + ) + target_vel = torch.randn(num_cubes, 6, device=device) + + # Prime the lazily-derived caches at the current sim timestamp. Without this they would + # recompute on first access after the write regardless of invalidation; priming them makes a + # missing reset_pose/reset_velocity observable as a stale read in the assertions below. + _ = cube_object.data.root_link_pose_w.torch + _ = cube_object.data.root_com_pose_w.torch + _ = cube_object.data.root_link_vel_w.torch + _ = cube_object.data.root_com_vel_w.torch + + # Alternate between the default and explicit environment selectors. + env_ids = None if i == 0 else env_idx if state_location == "com": - if i % 2 == 0: - cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) + cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + cube_object.write_root_com_velocity_to_sim_index(root_velocity=target_vel, env_ids=env_ids) elif state_location == "link": - if i % 2 == 0: - cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - cube_object.write_root_link_velocity_to_sim_index( - root_velocity=rand_state[..., 7:], env_ids=env_idx - ) + cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + cube_object.write_root_link_velocity_to_sim_index(root_velocity=target_vel, env_ids=env_ids) + elif state_location == "root": + cube_object.write_root_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + cube_object.write_root_velocity_to_sim_index(root_velocity=target_vel, env_ids=env_ids) # Snapshot the body-frame caches *before* reading the root-frame caches: touching a # root cache lazily recomputes the shared buffer and would mask a stale body cache. @@ -871,150 +645,64 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): # body_com_pose_w returned the pre-write buffer after a link-frame pose write). body_link_pose_w = cube_object.data.body_link_pose_w.torch.squeeze(1).clone() body_com_pose_w = cube_object.data.body_com_pose_w.torch.squeeze(1).clone() - body_link_vel_w = cube_object.data.body_link_vel_w.torch.squeeze(1).clone() body_com_vel_w = cube_object.data.body_com_vel_w.torch.squeeze(1).clone() + root_link_pose_w = cube_object.data.root_link_pose_w.torch + root_com_pose_w = cube_object.data.root_com_pose_w.torch + root_link_vel_w = cube_object.data.root_link_vel_w.torch + root_com_vel_w = cube_object.data.root_com_vel_w.torch + body_com_pose_b = cube_object.data.body_com_pose_b.torch if state_location == "com": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.root_com_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.root_com_vel_w.torch) - elif state_location == "link": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.root_link_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.root_link_vel_w.torch) + torch.testing.assert_close(target_pose, root_com_pose_w) + torch.testing.assert_close(target_vel, root_com_vel_w) + # the com pose was written, so the derived link pose must be refreshed + expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( + root_com_pose_w[:, :3], + root_com_pose_w[:, 3:], + quat_rotate(quat_inv(body_com_pose_b[:, 0, 3:7]), -body_com_pose_b[:, 0, :3]), + quat_inv(body_com_pose_b[:, 0, 3:7]), + ) + expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) + torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) + else: + torch.testing.assert_close(target_pose, root_link_pose_w) + # the root velocity is the center-of-mass velocity + written_vel_w = root_link_vel_w if state_location == "link" else root_com_vel_w + torch.testing.assert_close(target_vel, written_vel_w) + # the link pose was written, so the derived com pose must be refreshed + expected_com_pos, expected_com_quat = combine_frame_transforms( + root_link_pose_w[:, :3], + root_link_pose_w[:, 3:], + body_com_pose_b[:, 0, :3], + body_com_pose_b[:, 0, 3:7], + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) + torch.testing.assert_close(expected_com_pose, root_com_pose_w) + # skip lin_vel because it differs between the frames; angular velocity is frame-independent + # and only matches when the derived velocity was actually refreshed after the write + torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) # For a single-body rigid object the body-frame caches are exactly the root-frame # caches reshaped, so they must stay consistent after a write without a sim step. - torch.testing.assert_close(cube_object.data.root_link_pose_w.torch, body_link_pose_w) - torch.testing.assert_close(cube_object.data.root_com_pose_w.torch, body_com_pose_w) - torch.testing.assert_close(cube_object.data.root_link_vel_w.torch, body_link_vel_w) - torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, body_com_vel_w) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) -@pytest.mark.parametrize("state_location", ["com", "link", "root"]) -def test_write_state_functions_data_consistency(num_cubes, device, with_offset, state_location): - """Test the setters for root_state using both the link frame and center of mass as reference frame.""" - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Check if cube_object is initialized - assert cube_object.is_initialized - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) - - # Set center of mass offset via Newton API (position only) - com_pos = offset.unsqueeze(1) # (N, 1, 3) - cube_object.set_coms_index(coms=wp.from_torch(com_pos, dtype=wp.vec3f)) - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) - - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pos_b.torch.squeeze(1), offset) - - rand_state = torch.rand(num_cubes, 13, device=device) - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - # perform step + torch.testing.assert_close(root_link_pose_w, body_link_pose_w) + torch.testing.assert_close(root_com_pose_w, body_com_pose_w) + torch.testing.assert_close(root_link_vel_w, cube_object.data.body_link_vel_w.torch.squeeze(1)) + torch.testing.assert_close(root_com_vel_w, body_com_vel_w) + + # The written state persists into the solver: with gravity off, one step only integrates the + # written velocity. + written_pose_w = (root_com_pose_w if state_location == "com" else root_link_pose_w).clone() + written_com_vel_w = root_com_vel_w.clone() sim.step() - # update buffers cube_object.update(sim.cfg.dt) - - # Prime the lazily-derived caches at the current sim timestamp. Without this they would - # recompute on first access after the write regardless of invalidation; priming them makes a - # missing reset_pose/reset_velocity observable as a stale read in the assertions below. - _ = cube_object.data.root_link_pose_w.torch - _ = cube_object.data.root_com_pose_w.torch - _ = cube_object.data.root_link_vel_w.torch - _ = cube_object.data.root_com_vel_w.torch - - if state_location == "com": - cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "link": - cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "root": - cube_object.write_root_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - - if state_location == "com": - root_com_pose_w = cube_object.data.root_com_pose_w.torch - root_com_vel_w = cube_object.data.root_com_vel_w.torch - body_com_pose_b = cube_object.data.body_com_pose_b.torch - expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( - root_com_pose_w[:, :3], - root_com_pose_w[:, 3:], - quat_rotate(quat_inv(body_com_pose_b[:, 0, 3:7]), -body_com_pose_b[:, 0, :3]), - quat_inv(body_com_pose_b[:, 0, 3:7]), - ) - expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - # test both root_pose and root_link successfully updated when root_com updates - torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) - torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) - torch.testing.assert_close(root_com_vel_w[:, 3:], cube_object.data.root_com_vel_w.torch[:, 3:]) - elif state_location == "link": - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - body_com_pose_b = cube_object.data.body_com_pose_b.torch - expected_com_pos, expected_com_quat = combine_frame_transforms( - root_link_pose_w[:, :3], - root_link_pose_w[:, 3:], - body_com_pose_b[:, 0, :3], - body_com_pose_b[:, 0, 3:7], - ) - expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) - root_com_pose_w = cube_object.data.root_com_pose_w.torch - root_com_vel_w = cube_object.data.root_com_vel_w.torch - # test both root_pose and root_com successfully updated when root_link updates - torch.testing.assert_close(expected_com_pose, root_com_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(root_link_vel_w[:, 3:], root_com_vel_w[:, 3:]) - torch.testing.assert_close(root_link_pose_w, cube_object.data.root_link_pose_w.torch) - torch.testing.assert_close(root_link_vel_w[:, 3:], cube_object.data.root_com_vel_w.torch[:, 3:]) - elif state_location == "root": - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_com_vel_w = cube_object.data.root_com_vel_w.torch - body_com_pose_b = cube_object.data.body_com_pose_b.torch - expected_com_pos, expected_com_quat = combine_frame_transforms( - root_link_pose_w[:, :3], - root_link_pose_w[:, 3:], - body_com_pose_b[:, 0, :3], - body_com_pose_b[:, 0, 3:7], - ) - expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) - root_com_pose_w = cube_object.data.root_com_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - # test both root_com and root_link successfully updated when root_pose updates - torch.testing.assert_close(expected_com_pose, root_com_pose_w) - torch.testing.assert_close(root_com_vel_w, cube_object.data.root_com_vel_w.torch) - torch.testing.assert_close(root_link_pose_w, cube_object.data.root_link_pose_w.torch) - torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) + pose_w = cube_object.data.root_com_pose_w if state_location == "com" else cube_object.data.root_link_pose_w + torch.testing.assert_close(pose_w.torch, written_pose_w, rtol=1e-1, atol=1e-1) + torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, written_com_vel_w, rtol=1e-1, atol=1e-1) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"]) @pytest.mark.isaacsim_ci -def test_body_link_pose_w_fresh_after_root_pose_write(device, writer): +def test_body_link_pose_w_fresh_after_root_pose_write(device): """Regression: ``body_link_pose_w`` must reflect a freshly written root pose without an intervening sim step. After ``write_root_{link,com}_pose_to_sim_{index,mask}``, the cached ``_sim_bind_body_link_pose_w`` @@ -1042,45 +730,46 @@ def _fk_reset_mask_dirty() -> bool: sim.step() cube_object.update(sim.cfg.dt) - # Prime the body_link_pose_w cache with the current pose. - pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone().view(num_cubes, 7) - - # Clear the dirty flag so we can observe that the write sets it. - SimulationManager.forward() - assert not _fk_reset_mask_dirty() - - # Build a target pose clearly distinct from the current one in both translation and orientation. - # Quaternion in (x, y, z, w) for 90° about z: [0, 0, sin(pi/4), cos(pi/4)] = [0, 0, sqrt(0.5), sqrt(0.5)]. - target_pose = wp.to_torch(cube_object.data.root_link_pose_w).clone() - target_pose[..., 0] += 10.0 - target_pose[..., 1] += 5.0 - target_pose[..., 2] += 2.0 - sqrt_half = 0.7071067811865476 - target_pose[..., 3] = 0.0 - target_pose[..., 4] = 0.0 - target_pose[..., 5] = sqrt_half - target_pose[..., 6] = sqrt_half - - if writer == "link_index": - cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose) - elif writer == "link_mask": - cube_object.write_root_link_pose_to_sim_mask(root_pose=target_pose) - elif writer == "com_index": - cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose) - elif writer == "com_mask": - cube_object.write_root_com_pose_to_sim_mask(root_pose=target_pose) - - # The simulator-side dirty flag must be set before any property read clears it via forward(). - assert _fk_reset_mask_dirty(), "pose write must call SimulationManager.invalidate_fk()" - - # Read without stepping: getter must trigger forward kinematics and return the fresh pose. - body_link = wp.to_torch(cube_object.data.body_link_pose_w).view(num_cubes, 7) - # Defeat alias accidents: the property must not still return the pre-write value. - assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( - "body_link_pose_w returned the pre-write cached pose; forward() was not invoked" - ) - # Translation must match the write. - torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) - # Orientation: compare via |q1 · q2| ≈ 1 to account for the q ≡ -q double cover. - quat_dot = torch.abs((body_link[..., 3:7] * target_pose[..., 3:7]).sum(dim=-1)) - torch.testing.assert_close(quat_dot, torch.ones_like(quat_dot), rtol=1e-4, atol=1e-4) + for writer in ("link_index", "link_mask", "com_index", "com_mask"): + # Prime the body_link_pose_w cache with the current pose. + pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone().view(num_cubes, 7) + + # Clear the dirty flag so we can observe that the write sets it. + SimulationManager.forward() + assert not _fk_reset_mask_dirty() + + # Build a target pose clearly distinct from the current one in both translation and orientation. + # Quaternion in (x, y, z, w) for 90° about z: [0, 0, sin(pi/4), cos(pi/4)] = [0, 0, sqrt(0.5), sqrt(0.5)]. + target_pose = wp.to_torch(cube_object.data.root_link_pose_w).clone() + target_pose[..., 0] += 10.0 + target_pose[..., 1] += 5.0 + target_pose[..., 2] += 2.0 + sqrt_half = 0.7071067811865476 + target_pose[..., 3] = 0.0 + target_pose[..., 4] = 0.0 + target_pose[..., 5] = sqrt_half + target_pose[..., 6] = sqrt_half + + if writer == "link_index": + cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose) + elif writer == "link_mask": + cube_object.write_root_link_pose_to_sim_mask(root_pose=target_pose) + elif writer == "com_index": + cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose) + elif writer == "com_mask": + cube_object.write_root_com_pose_to_sim_mask(root_pose=target_pose) + + # The simulator-side dirty flag must be set before any property read clears it via forward(). + assert _fk_reset_mask_dirty(), f"{writer} pose write must call SimulationManager.invalidate_fk()" + + # Read without stepping: getter must trigger forward kinematics and return the fresh pose. + body_link = wp.to_torch(cube_object.data.body_link_pose_w).view(num_cubes, 7) + # Defeat alias accidents: the property must not still return the pre-write value. + assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( + f"body_link_pose_w returned the pre-write cached pose after {writer}; forward() was not invoked" + ) + # Translation must match the write. + torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) + # Orientation: compare via |q1 · q2| ≈ 1 to account for the q ≡ -q double cover. + quat_dot = torch.abs((body_link[..., 3:7] * target_pose[..., 3:7]).sum(dim=-1)) + torch.testing.assert_close(quat_dot, torch.ones_like(quat_dot), rtol=1e-4, atol=1e-4) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py index 183aae9276f..51c08a2a0a4 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -10,7 +10,7 @@ """Launch Isaac Sim Simulator first.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices +from isaaclab.test.utils import DeviceScope, resolve_test_sim_device, test_devices # launch omniverse app simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app @@ -36,7 +36,6 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.math import ( combine_frame_transforms, - default_orientation, quat_apply_inverse, quat_inv, quat_mul, @@ -131,36 +130,23 @@ def generate_cubes_scene( return cube_object_collection, torch.as_tensor(origins, device=device) +@pytest.mark.parametrize(("num_envs", "num_cubes", "spawn_unrelated_sibling"), [(1, 1, False), (2, 3, True)]) @pytest.mark.parametrize("device", test_devices()) -def test_initialization_ignores_unrelated_sibling_rigid_objects(device): - """Test that a collection view selects only its configured rigid objects.""" - num_envs = 2 - num_cubes = 3 +def test_initialization(num_envs, num_cubes, spawn_unrelated_sibling, device): + """Test initialization for prim with rigid body API at the provided prim path. + + With an unrelated rigid body next to the cubes in each environment, the collection view must still + select only its configured rigid objects. + """ with _newton_sim_context(device, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, _ = generate_cubes_scene( num_envs=num_envs, num_cubes=num_cubes, device=device, - spawn_unrelated_sibling=True, + spawn_unrelated_sibling=spawn_unrelated_sibling, ) - replicate(sim.get_clone_plan()) - sim.reset() - - assert object_collection.num_instances == num_envs - assert object_collection.root_view.count == num_envs * num_cubes - assert object_collection.data.default_body_pose.torch.shape == (num_envs, num_cubes, 7) - - -@pytest.mark.parametrize(("num_envs", "num_cubes"), [(1, 1), (2, 3)]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization(num_envs, num_cubes, device): - """Test initialization for prim with rigid body API at the provided prim path.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - # Check that the framework doesn't hold excessive strong references. assert sys.getrefcount(object_collection) < 10 @@ -170,19 +156,17 @@ def test_initialization(num_envs, num_cubes, device): # Check if object is initialized assert object_collection.is_initialized + assert object_collection.num_instances == num_envs + assert object_collection.root_view.count == num_envs * num_cubes assert len(object_collection.body_names) == num_cubes # Check buffers that exist and have correct shapes + assert object_collection.data.default_body_pose.torch.shape == (num_envs, num_cubes, 7) assert object_collection.data.body_link_pos_w.torch.shape == (num_envs, num_cubes, 3) assert object_collection.data.body_link_quat_w.torch.shape == (num_envs, num_cubes, 4) assert object_collection.data.body_mass.torch.shape == (num_envs, num_cubes) assert object_collection.data.body_inertia.torch.shape == (num_envs, num_cubes, 9) - # Simulate physics - for _ in range(2): - sim.step() - object_collection.update(sim.cfg.dt) - @pytest.mark.parametrize("device", test_devices()) def test_set_body_inertial_properties_updates_inverses(device): @@ -268,70 +252,21 @@ def test_initialization_with_no_rigid_body(): # Play sim replicate(sim.get_clone_plan()) - with pytest.raises(RuntimeError): + with pytest.raises(RuntimeError, match="Expected 1 prims at"): sim.reset() -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(device): - """Test if external force buffer correctly updates in the force value is zero case.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - num_envs = 2 - num_cubes = 1 - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - - # find objects to apply the force - object_ids, object_names = object_collection.find_bodies(".*") - # reset object - object_collection.reset() - - # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) - - # decide if zero or non-zero force - if step == 0 or step == 3: - force = 1.0 - else: - force = 0.0 - - # apply force to the object - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - object_collection.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=object_ids, - env_ids=None, - ) - - # check if the object collection's force and torque buffers are correctly updated - for i in range(num_envs): - assert object_collection._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force - assert object_collection._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force - - object_collection.instantaneous_wrench_composer.add_forces_and_torques_index( - body_ids=object_ids, - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - ) - - # apply action to the object collection - object_collection.write_data_to_sim() - sim.step() - object_collection.update(sim.cfg.dt) - - @pytest.mark.parametrize("num_envs", [2]) @pytest.mark.parametrize("num_cubes", [4]) @pytest.mark.parametrize("device", test_devices()) def test_external_force_on_single_body(num_envs, num_cubes, device): - """Test application of external force on the base of the object.""" + """Test application of external force on the base of the object. + + In the first phase, a force equal to the weight of every 2nd object keeps it in place while the others + fall. In the second phase, the force is applied at 1m in the Y direction and the object must rotate + around its X axis. Both phases run in the global and the local frame, and resetting the collection + must clear the wrench applied in the previous iteration. + """ with _newton_sim_context(device, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) @@ -341,22 +276,35 @@ def test_external_force_on_single_body(num_envs, num_cubes, device): # find objects to apply the force object_ids, object_names = object_collection.find_bodies(".*") - # Sample a force equal to the weight of the object - external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) - # Every 2nd cube should have a force applied to it - external_wrench_b[:, 0::2, 2] = 9.81 * object_collection.data.body_mass.torch[:, 0::2] - - for i in range(5): - # reset object state + def reset_objects(): + # reset object state; shift the cubes to their origins so they are not on top of each other body_pose = object_collection.data.default_body_pose.torch.clone() body_vel = object_collection.data.default_body_vel.torch.clone() - # need to shift the position of the cubes otherwise they will be on top of each other body_pose[..., :2] += origins.unsqueeze(1)[..., :2] object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - # reset object object_collection.reset() + # Reset should zero external forces and torques + assert torch.count_nonzero(object_collection.instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection.instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(object_collection.permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection.permanent_wrench_composer.out_torque_b.torch) == 0 + + def simulate(): + for _ in range(10): + object_collection.write_data_to_sim() + sim.step() + object_collection.update(sim.cfg.dt) + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[:, 0::2, 2] = 9.81 * object_collection.data.body_mass.torch[:, 0::2] + + for i in range(2): + reset_objects() + is_global = False if i % 2 == 0: positions = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] @@ -373,13 +321,7 @@ def test_external_force_on_single_body(num_envs, num_cubes, device): env_ids=None, is_global=is_global, ) - for _ in range(10): - # write data to sim - object_collection.write_data_to_sim() - # step sim - sim.step() - # update object collection - object_collection.update(sim.cfg.dt) + simulate() # First object should still be at the same Z position (1.0) torch.testing.assert_close( @@ -389,46 +331,16 @@ def test_external_force_on_single_body(num_envs, num_cubes, device): # Second object should have fallen, so it's Z height should be less than initial height of 1.0 assert torch.all(object_collection.data.body_link_pos_w.torch[:, 1::2, 2] < 1.0) - -@pytest.mark.parametrize("num_envs", [2]) -@pytest.mark.parametrize("num_cubes", [4]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(num_envs, num_cubes, device): - """Test application of external force on the base of the object at a specific position. - - In this test, we apply a force equal to the weight of an object on the base of - one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. - For the other object, we do not apply any force and check that it falls down. - """ - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - - # find objects to apply the force - object_ids, object_names = object_collection.find_bodies(".*") - - # Sample a force equal to the weight of the object + # Apply a force at 1m in the Y direction on every 2nd cube external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) external_wrench_positions_b = torch.zeros( object_collection.num_instances, len(object_ids), 3, device=sim.device ) - # Every 2nd cube should have a force applied to it external_wrench_b[:, 0::2, 2] = 50.0 external_wrench_positions_b[:, 0::2, 1] = 1.0 - # Desired force and torque - for i in range(5): - # reset object state - body_pose = object_collection.data.default_body_pose.torch.clone() - body_vel = object_collection.data.default_body_vel.torch.clone() - # need to shift the position of the cubes otherwise they will be on top of each other - body_pose[..., :2] += origins.unsqueeze(1)[..., :2] - object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) - object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - # reset object - object_collection.reset() + for i in range(2): + reset_objects() is_global = False if i % 2 == 0: @@ -459,14 +371,7 @@ def test_external_force_on_single_body_at_position(num_envs, num_cubes, device): body_ids=object_ids, is_global=is_global, ) - - for _ in range(10): - # write data to sim - object_collection.write_data_to_sim() - # step sim - sim.step() - # update object collection - object_collection.update(sim.cfg.dt) + simulate() # First object should be rotating around it's X axis assert torch.all(object_collection.data.body_com_ang_vel_b.torch[:, 0::2, 0] > 0.1) @@ -474,168 +379,35 @@ def test_external_force_on_single_body_at_position(num_envs, num_cubes, device): assert torch.all(object_collection.data.body_link_pos_w.torch[:, 1::2, 2] < 1.0) +@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_envs", [3]) @pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_set_object_state(num_envs, num_cubes, device): - """Test setting the state of the object. +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +def test_gravity_vec_w_tracks_model_gravity(num_envs, num_cubes, device): + """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - .. note:: - Turn off gravity for this test as we don't want any external forces acting on the object - to ensure state remains static + Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's + gravity broadcast to every env and body, hiding per-env gravity + randomization (e.g. :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). """ - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - - state_types = ["body_link_pos_w", "body_link_quat_w", "body_com_lin_vel_w", "body_com_ang_vel_w"] - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in state_types: - state_dict = { - "body_link_pos_w": torch.zeros_like(object_collection.data.body_link_pos_w.torch, device=sim.device), - "body_link_quat_w": default_orientation(num=num_cubes * num_envs, device=sim.device).view( - num_envs, num_cubes, 4 - ), - "body_com_lin_vel_w": torch.zeros_like( - object_collection.data.body_com_lin_vel_w.torch, device=sim.device - ), - "body_com_ang_vel_w": torch.zeros_like( - object_collection.data.body_com_ang_vel_w.torch, device=sim.device - ), - } - - for _ in range(5): - # reset object - object_collection.reset() - - # Set random state - if state_type_to_randomize == "body_link_quat_w": - state_dict[state_type_to_randomize] = random_orientation( - num=num_cubes * num_envs, device=sim.device - ).view(num_envs, num_cubes, 4) - else: - state_dict[state_type_to_randomize] = torch.randn(num_envs, num_cubes, 3, device=sim.device) - # make sure objects do not overlap - if state_type_to_randomize == "body_link_pos_w": - state_dict[state_type_to_randomize][..., :2] += origins.unsqueeze(1)[..., :2] - - # perform simulation - for _ in range(5): - body_pose = torch.cat( - [state_dict["body_link_pos_w"], state_dict["body_link_quat_w"]], - dim=-1, - ) - body_vel = torch.cat( - [state_dict["body_com_lin_vel_w"], state_dict["body_com_ang_vel_w"]], - dim=-1, - ) - # reset object state - object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) - object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - sim.step() - - # assert that set object quantities are equal to the ones set in the state_dict - for key, expected_value in state_dict.items(): - value = getattr(object_collection.data, key).torch - # Newton reads state directly from sim (not cached), so post-step drift - # from velocity integration causes larger differences than PhysX - torch.testing.assert_close(value, expected_value, rtol=1e-1, atol=1e-1) - - object_collection.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset_object_collection(num_envs, num_cubes, device): - """Test resetting the state of the rigid object.""" with _newton_sim_context(device, gravity_enabled=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) replicate(sim.get_clone_plan()) sim.reset() - for i in range(5): - sim.step() - object_collection.update(sim.cfg.dt) - - # Move the object to a random position - body_pose = object_collection.data.default_body_pose.torch.clone() - body_pose[..., :3] = torch.randn(num_envs, num_cubes, 3, device=sim.device) - # Random orientation - body_pose[..., 3:7] = random_orientation(num=num_cubes, device=sim.device) - object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) - body_vel = object_collection.data.default_body_vel.torch.clone() - object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - - if i % 2 == 0: - object_collection.reset() - - # Reset should zero external forces and torques - assert not object_collection._instantaneous_wrench_composer.active - assert not object_collection._permanent_wrench_composer.active - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_torque_b.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_torque_b.torch) == 0 - - -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [True, False]) -def test_gravity_vec_w(num_envs, num_cubes, device, gravity_enabled): - """Test that gravity vector direction is set correctly for the rigid object.""" - with _newton_sim_context(device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - - # GRAVITY_VEC_W now binds to Newton's per-env gravity array directly, - # so it carries full m/s^2 values and is shaped per-instance (not - # per-instance-per-body). - expected_g = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - - replicate(sim.get_clone_plan()) - sim.reset() - # Check if gravity vector is set correctly torch.testing.assert_close( - object_collection.data.GRAVITY_VEC_W.torch[0], torch.tensor(expected_g, device=device) + object_collection.data.GRAVITY_VEC_W.torch[0], torch.tensor((0.0, 0.0, -9.81), device=device) ) - # Perform simulation + # The free-falling cubes accelerate with gravity and keep their identity orientation. for _ in range(2): sim.step() object_collection.update(sim.cfg.dt) - - # Expected gravity value is the acceleration of the body - gravity = torch.zeros(num_envs, num_cubes, 6, device=device) - if gravity_enabled: - gravity[..., 2] = -9.81 - - # Check the body accelerations are correct - torch.testing.assert_close(object_collection.data.body_com_acc_w.torch, gravity) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_gravity_vec_w_tracks_model_gravity(num_envs, num_cubes, device): - """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - - Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's - gravity broadcast to every env and body, hiding per-env gravity - randomization (e.g. :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). - """ - with _newton_sim_context(device, gravity_enabled=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() + gravity = torch.zeros(num_envs, num_cubes, 6, device=device) + gravity[..., 2] = -9.81 + torch.testing.assert_close(object_collection.data.body_com_acc_w.torch, gravity) # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. model = SimulationManager.get_model() @@ -665,8 +437,7 @@ def test_gravity_vec_w_tracks_model_gravity(num_envs, num_cubes, device): @pytest.mark.parametrize("num_envs", [4]) @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -def test_object_state_properties(num_envs, num_cubes, device, with_offset): +def test_object_state_properties(num_envs, num_cubes, device): """Test the object_com_state_w and object_link_state_w properties.""" with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -679,11 +450,7 @@ def test_object_state_properties(num_envs, num_cubes, device, with_offset): assert cube_object.is_initialized # change center of mass offset from link frame - offset = ( - torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - if with_offset - else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - ) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) # Set center of mass offset via Newton API (position only, shape (E, B, 3)) cube_object.set_coms_index(coms=wp.from_torch(offset, dtype=wp.vec3f)) @@ -713,60 +480,58 @@ def test_object_state_properties(num_envs, num_cubes, device, with_offset): object_com_pose_w = cube_object.data.body_com_pose_w.torch object_com_vel_w = cube_object.data.body_com_vel_w.torch - # if offset is [0,0,0] all object_state_%_w will match and all body_%_w will match - if not with_offset: - torch.testing.assert_close(object_link_pose_w, object_com_pose_w) - torch.testing.assert_close(object_com_vel_w, object_link_vel_w) - else: - _tol = dict(atol=2e-3, rtol=2e-3) - # cubes are spinning around center of mass - # position will not match - # center of mass position will be constant (i.e. spinning around com) - torch.testing.assert_close(init_com, object_com_pose_w[..., :3], **_tol) - - # link position will be moving but should stay constant away from center of mass - object_link_state_pos_rel_com = quat_apply_inverse( - object_link_pose_w[..., 3:], - object_link_pose_w[..., :3] - object_com_pose_w[..., :3], - ) + _tol = dict(atol=2e-3, rtol=2e-3) + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + torch.testing.assert_close(init_com, object_com_pose_w[..., :3], **_tol) - torch.testing.assert_close(-offset, object_link_state_pos_rel_com, **_tol) + # link position will be moving but should stay constant away from center of mass + object_link_state_pos_rel_com = quat_apply_inverse( + object_link_pose_w[..., 3:], + object_link_pose_w[..., :3] - object_com_pose_w[..., :3], + ) - # orientation of com will be a constant rotation from link orientation - com_quat_b = cube_object.data.body_com_quat_b.torch - com_quat_w = quat_mul(object_link_pose_w[..., 3:], com_quat_b) - torch.testing.assert_close(com_quat_w, object_com_pose_w[..., 3:], **_tol) + torch.testing.assert_close(-offset, object_link_state_pos_rel_com, **_tol) - # lin_vel will not match - # center of mass vel will be constant (i.e. spinning around com) - torch.testing.assert_close( - torch.zeros_like(object_com_vel_w[..., :3]), - object_com_vel_w[..., :3], - **_tol, - ) + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.body_com_quat_b.torch + com_quat_w = quat_mul(object_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, object_com_pose_w[..., 3:], **_tol) - # link frame will be moving, and should be equal to input angular velocity cross offset - lin_vel_rel_object_gt = quat_apply_inverse(object_link_pose_w[..., 3:], object_link_vel_w[..., :3]) - lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_envs, num_cubes, 1)[..., 3:], -offset) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_object_gt, **_tol) + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close( + torch.zeros_like(object_com_vel_w[..., :3]), + object_com_vel_w[..., :3], + **_tol, + ) - # ang_vel will always match - torch.testing.assert_close(object_com_vel_w[..., 3:], object_link_vel_w[..., 3:]) + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_object_gt = quat_apply_inverse(object_link_pose_w[..., 3:], object_link_vel_w[..., :3]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_envs, num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_object_gt, **_tol) + + # ang_vel will always match + torch.testing.assert_close(object_com_vel_w[..., 3:], object_link_vel_w[..., 3:]) @pytest.mark.parametrize("num_envs", [3]) @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -def test_write_object_state(num_envs, num_cubes, device, with_offset, state_location): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" +@pytest.mark.parametrize("state_location", ["com", "link", "root"]) +def test_write_object_state(num_envs, num_cubes, device, state_location): + """Test the object state setters in the center-of-mass frame, the link frame, and the default root frames. + + A write must be readable in the written frame and refresh the derived frame without a sim step, and the + written state must persist into the solver across a step. + """ with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Create a scene with random cubes cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) - env_ids = torch.tensor([x for x in range(num_envs)], dtype=torch.int32) - object_ids = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) + env_ids = torch.tensor([x for x in range(num_envs)], dtype=torch.int32, device=device) + object_ids = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32, device=device) replicate(sim.get_clone_plan()) sim.reset() @@ -775,11 +540,7 @@ def test_write_object_state(num_envs, num_cubes, device, with_offset, state_loca assert cube_object.is_initialized # change center of mass offset from link frame - offset = ( - torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - if with_offset - else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - ) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) # Set center of mass offset via Newton API (position only, shape (E, B, 3)) cube_object.set_coms_index(coms=wp.from_torch(offset, dtype=wp.vec3f)) @@ -790,193 +551,88 @@ def test_write_object_state(num_envs, num_cubes, device, with_offset, state_loca # check center of mass has been set torch.testing.assert_close(cube_object.data.body_com_pos_b.torch, offset) - rand_state = torch.zeros(num_envs, num_cubes, 13, device=device) - rand_state[..., :7] = cube_object.data.default_body_pose.torch - rand_state[..., :3] += cube_object.data.body_link_pos_w.torch - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_ids = env_ids.to(device) - object_ids = object_ids.to(device) - for i in range(10): + for i in range(2): sim.step() cube_object.update(sim.cfg.dt) - if state_location == "com": - if i % 2 == 0: - cube_object.write_body_com_pose_to_sim_index(body_poses=rand_state[..., :7]) - cube_object.write_body_com_velocity_to_sim_index(body_velocities=rand_state[..., 7:]) - else: - cube_object.write_body_com_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_com_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - elif state_location == "link": - if i % 2 == 0: - cube_object.write_body_link_pose_to_sim_index(body_poses=rand_state[..., :7]) - cube_object.write_body_link_velocity_to_sim_index(body_velocities=rand_state[..., 7:]) - else: - cube_object.write_body_link_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_link_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) + body_link_pose_w = cube_object.data.body_link_pose_w.torch + body_com_pose_w = cube_object.data.body_com_pose_w.torch + object_link_to_com_pos, object_link_to_com_quat = subtract_frame_transforms( + body_link_pose_w[..., :3].view(-1, 3), + body_link_pose_w[..., 3:7].view(-1, 4), + body_com_pose_w[..., :3].view(-1, 3), + body_com_pose_w[..., 3:7].view(-1, 4), + ) + + # A target state distinct from the current one in position, orientation, and velocity. + target_pose = torch.cat( + [ + body_link_pose_w[..., :3] + 0.3 * torch.rand(num_envs, num_cubes, 3, device=device), + random_orientation(num_envs * num_cubes, device).view(num_envs, num_cubes, 4), + ], + dim=-1, + ) + target_vel = torch.randn(num_envs, num_cubes, 6, device=device) + # Alternate between the default and explicit environment and body selectors. + selectors = {} if i == 0 else {"env_ids": env_ids, "body_ids": object_ids} if state_location == "com": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.body_com_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.body_com_vel_w.torch) + cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose, **selectors) + cube_object.write_body_com_velocity_to_sim_index(body_velocities=target_vel, **selectors) elif state_location == "link": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.body_link_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.body_link_vel_w.torch) + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose, **selectors) + cube_object.write_body_link_velocity_to_sim_index(body_velocities=target_vel, **selectors) + elif state_location == "root": + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose, **selectors) + cube_object.write_body_com_velocity_to_sim_index(body_velocities=target_vel, **selectors) - -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) -@pytest.mark.parametrize("state_location", ["com", "link", "root"]) -def test_write_object_state_functions_data_consistency(num_envs, num_cubes, device, with_offset, state_location): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) - env_ids = torch.tensor([x for x in range(num_envs)], dtype=torch.int32) - object_ids = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - replicate(sim.get_clone_plan()) - sim.reset() - - # Check if cube_object is initialized - assert cube_object.is_initialized - - # change center of mass offset from link frame - offset = ( - torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - if with_offset - else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - ) - - # Set center of mass offset via Newton API (position only, shape (E, B, 3)) - cube_object.set_coms_index(coms=wp.from_torch(offset, dtype=wp.vec3f)) - # Flush the model change immediately so it takes effect before the next step - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) - - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pos_b.torch, offset) - - rand_state = torch.rand(num_envs, num_cubes, 13, device=device) - rand_state[..., :3] += cube_object.data.body_link_pos_w.torch - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_ids = env_ids.to(device) - object_ids = object_ids.to(device) - sim.step() - cube_object.update(sim.cfg.dt) - - body_link_pose_w = cube_object.data.body_link_pose_w.torch - body_com_pose_w = cube_object.data.body_com_pose_w.torch - object_link_to_com_pos, object_link_to_com_quat = subtract_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:7].view(-1, 4), - body_com_pose_w[..., :3].view(-1, 3), - body_com_pose_w[..., 3:7].view(-1, 4), - ) - - if state_location == "com": - cube_object.write_body_com_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_com_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - elif state_location == "link": - cube_object.write_body_link_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_link_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - elif state_location == "root": - cube_object.write_body_link_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_com_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - - if state_location == "com": - com_pose_w = cube_object.data.body_com_pose_w.torch - com_vel_w = cube_object.data.body_com_vel_w.torch - expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( - com_pose_w[..., :3].view(-1, 3), - com_pose_w[..., 3:].view(-1, 4), - quat_rotate(quat_inv(object_link_to_com_quat), -object_link_to_com_pos), - quat_inv(object_link_to_com_quat), - ) - expected_object_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1).view( - num_envs, -1, 7 - ) - link_pose_w = cube_object.data.body_link_pose_w.torch - link_vel_w = cube_object.data.body_link_vel_w.torch - # test both root_pose and root_link successfully updated when root_com updates - torch.testing.assert_close(expected_object_link_pose, link_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(com_vel_w[..., 3:], link_vel_w[..., 3:]) - torch.testing.assert_close(expected_object_link_pose, link_pose_w) - torch.testing.assert_close(com_vel_w[..., 3:], cube_object.data.body_com_vel_w.torch[..., 3:]) - elif state_location == "link": link_pose_w = cube_object.data.body_link_pose_w.torch link_vel_w = cube_object.data.body_link_vel_w.torch - expected_com_pos, expected_com_quat = combine_frame_transforms( - link_pose_w[..., :3].view(-1, 3), - link_pose_w[..., 3:].view(-1, 4), - object_link_to_com_pos, - object_link_to_com_quat, - ) - expected_object_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1).view(num_envs, -1, 7) com_pose_w = cube_object.data.body_com_pose_w.torch com_vel_w = cube_object.data.body_com_vel_w.torch - # test both root_pose and root_com successfully updated when root_link updates - torch.testing.assert_close(expected_object_com_pose, com_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(link_vel_w[..., 3:], com_vel_w[..., 3:]) - torch.testing.assert_close(link_pose_w, cube_object.data.body_link_pose_w.torch) - torch.testing.assert_close(link_vel_w[..., 3:], cube_object.data.body_com_vel_w.torch[..., 3:]) - elif state_location == "root": - body_link_pose_w = cube_object.data.body_link_pose_w.torch - body_com_vel_w = cube_object.data.body_com_vel_w.torch - expected_object_com_pos, expected_object_com_quat = combine_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:].view(-1, 4), - object_link_to_com_pos, - object_link_to_com_quat, - ) - expected_object_com_pose = torch.cat((expected_object_com_pos, expected_object_com_quat), dim=1).view( - num_envs, -1, 7 - ) - com_pose_w = cube_object.data.body_com_pose_w.torch - com_vel_w = cube_object.data.body_com_vel_w.torch - link_pose_w = cube_object.data.body_link_pose_w.torch - link_vel_w = cube_object.data.body_link_vel_w.torch - # test both root_com and root_link successfully updated when root_pose updates - torch.testing.assert_close(expected_object_com_pose, com_pose_w) - torch.testing.assert_close(body_com_vel_w, com_vel_w) - torch.testing.assert_close(body_link_pose_w, link_pose_w) - torch.testing.assert_close(body_com_vel_w[..., 3:], link_vel_w[..., 3:]) + if state_location == "com": + torch.testing.assert_close(target_pose, com_pose_w) + torch.testing.assert_close(target_vel, com_vel_w) + # the com pose was written, so the derived link pose must be refreshed + expected_link_pos, expected_link_quat = combine_frame_transforms( + com_pose_w[..., :3].view(-1, 3), + com_pose_w[..., 3:].view(-1, 4), + quat_rotate(quat_inv(object_link_to_com_quat), -object_link_to_com_pos), + quat_inv(object_link_to_com_quat), + ) + expected_link_pose = torch.cat((expected_link_pos, expected_link_quat), dim=1).view(num_envs, -1, 7) + torch.testing.assert_close(expected_link_pose, link_pose_w) + else: + torch.testing.assert_close(target_pose, link_pose_w) + written_vel_w = link_vel_w if state_location == "link" else com_vel_w + torch.testing.assert_close(target_vel, written_vel_w) + # the link pose was written, so the derived com pose must be refreshed + expected_com_pos, expected_com_quat = combine_frame_transforms( + link_pose_w[..., :3].view(-1, 3), + link_pose_w[..., 3:].view(-1, 4), + object_link_to_com_pos, + object_link_to_com_quat, + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1).view(num_envs, -1, 7) + torch.testing.assert_close(expected_com_pose, com_pose_w) + # skip lin_vel because it differs between the frames; angular velocity is frame-independent + # and only matches when the derived velocity was actually refreshed after the write + torch.testing.assert_close(com_vel_w[..., 3:], link_vel_w[..., 3:]) + + # The written state persists into the solver: with gravity off, one step only integrates the + # written velocity. + written_pose_w = (com_pose_w if state_location == "com" else link_pose_w).clone() + written_com_vel_w = com_vel_w.clone() + sim.step() + cube_object.update(sim.cfg.dt) + pose_w = cube_object.data.body_com_pose_w if state_location == "com" else cube_object.data.body_link_pose_w + torch.testing.assert_close(pose_w.torch, written_pose_w, rtol=1e-1, atol=1e-1) + torch.testing.assert_close(cube_object.data.body_com_vel_w.torch, written_com_vel_w, rtol=1e-1, atol=1e-1) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"]) @pytest.mark.isaacsim_ci -def test_body_pose_write_marks_fk_reset_mask(device, writer): +def test_body_pose_write_marks_fk_reset_mask(device): """Regression: ``write_body_{link,com}_pose_to_sim_{index,mask}`` must mark FK dirty. For a collection, ``_sim_bind_body_link_pose_w`` is bound directly to the simulator's root-transforms @@ -1005,33 +661,34 @@ def _fk_reset_mask_dirty() -> bool: sim.step() cube_object.update(sim.cfg.dt) - # Clear the dirty flag so we can observe that the write sets it. - SimulationManager.forward() - assert not _fk_reset_mask_dirty() - - pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() - - target_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() - target_pose[..., 0] += 10.0 - target_pose[..., 1] += 5.0 - target_pose[..., 2] += 2.0 - - if writer == "link_index": - cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose) - elif writer == "link_mask": - cube_object.write_body_link_pose_to_sim_mask(body_poses=target_pose) - elif writer == "com_index": - cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose) - elif writer == "com_mask": - cube_object.write_body_com_pose_to_sim_mask(body_poses=target_pose) - - assert _fk_reset_mask_dirty(), "pose write must call SimulationManager.invalidate_fk()" - - # body_link_pose_w must reflect the write immediately — its underlying buffer is the write - # target. A regression that moves this property to a separate cached buffer (mirroring the - # single-object case) would silently break this invariant. - body_link = wp.to_torch(cube_object.data.body_link_pose_w) - assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( - "body_link_pose_w still aliases the pre-write pose; the underlying buffer was not written" - ) - torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) + for writer in ("link_index", "link_mask", "com_index", "com_mask"): + # Clear the dirty flag so we can observe that the write sets it. + SimulationManager.forward() + assert not _fk_reset_mask_dirty() + + pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() + + target_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() + target_pose[..., 0] += 10.0 + target_pose[..., 1] += 5.0 + target_pose[..., 2] += 2.0 + + if writer == "link_index": + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose) + elif writer == "link_mask": + cube_object.write_body_link_pose_to_sim_mask(body_poses=target_pose) + elif writer == "com_index": + cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose) + elif writer == "com_mask": + cube_object.write_body_com_pose_to_sim_mask(body_poses=target_pose) + + assert _fk_reset_mask_dirty(), f"{writer} pose write must call SimulationManager.invalidate_fk()" + + # body_link_pose_w must reflect the write immediately — its underlying buffer is the write + # target. A regression that moves this property to a separate cached buffer (mirroring the + # single-object case) would silently break this invariant. + body_link = wp.to_torch(cube_object.data.body_link_pose_w) + assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( + f"body_link_pose_w still aliases the pre-write pose after {writer}; the buffer was not written" + ) + torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) diff --git a/source/isaaclab_newton/test/assets/test_wrench_kernels.py b/source/isaaclab_newton/test/assets/test_wrench_kernels.py deleted file mode 100644 index 4dc272cc941..00000000000 --- a/source/isaaclab_newton/test/assets/test_wrench_kernels.py +++ /dev/null @@ -1,73 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Regression tests for Newton external-wrench packing kernels.""" - -import numpy as np -import warp as wp -from isaaclab_newton.assets import kernels as shared_kernels -from isaaclab_newton.assets.articulation import kernels as articulation_kernels - -_IDENTITY_QUAT = (0.0, 0.0, 0.0, 1.0) -_QUARTER_TURN_Z_QUAT = (0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)) - - -def _body_link_poses_w( - poses: list[tuple[tuple[float, float, float], tuple[float, float, float, float]]], -) -> wp.array: - """Create one-environment body-link poses from translations and quaternions.""" - poses = np.asarray([[[*translation, *quaternion] for translation, quaternion in poses]], dtype=np.float32) - return wp.array(poses, dtype=wp.transformf, device="cpu") - - -def test_update_wrench_array_rotates_body_wrenches_to_world_frame() -> None: - """Rotate body-frame force and torque into the world-frame wrench.""" - forces = wp.array(np.asarray([[[2.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - torques = wp.array(np.asarray([[[3.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - # A COM wrench rotation must not add a link-origin moment from this translation. - body_link_pose_w = _body_link_poses_w([((7.0, -5.0, 11.0), _QUARTER_TURN_Z_QUAT)]) - wrench = wp.zeros((1, 1), dtype=wp.spatial_vectorf, device="cpu") - env_mask = wp.array(np.asarray([True]), dtype=wp.bool, device="cpu") - body_mask = wp.array(np.asarray([True]), dtype=wp.bool, device="cpu") - - wp.launch( - shared_kernels.update_wrench_array_with_force_and_torque, - dim=(1, 1), - inputs=[forces, torques, body_link_pose_w, wrench, env_mask, body_mask], - device="cpu", - ) - - np.testing.assert_allclose( - wrench.numpy(), np.asarray([[[0.0, 2.0, 0.0, 0.0, 3.0, 0.0]]], dtype=np.float32), atol=1e-6 - ) - - -def test_update_wrench_array_ordered_rotates_and_scatter_wrenches_to_backend_order() -> None: - """Rotate public-order body wrenches and scatter them into backend order.""" - forces = wp.array(np.asarray([[[1.0, 0.0, 0.0], [3.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - torques = wp.array(np.asarray([[[2.0, 0.0, 0.0], [4.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - # Link poses are public ordered: public body 0 is rotated, then maps to - # backend body 1. The rotated body's nonzero translation must not add a - # p x f moment. - body_link_pose_w = _body_link_poses_w( - [((2.0, -3.0, 4.0), _QUARTER_TURN_Z_QUAT), ((-1.0, 6.0, 8.0), _IDENTITY_QUAT)] - ) - user_to_backend = wp.array(np.asarray([1, 0], dtype=np.int32), dtype=wp.int32, device="cpu") - wrench = wp.zeros((1, 2), dtype=wp.spatial_vectorf, device="cpu") - env_mask = wp.array(np.asarray([True]), dtype=wp.bool, device="cpu") - body_mask = wp.array(np.asarray([True, True]), dtype=wp.bool, device="cpu") - - wp.launch( - articulation_kernels.update_wrench_array_with_force_and_torque_ordered, - dim=(1, 2), - inputs=[forces, torques, body_link_pose_w, user_to_backend, wrench, env_mask, body_mask], - device="cpu", - ) - - np.testing.assert_allclose( - wrench.numpy(), - np.asarray([[[3.0, 0.0, 0.0, 4.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 2.0, 0.0]]], dtype=np.float32), - atol=1e-6, - ) diff --git a/source/isaaclab_newton/test/cloner/test_collision_approximation.py b/source/isaaclab_newton/test/cloner/test_collision_approximation.py index 854730f6faf..b80b73fc68b 100644 --- a/source/isaaclab_newton/test/cloner/test_collision_approximation.py +++ b/source/isaaclab_newton/test/cloner/test_collision_approximation.py @@ -107,6 +107,10 @@ def _make_mixed_visual_stage() -> Usd.Stage: UsdGeom.Sphere.Define(stage, f"{_SOURCE}/StaticAuthored/visual") static_collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/StaticAuthored/collider") UsdPhysics.CollisionAPI.Apply(static_collider.GetPrim()) + + mesh_body = UsdGeom.Xform.Define(stage, f"{_SOURCE}/MeshOnly") + UsdPhysics.RigidBodyAPI.Apply(mesh_body.GetPrim()) + _add_l_prism(stage, f"{_SOURCE}/MeshOnly/geom", None, offset=8.0) return stage @@ -153,11 +157,6 @@ def test_authored_approximation_produces_expected_shape(self, approximation, exp shapes = _collision_shapes(_build(_make_stage(approximation))) assert list(shapes.values()) == [expected] - def test_unauthored_mesh_is_never_approximated(self): - """The cloner approximates nothing on its own: USD defaults ``physics:approximation`` to ``none``.""" - shapes = _collision_shapes(_build(_make_stage(None))) - assert list(shapes.values()) == [GeoType.MESH] - def test_only_the_authored_mesh_is_remeshed_in_a_mixed_stage(self): """A sibling that authors nothing keeps its trimesh while the authored one is remeshed.""" stage = _make_stage("boundingCube") @@ -182,15 +181,6 @@ def test_differing_sources_each_keep_their_own_authored_mode(self): assert list(_collision_shapes(builders[sources[0]]).values()) == [GeoType.SPHERE] assert list(_collision_shapes(builders[sources[1]]).values()) == [GeoType.MESH] - def test_heterogeneous_sources_with_equal_sequences_stay_honored(self): - """Identically authored sources keep their authored modes (no fallback).""" - stage, sources = _make_two_source_stage("boundingSphere", "boundingSphere") - - builders = _build_sources(stage, sources) - - for source in sources: - assert list(_collision_shapes(builders[source]).values()) == [GeoType.SPHERE] - def test_sdf_collider_is_never_remeshed(self): """``physics:approximation`` is ignored on an SDF collider, matching Newton's importer. @@ -205,7 +195,11 @@ def test_sdf_collider_is_never_remeshed(self): assert list(shapes.values()) == [GeoType.MESH] def test_primitive_collider_remains_visible_in_mixed_visual_model(self): - """Colliders remain visible only when their body or static parent has no visual shape.""" + """Colliders remain visible only when their body or static parent has no visual shape. + + A lone default-purpose collision mesh also stays visible: assets that author a single mesh as + both collider and render geometry have no visual-only shape to fall back on. + """ builder = _build(_make_mixed_visual_stage()) flags_by_label = dict(zip(builder.shape_label, builder.shape_flags, strict=True)) @@ -213,21 +207,4 @@ def test_primitive_collider_remains_visible_in_mixed_visual_model(self): assert not flags_by_label[f"{_SOURCE}/Authored/collider"] & ShapeFlags.VISIBLE assert flags_by_label[f"{_SOURCE}/StaticPrimitive/geometry"] & ShapeFlags.VISIBLE assert not flags_by_label[f"{_SOURCE}/StaticAuthored/collider"] & ShapeFlags.VISIBLE - - def test_mesh_collider_remains_visible_in_mixed_visual_model(self): - """A lone default-purpose collision mesh renders even when other bodies have visuals. - - Assets that author a single mesh as both collider and render geometry have no - visual-only shape to fall back on, so hiding their collider makes the body - disappear from the viewer. - """ - stage = _make_mixed_visual_stage() - mesh_body = UsdGeom.Xform.Define(stage, f"{_SOURCE}/MeshOnly") - UsdPhysics.RigidBodyAPI.Apply(mesh_body.GetPrim()) - _add_l_prism(stage, f"{_SOURCE}/MeshOnly/geom", None, offset=8.0) - - builder = _build(stage) - flags_by_label = dict(zip(builder.shape_label, builder.shape_flags, strict=True)) - assert flags_by_label[f"{_SOURCE}/MeshOnly/geom"] & ShapeFlags.VISIBLE - assert not flags_by_label[f"{_SOURCE}/Authored/collider"] & ShapeFlags.VISIBLE diff --git a/source/isaaclab_newton/test/cloner/test_visual_shape_import.py b/source/isaaclab_newton/test/cloner/test_visual_shape_import.py index 9487a65ed8c..c42705d5771 100644 --- a/source/isaaclab_newton/test/cloner/test_visual_shape_import.py +++ b/source/isaaclab_newton/test/cloner/test_visual_shape_import.py @@ -8,6 +8,7 @@ from types import SimpleNamespace import newton +import pytest from isaaclab_newton.cloner import newton_clone_utils from isaaclab_newton.cloner.newton_clone_utils import build_source_builders from newton import ShapeFlags @@ -62,17 +63,12 @@ def _build(stage: Usd.Stage, **kwargs) -> newton.ModelBuilder: class TestClonerVisualShapeImport: """``build_source_builders`` must keep colliders regardless of the visual-shape flag.""" - def test_load_visual_shapes_imports_visual_only_geometry(self): - """The default import keeps the visual-only cube alongside the collider.""" - colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=True)) + @pytest.mark.parametrize("load_visual_shapes", [True, False]) + def test_visual_shape_flag_gates_only_visual_only_geometry(self, load_visual_shapes): + """The visual-only cube is imported only with visual shapes; the collider is always kept.""" + colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=load_visual_shapes)) assert colliding == 1 - assert visual_only == 1 - - def test_skipping_visual_shapes_keeps_colliders(self): - """Disabling visual shapes drops the visual-only cube and nothing else.""" - colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=False)) - assert colliding == 1 - assert visual_only == 0 + assert visual_only == int(load_visual_shapes) def test_skipping_visual_shapes_skips_collider_visibility_resolution(self): """Without visual shapes no collider is hidden, so the restore pass must not run. @@ -123,24 +119,6 @@ def test_nested_static_collider_uses_rigid_body_root_for_visual_lookup(self): assert not builder.shape_flags[0] & ShapeFlags.VISIBLE - def test_static_collider_without_rigid_body_or_visual_remains_visible(self): - """A standalone static collider retains its authored viewport visibility.""" - stage = Usd.Stage.CreateInMemory() - collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/collision") - UsdPhysics.CollisionAPI.Apply(collider.GetPrim()) - builder = SimpleNamespace( - shape_body=[-1], - shape_flags=[ShapeFlags.COLLIDE_SHAPES], - shape_label=[str(collider.GetPrim().GetPath())], - shape_type=[newton.GeoType.BOX], - ) - - newton_clone_utils._restore_visible_colliders_without_visual_shapes( - builder, stage, {str(collider.GetPrim().GetPath()): 0} - ) - - assert builder.shape_flags[0] & ShapeFlags.VISIBLE - def test_generated_proxy_collider_visual_remains_hidden(self): """Newton's unauthored visual companion must not expose a proxy collider.""" stage = Usd.Stage.CreateInMemory() diff --git a/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py b/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py index 4f37651410e..7ea6e8733cc 100644 --- a/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py +++ b/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py @@ -104,20 +104,6 @@ def test_solve_writes_output_buffer(monkeypatch): assert torch.allclose(wp.to_torch(result), torch.tensor([[2.0, 3.0], [4.0, 5.0]])) -def test_constraint_objectives_carry_no_target_or_action(monkeypatch): - _patch_newton_ik(monkeypatch) - solver = _pose_solver( - objectives=[ - NewtonIKPoseObjectiveCfg(body_name="ee"), - NewtonIKJointLimitObjectiveCfg(weight=0.1), - ] - ) - # Only the pose objective is named and command-driven; the joint limit is a - # pure constraint (no name, no action dimensions). - assert list(solver.objectives_by_name) == ["ee"] - assert [obj.action_dim for obj in solver.objectives] == [6, 0] - - def test_multiple_pose_objectives_register_distinct_targets(monkeypatch): _patch_newton_ik(monkeypatch) solver = _pose_solver( @@ -127,7 +113,10 @@ def test_multiple_pose_objectives_register_distinct_targets(monkeypatch): NewtonIKJointLimitObjectiveCfg(weight=0.1), ] ) + # Only pose objectives are named and command-driven; the joint limit is a + # pure constraint (no name, no action dimensions). assert list(solver.objectives_by_name) == ["ee", "torso"] + assert [obj.action_dim for obj in solver.objectives] == [6, 6, 0] assert solver.objectives_by_name["ee"].link_index == 0 assert solver.objectives_by_name["torso"].link_index == 1 diff --git a/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py b/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py index ff406ced13e..93c6413fb63 100644 --- a/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py +++ b/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py @@ -108,7 +108,7 @@ def test_a_view_without_the_actuator_frequency_resolves_to_nothing(): def test_a_tendon_no_actuator_drives_gets_no_column(): - """A passive tendon keeps column -1; the other tendons still find their actuators.""" + """A passive tendon gets no column; the other tendons still find their actuators.""" view, model = _make_view_and_model(tendon_count=3) columns, tendon_ids = resolve_fixed_tendon_actuator_columns(view, model) @@ -140,7 +140,7 @@ def test_tendons_no_actuator_transmits_to_are_named_once(caplog): with caplog.at_level(logging.WARNING): MjWarpTendonControl(articulation, _pair([1, 2], [0, 2]), articulation.root_view) - assert "passive" in caplog.text + assert caplog.text.count("passive") == 1 assert "rh_FFJ0" not in caplog.text diff --git a/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py b/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py index 18a5fd0d1bb..c22ba0b0d7e 100644 --- a/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py +++ b/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py @@ -46,20 +46,15 @@ def cpu_mpm_solver_and_state(): return solver, model.state() -def test_mpm_reset_accepts_global_sentinel_mask(cpu_mpm_solver_and_state): - """The manager factory uses Newton's current global-sentinel mask contract.""" - solver, state = cpu_mpm_solver_and_state - - solver.reset(state, world_mask=wp.array([False, True, False], dtype=wp.bool, device="cpu"), flags=0) - - def test_mpm_manager_explicit_reset_accepts_canonical_mask(cpu_mpm_solver_and_state, monkeypatch): - """The explicit task reset forwards Newton's canonical mask unchanged.""" + """The explicit task reset forwards Newton's canonical global-sentinel mask to the real solver.""" solver, state = cpu_mpm_solver_and_state calls = [] + newton_reset = SolverImplicitMPM.reset def record_reset(self, state, world_mask=None, flags=None): calls.append((state, world_mask, flags)) + newton_reset(self, state, world_mask=world_mask, flags=flags) monkeypatch.setattr(SolverImplicitMPM, "reset", record_reset) monkeypatch.setattr(NewtonManager, "_solver", solver) 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 7002b3973bf..239b6f8a737 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 @@ -160,6 +160,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): transforms: write asset state, then render without an intervening physics step or asset-data read. The assertion reads Fabric's world matrix, which is the transform consumed by Kit/RTX; USD is intentionally not written back. + The synchronized pose must also keep the body's authored USD scale. """ device = "cuda:0" sim_cfg = SimulationCfg( @@ -174,16 +175,21 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): scene = InteractiveScene(_RenderSceneCfg(num_envs=1, env_spacing=2.0)) sim.register_interactive_scene(scene) try: + body_path = "/World/envs/env_0/Cube" + authored_scale = torch.tensor([0.25, 0.5, 0.75]) + body_prim = sim_utils.get_current_stage().GetPrimAtPath(body_path) + body_prim.GetAttribute("xformOp:scale").Set(UsdGf.Vec3d(*authored_scale.tolist())) + sim.reset() scene.reset() _render(sim, scene) + torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) 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" target_pose = torch.tensor( [[1.5, -0.75, 2.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, @@ -238,6 +244,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): rtol=0.0, atol=1.0e-4, ) + torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) finally: sim.register_interactive_scene(None) @@ -246,8 +253,8 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") @pytest.mark.parametrize( ("device", "renderer_cfg"), - [("cpu", IsaacRtxRendererCfg()), ("cuda:0", IsaacRtxRendererCfg()), ("cuda:0", NewtonWarpRendererCfg())], - ids=["rtx-cpu", "rtx-cuda", "newton-warp"], + [("cpu", IsaacRtxRendererCfg()), ("cuda:0", NewtonWarpRendererCfg())], + ids=["rtx-cpu", "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.""" @@ -527,35 +534,15 @@ def test_frame_view_pose_write_reaches_fabric_when_the_scope_raises(): _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_on_body_child_survives_body_motion(): - """A body-attached frame renders at the written pose and keeps tracking the body.""" - device = "cuda:0" - body_path = "/World/envs/env_0/Cube" - frame_path = f"{body_path}/Frame" - - with _frame_scene(frame_path, (0.0, 0.0, 0.35), device) as (sim, scene, view): - 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, 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, scene) - - expected = body_pose[0, :3].cpu() + (written_position - body_start) - _assert_position(_reported_position(view), expected) - _assert_position(_fabric_position(frame_path), expected) - - @pytest.mark.isaacsim_ci @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): - """Building the mirror must not reseed its prims from USD, which would unrender the moved body.""" + """A body-attached frame renders at written poses while the body moves. + + Building the mirror on the first frame write must not reseed its prims from USD, which would + unrender the moved body. The written frame then keeps tracking later body writes, and a frame + write after unrendered physics steps composes against the live body pose. + """ device = "cuda:0" body_path = "/World/envs/env_0/Cube" frame_path = f"{body_path}/Frame" @@ -567,6 +554,7 @@ def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): _render(sim, scene) _assert_position(_fabric_position(body_path), body_target) + # The first frame write must be the one that builds the mirror. 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())) @@ -578,22 +566,22 @@ def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): _assert_position(_fabric_position(frame_path), written_position) _assert_position(_fabric_position(late_child), written_position + offset) + # The written frame keeps tracking its body. + moved_pose = torch.tensor([[-1.0, 0.5, 1.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=moved_pose) + _render(sim, scene) -@pytest.mark.isaacsim_ci -@pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") -def test_frame_view_pose_write_after_unrendered_steps_reaches_fabric(): - """A pose write renders correctly even when the body moved since the last render.""" - device = "cuda:0" - body_path = "/World/envs/env_0/Cube" - frame_path = f"{body_path}/Frame" + expected = moved_pose[0, :3].cpu() + (written_position - body_target) + _assert_position(_reported_position(view), expected) + _assert_position(_fabric_position(frame_path), expected) - with _frame_scene(frame_path, (0.0, 0.0, 0.35), device) as (sim, scene, view): + # Move the body without rendering, then write the frame. velocity = torch.zeros((1, 6), dtype=torch.float32, device=device) velocity[0, 0] = 5.0 scene["cube"].write_root_com_velocity_to_sim_index(root_velocity=velocity) for _ in range(30): sim.step(render=False) - assert _fabric_position(body_path)[0].item() == pytest.approx(0.0, abs=1.0e-4) + _assert_position(_fabric_position(body_path), moved_pose[0, :3].cpu()) target_position = torch.tensor([0.0, 0.0, 1.5]) _write_frame_world_position(view, target_position.to(device)) 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 c5cf56892cd..d93a1229a01 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -7,16 +7,11 @@ Covers: -* :attr:`NewtonSolverCfg.class_type` resolves to the matching manager subclass. * :meth:`NewtonCfg.__post_init__` propagates ``solver_cfg.class_type`` onto :attr:`NewtonCfg.class_type` so that ``SimulationContext`` picks the right manager. -* Each leaf manager subclasses :class:`NewtonManager` and implements - :meth:`_build_solver` (with the abstract base raising ``NotImplementedError``). * The cross-config validation in :meth:`NewtonMJWarpManager._build_solver` rejects the ``MJWarp + use_mujoco_contacts=True + collision_cfg`` combination. -* Manager name dispatch (used by :class:`InteractiveScene` and the various - factory dispatchers) still starts with ``"newton"``. * Fixed-root pose writes refresh MuJoCo's solver-owned root transform. * End-to-end: spinning up a simulation with each solver builds the correct solver, sets the right ``_use_single_state`` / ``_needs_collision_pipeline`` @@ -60,7 +55,6 @@ NewtonMJWarpManager, NewtonMPMManager, NewtonShapeCfg, - NewtonSolverCfg, NewtonVBDManager, NewtonXPBDManager, VBDSolverCfg, @@ -165,21 +159,6 @@ # --------------------------------------------------------------------------- -@pytest.mark.parametrize( - "solver_cfg_factory, expected_manager, _solver_cls, _single_state, _pipeline", - SOLVER_MATRIX, -) -def test_solver_cfg_class_type_resolves_to_subclass( - solver_cfg_factory, expected_manager, _solver_cls, _single_state, _pipeline -): - """Each ``*SolverCfg.class_type`` resolves to its matching manager subclass.""" - solver_cfg = solver_cfg_factory() - # ``class_type`` is a lazy ``"module:Class"`` reference; calling its - # ``_resolve()`` returns the actual class. ``__name__`` works without - # forcing import (LazyType caches metadata) and is sufficient identity. - assert solver_cfg.class_type.__name__ == expected_manager.__name__ - - @pytest.mark.parametrize( "solver_cfg_factory, expected_manager, _solver_cls, _single_state, _pipeline", SOLVER_MATRIX, @@ -196,11 +175,9 @@ def test_newton_cfg_post_init_propagates_class_type( "num_substeps, collision_decimation, should_warn", [ (8, 0, False), # Default: feature disabled, no warning. - (8, 1, False), # Valid: re-collide every substep. - (8, 2, False), # Valid: re-collide every 2 substeps. (8, 7, False), # Valid edge: one mid-loop re-collide at i=6. (8, 8, True), # Equal to num_substeps: gate never fires. - (8, 16, True), # Larger than num_substeps: gate never fires. + (8, 16, True), # Above num_substeps: pins the ``>=`` rather than ``==`` boundary. ], ) def test_newton_cfg_collision_decimation_warning(num_substeps, collision_decimation, should_warn, caplog): @@ -462,27 +439,24 @@ def test_mpm_solver_cfg_maps_only_newton_solver_fields(): @pytest.mark.parametrize( - "deprecated_value, replacement", + "mode, expected, deprecated", [ - ("instantaneous", "forward"), - ("finite_difference", "backward"), + ("instantaneous", "forward", True), + ("finite_difference", "backward", True), + ("forward", "forward", False), + ("backward", "backward", False), ], ) -def test_mpm_solver_cfg_translates_deprecated_collider_velocity_modes(deprecated_value, replacement): - """Deprecated collider velocity modes warn and map to Newton's current values.""" - with pytest.warns(DeprecationWarning, match=f"use {replacement!r}"): - newton_cfg = _make_solver_config(MPMSolverCfg(collider_velocity_mode=deprecated_value)) - - assert newton_cfg.collider_velocity_mode == replacement - - -@pytest.mark.parametrize("mode", ["forward", "backward"]) -def test_mpm_solver_cfg_preserves_canonical_collider_velocity_modes(mode, recwarn): - """Canonical collider velocity modes pass through without deprecation warnings.""" +def test_mpm_solver_cfg_translates_deprecated_collider_velocity_modes(mode, expected, deprecated, recwarn): + """Deprecated collider velocity modes warn and map to Newton's values; canonical modes pass through silently.""" newton_cfg = _make_solver_config(MPMSolverCfg(collider_velocity_mode=mode)) - assert newton_cfg.collider_velocity_mode == mode - assert not [warning for warning in recwarn if issubclass(warning.category, DeprecationWarning)] + assert newton_cfg.collider_velocity_mode == expected + deprecations = [str(w.message) for w in recwarn if issubclass(w.category, DeprecationWarning)] + if deprecated: + assert any(f"use {expected!r}" in message for message in deprecations) + else: + assert not deprecations # Tuples of ``(field_name, non_default_value)`` covering every solver-tunable @@ -514,19 +488,19 @@ def test_mpm_solver_cfg_preserves_canonical_collider_velocity_modes(mode, recwar ] -@pytest.mark.parametrize("field_name, value", _MPM_FIELD_VALUES) -def test_mpm_solver_cfg_forwards_every_solver_field(field_name, value): +def test_mpm_solver_cfg_forwards_every_solver_field(): """Every tunable MPM cfg field round-trips into ``SolverImplicitMPM.Config``. Guards against MPM manager construction dropping or mis-naming a field if Newton's config surface changes. """ - solver_cfg = MPMSolverCfg(**{field_name: value}) + solver_cfg = MPMSolverCfg(**dict(_MPM_FIELD_VALUES)) newton_cfg = _make_solver_config(solver_cfg) - assert hasattr(newton_cfg, field_name), ( - f"{field_name!r} disappeared from SolverImplicitMPM.Config — MPMSolverCfg needs to drop or rename it." - ) - assert getattr(newton_cfg, field_name) == value + for field_name, value in _MPM_FIELD_VALUES: + assert hasattr(newton_cfg, field_name), ( + f"{field_name!r} disappeared from SolverImplicitMPM.Config — MPMSolverCfg needs to drop or rename it." + ) + assert getattr(newton_cfg, field_name) == value _KAMINO_PADMM_FIELD_VALUES = [ @@ -571,16 +545,20 @@ def test_mpm_solver_cfg_forwards_every_solver_field(field_name, value): ] -@pytest.mark.parametrize("field_name, value", _KAMINO_PADMM_FIELD_VALUES) -def test_kamino_solver_cfg_forwards_padmm_fields(field_name, value): +def test_kamino_solver_cfg_forwards_padmm_fields(): """Every tunable P-ADMM cfg field round-trips into ``PADMMSolverConfig``.""" - sparse_kwargs = {"sparse_jacobian": True, "sparse_dynamics": True} if field_name == "penalty_update_method" else {} - solver_cfg = KaminoPADMMSolverCfg(**sparse_kwargs, dynamics_solver_cfg=KaminoPADMMCfg(**{field_name: value})) - newton_cfg = solver_cfg.to_solver_config() - assert hasattr(newton_cfg.padmm, field_name), ( - f"{field_name!r} disappeared from PADMMSolverConfig — KaminoPADMMCfg needs to drop or rename it." + # Adaptive penalty updates require the sparse solver path. + solver_cfg = KaminoPADMMSolverCfg( + sparse_jacobian=True, + sparse_dynamics=True, + dynamics_solver_cfg=KaminoPADMMCfg(**dict(_KAMINO_PADMM_FIELD_VALUES)), ) - assert getattr(newton_cfg.padmm, field_name) == value + newton_cfg = solver_cfg.to_solver_config() + for field_name, value in _KAMINO_PADMM_FIELD_VALUES: + assert hasattr(newton_cfg.padmm, field_name), ( + f"{field_name!r} disappeared from PADMMSolverConfig — KaminoPADMMCfg needs to drop or rename it." + ) + assert getattr(newton_cfg.padmm, field_name) == value def test_kamino_padmm_rejects_adaptive_penalties_with_dense_dynamics(): @@ -590,18 +568,18 @@ def test_kamino_padmm_rejects_adaptive_penalties_with_dense_dynamics(): solver_cfg.to_solver_config() -@pytest.mark.parametrize("field_name, value", _KAMINO_DVI_FIELD_VALUES) -def test_kamino_solver_cfg_forwards_dvi_fields(field_name, value): +def test_kamino_solver_cfg_forwards_dvi_fields(): """Every tunable DVI cfg field round-trips into ``DVISolverConfig``.""" solver_cfg = KaminoDVISolverCfg( dynamics=KaminoDynamicsCfg(preconditioning=False), - dynamics_solver_cfg=KaminoDVICfg(**{field_name: value}), + dynamics_solver_cfg=KaminoDVICfg(**dict(_KAMINO_DVI_FIELD_VALUES)), ) newton_cfg = solver_cfg.to_solver_config() - assert hasattr(newton_cfg.dvi, field_name), ( - f"{field_name!r} disappeared from DVISolverConfig — KaminoDVICfg needs to drop or rename it." - ) - assert getattr(newton_cfg.dvi, field_name) == value + for field_name, value in _KAMINO_DVI_FIELD_VALUES: + assert hasattr(newton_cfg.dvi, field_name), ( + f"{field_name!r} disappeared from DVISolverConfig — KaminoDVICfg needs to drop or rename it." + ) + assert getattr(newton_cfg.dvi, field_name) == value @pytest.mark.parametrize("field_name, value", _KAMINO_DYNAMICS_FIELD_VALUES) @@ -643,52 +621,29 @@ def test_kamino_dvi_rejects_preconditioning(): solver_cfg.to_solver_config() -def test_mpm_register_builder_attributes_is_idempotent(): - """The MPM custom-attribute hook is a no-op when attributes are already registered.""" - import newton - - builder = newton.ModelBuilder() - assert not builder.has_custom_attribute("mpm:young_modulus") - - NewtonMPMManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mpm:young_modulus") - - # Second call must be a no-op (no exceptions, attribute still present). - NewtonMPMManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mpm:young_modulus") - - -def test_mjwarp_register_builder_attributes_is_idempotent(): - """The MJWarp hook registers native MuJoCo entities exactly once.""" - import newton - - builder = newton.ModelBuilder() - assert not builder.has_custom_attribute("mujoco:actuator_gainprm") - - NewtonMJWarpManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mujoco:actuator_gainprm") - assert "mujoco:actuator" in builder.custom_frequencies - assert "mujoco:tendon" in builder.custom_frequencies - - NewtonMJWarpManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mujoco:actuator_gainprm") - - @pytest.mark.parametrize( ("manager", "active", "inactive"), [ (NewtonMJWarpManager, "mujoco:condim", ("kamino:max_solver_iterations", "mpm:young_modulus")), (NewtonKaminoManager, "kamino:max_solver_iterations", ("mujoco:condim", "mpm:young_modulus")), + (NewtonMPMManager, "mpm:young_modulus", ("mujoco:condim", "kamino:max_solver_iterations")), ], ) -def test_rigid_solver_registers_only_its_builder_attributes(manager, active, inactive): - """A rigid solver declares its own builder schema and no inactive solver schema.""" +def test_solver_registers_only_its_builder_attributes(manager, active, inactive): + """A solver declares its own builder schema once and no inactive solver schema.""" builder = ModelBuilder() manager._register_builder_attributes(builder) assert builder.has_custom_attribute(active) assert all(not builder.has_custom_attribute(name) for name in inactive) + if manager is NewtonMJWarpManager: + assert "mujoco:actuator" in builder.custom_frequencies + assert "mujoco:tendon" in builder.custom_frequencies + + # A second registration on the same builder is a no-op. + manager._register_builder_attributes(builder) + assert builder.has_custom_attribute(active) def test_clone_source_builder_has_no_solver_dependency(): @@ -802,6 +757,9 @@ def test_production_imports_scope_mujoco_joint_properties( joint.CreateBody1Rel().SetTargets([child_path]) joint.GetPrim().CreateAttribute("mjc:frictionloss", Sdf.ValueTypeNames.Double, True).Set(0.11) joint.GetPrim().CreateAttribute("mjc:damping", Sdf.ValueTypeNames.Double, True).Set(0.23) + # PhysX joint properties take precedence over the MuJoCo fallback. + joint.GetPrim().CreateAttribute("mjc:armature", Sdf.ValueTypeNames.Double, True).Set(0.12) + joint.GetPrim().CreateAttribute("physxJoint:armature", Sdf.ValueTypeNames.Float, True).Set(0.21) physics_cfg = NewtonCfg(solver_cfg=solver_cfg, load_visual_shapes=False) monkeypatch.setattr( @@ -845,6 +803,7 @@ def test_production_imports_scope_mujoco_joint_properties( assert model.joint_friction.numpy()[-1] == pytest.approx(expected_friction) assert model.joint_damping.numpy()[-1] == pytest.approx(expected_damping) + assert model.joint_armature.numpy()[-1] == pytest.approx(0.21) @pytest.mark.parametrize( @@ -854,14 +813,7 @@ def test_production_imports_scope_mujoco_joint_properties( pytest.param(NewtonFeatherstoneManager, False, id="featherstone"), ], ) -@pytest.mark.parametrize( - "author_newton_values", - [ - pytest.param(False, id="physx-over-mjc"), - pytest.param(True, id="newton-over-physx-over-mjc"), - ], -) -def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco, author_newton_values): +def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco): """Resolver precedence and MuJoCo fallback selection follow active solver needs.""" from pxr import Sdf, Usd, UsdGeom, UsdPhysics @@ -882,9 +834,8 @@ def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco, auth joint_prim.CreateAttribute("mjc:damping", Sdf.ValueTypeNames.Double, True).Set(0.23) joint_prim.CreateAttribute("mjc:armature", Sdf.ValueTypeNames.Double, True).Set(0.12) joint_prim.CreateAttribute("physxJoint:armature", Sdf.ValueTypeNames.Float, True).Set(0.21) - if author_newton_values: - joint_prim.CreateAttribute("newton:friction", Sdf.ValueTypeNames.Double, True).Set(0.31) - joint_prim.CreateAttribute("newton:armature", Sdf.ValueTypeNames.Double, True).Set(0.41) + joint_prim.CreateAttribute("newton:friction", Sdf.ValueTypeNames.Double, True).Set(0.31) + joint_prim.CreateAttribute("newton:armature", Sdf.ValueTypeNames.Double, True).Set(0.41) schema_resolvers = manager_cls._get_usd_import_schema_resolvers() builder = ModelBuilder() @@ -892,12 +843,10 @@ def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco, auth builder.add_usd(stage, schema_resolvers=schema_resolvers) model = builder.finalize(device="cpu") - expected_friction = 0.31 if author_newton_values else (0.11 if imports_mujoco else 0.0) - expected_damping = 0.23 if imports_mujoco else 0.0 - expected_armature = 0.41 if author_newton_values else 0.21 - assert model.joint_friction.numpy()[-1] == pytest.approx(expected_friction) - assert model.joint_damping.numpy()[-1] == pytest.approx(expected_damping) - assert model.joint_armature.numpy()[-1] == pytest.approx(expected_armature) + # Newton values win over PhysX and MuJoCo; the MuJoCo fallback fills only unset fields. + assert model.joint_friction.numpy()[-1] == pytest.approx(0.31) + assert model.joint_damping.numpy()[-1] == pytest.approx(0.23 if imports_mujoco else 0.0) + assert model.joint_armature.numpy()[-1] == pytest.approx(0.41) @pytest.mark.parametrize("project_outside", [True, False]) @@ -1286,29 +1235,10 @@ def reset(self, state, world_mask=None, flags=0): # --------------------------------------------------------------------------- -# Manager class hierarchy and factory contracts +# Manager lifecycle contracts # --------------------------------------------------------------------------- -@pytest.mark.parametrize( - "manager", - [ - NewtonMJWarpManager, - NewtonXPBDManager, - NewtonVBDManager, - NewtonFeatherstoneManager, - NewtonKaminoManager, - NewtonMPMManager, - ], -) -def test_subclass_of_newton_manager(manager): - """All concrete managers inherit from :class:`NewtonManager`.""" - assert issubclass(manager, NewtonManager) - # Subclasses must override the abstract factory. - assert manager._build_solver is not NewtonManager._build_solver - assert manager._create_solver is not NewtonManager._create_solver - - def test_clear_resets_rigid_body_force_capability(monkeypatch): """Teardown clears the canonical solver capability without subclass shadowing.""" monkeypatch.setattr(NewtonManager, "_supports_rigid_body_force_input", True) @@ -1429,36 +1359,6 @@ def build_solver_with_actuator_mode(cls, model, solver_cfg): assert events == ["body", "ready", *expected_events] * 2 -def test_abstract_build_solver_raises(): - """Calling :meth:`_build_solver` on the abstract base raises.""" - with pytest.raises(NotImplementedError): - NewtonManager._build_solver(model=None, solver_cfg=NewtonSolverCfg()) - - -def test_abstract_create_solver_raises(): - """Calling :meth:`_create_solver` on the base manager raises.""" - with pytest.raises(NotImplementedError): - NewtonManager._create_solver(model=None, solver_cfg=NewtonSolverCfg()) - - -@pytest.mark.parametrize( - "manager", - [ - NewtonMJWarpManager, - NewtonXPBDManager, - NewtonVBDManager, - NewtonFeatherstoneManager, - NewtonKaminoManager, - NewtonMPMManager, - ], -) -def test_manager_name_starts_with_newton(manager): - """The ``"newton"`` prefix is required by :class:`InteractiveScene` and the - various backend factories that dispatch on ``physics_manager.__name__.lower()``. - """ - assert manager.__name__.lower().startswith("newton") - - # --------------------------------------------------------------------------- # End-to-end: build each solver via SimulationContext # --------------------------------------------------------------------------- @@ -1618,9 +1518,7 @@ def test_mjwarp_internal_contacts_with_collision_cfg_raises(): [ (8, 0, 0), # Feature disabled. (8, 2, 3), # Re-collide after substeps 2, 4, 6 (skip last). - (8, 4, 1), # Re-collide after substep 4 only. (8, 7, 1), # Re-collide after substep 7 only. - (8, 8, 0), # Gated off (>= num_substeps). ], ) def test_collision_decimation_invokes_mid_loop_collide(num_substeps, collision_decimation, expected_mid_loop_collides): @@ -1735,8 +1633,7 @@ def clear_forces(self): # --------------------------------------------------------------------------- -@pytest.mark.parametrize("num_steps", [1, 3]) -def test_reset_lands_in_state_0_after_odd_kamino_steps_without_cuda_graph(num_steps): +def test_reset_lands_in_state_0_after_odd_kamino_steps_without_cuda_graph(): """An env reset written through the data-layer binding lands in ``_state_0``. Kamino is double-buffered (``_use_single_state=False``), so each substep @@ -1759,6 +1656,7 @@ def test_reset_lands_in_state_0_after_odd_kamino_steps_without_cuda_graph(num_st the sentinel lands in ``_state_1`` instead, so the final assertion fails. """ sentinel = 1.2345 + num_steps = 1 # any odd count leaves a swapped buffer without the copy-on-last sim_cfg = SimulationCfg( dt=1.0 / 120.0, device="cuda:0", diff --git a/source/isaaclab_newton/test/physics/test_newton_solver_reset.py b/source/isaaclab_newton/test/physics/test_newton_solver_reset.py index 9694220f48d..fd4ac657832 100644 --- a/source/isaaclab_newton/test/physics/test_newton_solver_reset.py +++ b/source/isaaclab_newton/test/physics/test_newton_solver_reset.py @@ -27,6 +27,7 @@ from isaaclab.assets import ArticulationCfg from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate from isaaclab.sim import SimulationCfg, build_simulation_context +from isaaclab.test.utils import DeviceScope, test_devices from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -59,7 +60,7 @@ def _generate_single_joint_articulations(num_articulations: int, device: str) -> return articulation -@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.DEFAULT_CUDA)) def test_env_reset_clears_selected_mjwarp_solver_internals(device): """An env reset clears the flagged world's MuJoCo warm-start history and keeps the others. diff --git a/source/isaaclab_newton/test/physics/test_vbd_core.py b/source/isaaclab_newton/test/physics/test_vbd_core.py index 3915142a275..78d56c2342f 100644 --- a/source/isaaclab_newton/test/physics/test_vbd_core.py +++ b/source/isaaclab_newton/test/physics/test_vbd_core.py @@ -16,18 +16,19 @@ from isaaclab.sim import SimulationContext +# The soft-contact and simulation axes are independent, so each value is covered once. @pytest.mark.parametrize( - ("soft_contact_cfg", "expected"), + ("soft_contact_cfg", "expected", "simulation"), [ - pytest.param(None, (7.0, 8.0, 9.0), id="preserve"), + pytest.param(None, (7.0, 8.0, 9.0), True, id="preserve-physics"), pytest.param( NewtonSoftContactCfg(soft_contact_ke=11.0, soft_contact_kd=12.0, soft_contact_mu=13.0), (11.0, 12.0, 13.0), - id="override", + False, + id="override-render", ), ], ) -@pytest.mark.parametrize("simulation", [False, True], ids=["render", "physics"]) def test_soft_contact_cfg_updates_finalized_model(soft_contact_cfg, expected, simulation): """Registry construction shares the builder and applies model options before native allocation.""" state_values = [] @@ -185,22 +186,25 @@ def color(self, *, balance_colors): assert events == [("color", False), "start"] -@pytest.mark.parametrize("external_rigid_solver", [False, True]) -def test_vbd_solver_force_input_capability(monkeypatch, external_rigid_solver): - """VBD accepts rigid forces only when it integrates rigid bodies.""" +def test_vbd_solver_force_input_capability(monkeypatch): + """VBD rejects rigid forces when an external solver integrates rigid bodies. + + The default (VBD integrates rigid bodies itself) is covered end to end by + ``test_initialize_solver_populates_canonical_state``. + """ physics = importlib.import_module("isaaclab_newton.physics") solver = object() monkeypatch.setattr(physics.NewtonVBDManager, "_create_solver", lambda model, cfg: solver) monkeypatch.setattr(NewtonManager, "_solver", None) monkeypatch.setattr(NewtonManager, "_use_single_state", True) monkeypatch.setattr(NewtonManager, "_needs_collision_pipeline", False) - monkeypatch.setattr(NewtonManager, "_supports_rigid_body_force_input", False) + monkeypatch.setattr(NewtonManager, "_supports_rigid_body_force_input", True) - solver_cfg = physics.VBDSolverCfg(integrate_with_external_rigid_solver=external_rigid_solver) + solver_cfg = physics.VBDSolverCfg(integrate_with_external_rigid_solver=True) physics.NewtonVBDManager._build_solver(object(), solver_cfg) assert NewtonManager._solver is solver - assert NewtonManager._supports_rigid_body_force_input is not external_rigid_solver + assert NewtonManager._supports_rigid_body_force_input is False def test_vbd_rebuilds_particle_bvh_before_physics_step(monkeypatch): diff --git a/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py b/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py index b2deb4f9122..f1501049cc6 100644 --- a/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py +++ b/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py @@ -23,6 +23,7 @@ from isaaclab_newton.renderers.newton_warp_renderer import NewtonWarpRenderer from isaaclab.sim import build_simulation_context +from isaaclab.test.utils import DeviceScope, test_devices _CONTRACT_DIR = Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "renderers" if str(_CONTRACT_DIR) not in sys.path: @@ -48,7 +49,7 @@ def test_kinematic_rigid_object_scale_and_pose_are_rendered() -> None: ) -@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.DEFAULT_CUDA)) def test_intrinsic_updates_preserve_shared_ray_storage(device): """Uniform updates reuse the ray field; nonuniform calibration fails without changing rays.""" data = SimpleNamespace( diff --git a/source/isaaclab_newton/test/renderers/test_segmentation.py b/source/isaaclab_newton/test/renderers/test_segmentation.py index af5368ba48b..1b9b45b054f 100644 --- a/source/isaaclab_newton/test/renderers/test_segmentation.py +++ b/source/isaaclab_newton/test/renderers/test_segmentation.py @@ -101,28 +101,6 @@ def test_instance_segmentation_groups_by_labelled_ancestor(): assert mapping.info["idToSemantics"][ids[0]] == {"class": "cartpole"} -def test_colorize_info_keys_are_color_tuples(): - """With colorization, info keys are ``(r, g, b, a)`` color tuples and a color palette is built.""" - stage, shape_paths = _scene() - mapper = NewtonSegmentationMapper(_model(shape_paths), stage, _cfg(), _empty_clone_plan()) - mapper.build_mapping("semantic_segmentation", colorize=True) - mapping = mapper.get_mapping("semantic_segmentation", colorize=True) - - assert mapping.shape_to_color is not None - assert random_color_from_id(BACKGROUND_ID) in mapping.info["idToLabels"] - assert random_color_from_id(UNLABELLED_ID) in mapping.info["idToLabels"] - - -def test_semantic_filter_excludes_non_matching_types(): - """A filter restricted to an absent type marks every shape UNLABELLED.""" - stage, shape_paths = _scene() - mapper = NewtonSegmentationMapper(_model(shape_paths), stage, _cfg(semantic_filter=["shape"]), _empty_clone_plan()) - mapper.build_mapping("semantic_segmentation", colorize=False) - mapping = mapper.get_mapping("semantic_segmentation", colorize=False) - - assert mapping.shape_to_id.numpy().tolist() == [UNLABELLED_ID] * len(shape_paths) - - def test_semantic_filter_comma_separated_type_clauses(): """Comma-separated ``type:label`` pairs within one semicolon group each match independently. @@ -266,7 +244,10 @@ def test_prototype_fallback_respects_semantic_filter(): def test_semantic_segmentation_mapping_overrides_color(): - """``semantic_segmentation_mapping`` forces the class color and its info key.""" + """``semantic_segmentation_mapping`` forces the class color and its info key. + + With colorization, info keys are ``(r, g, b, a)`` color tuples and a color palette is built. + """ stage, shape_paths = _scene() override = (255, 36, 66, 255) mapper = NewtonSegmentationMapper( @@ -278,6 +259,9 @@ def test_semantic_segmentation_mapping_overrides_color(): mapper.build_mapping("semantic_segmentation", colorize=True) mapping = mapper.get_mapping("semantic_segmentation", colorize=True) + assert mapping.shape_to_color is not None + assert random_color_from_id(BACKGROUND_ID) in mapping.info["idToLabels"] + assert random_color_from_id(UNLABELLED_ID) in mapping.info["idToLabels"] # The cartpole class id must be colored with the override, and keyed by it in idToLabels. assert override in mapping.info["idToLabels"] assert mapping.info["idToLabels"][override] == {"class": "cartpole"} diff --git a/source/isaaclab_newton/test/renderers/test_visual_material.py b/source/isaaclab_newton/test/renderers/test_visual_material.py index 2bd50dcdece..e553571b405 100644 --- a/source/isaaclab_newton/test/renderers/test_visual_material.py +++ b/source/isaaclab_newton/test/renderers/test_visual_material.py @@ -7,6 +7,7 @@ import torch import warp as wp +from isaaclab_newton.physics import NewtonManager from isaaclab_newton.renderers.newton_warp_renderer import NewtonWarpRenderer from isaaclab_newton.renderers.visual_material import ( VisualMaterialWriter, @@ -142,4 +143,4 @@ def test_shape_writer_samples_each_body_and_selected_environment_independently() def test_newton_renderer_exposes_shared_writer_factory() -> None: renderer = object.__new__(NewtonWarpRenderer) - assert renderer.visual_material_writer.__func__.__name__ == "create_visual_material_writer" + assert renderer.visual_material_writer == NewtonManager.create_visual_material_writer 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 79e056feef1..10e5a386ecd 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 @@ -98,7 +98,32 @@ def _fisheye_distortion(apply_lens_distortion: bool) -> OpenCvFisheyeDistortionC ) -def _expected_pinhole_ground_distance(px: int, py: int) -> float: +def _invert_monotonic(forward, target: float) -> float: + """Bisect ``forward(x) == target`` on ``[0, target]`` for a monotonic map with ``forward(x) >= x``.""" + lower, upper = 0.0, target + for _ in range(64): + middle = 0.5 * (lower + upper) + if forward(middle) < target: + lower = middle + else: + upper = middle + return 0.5 * (lower + upper) + + +def _pinhole_undistorted_radius(radius_d: float) -> float: + """Invert the pinhole radial model ``r_d = r_u * (1 + k1 * r_u**2)``.""" + return _invert_monotonic(lambda r: r * (1.0 + _PINHOLE_K1 * r**2), radius_d) + + +def _fisheye_undistorted_radius(radius_d: float) -> float: + """Invert the equidistant model ``theta_d = theta * (1 + k1 theta^2 + k2 theta^4)``; return ``tan(theta)``.""" + k1, k2 = _FISHEYE_COEFFS["k1"], _FISHEYE_COEFFS["k2"] + # k3 = k4 = 0; the map is monotonic over the image's field of view. + theta = _invert_monotonic(lambda t: t * (1.0 + k1 * t**2 + k2 * t**4), radius_d) + return float(np.tan(theta)) + + +def _expected_ground_distance(px: int, py: int, undistorted_radius) -> float: """Compute the expected distorted-ray distance to the ground plane [m].""" u = px + 0.5 v = py + 0.5 @@ -107,14 +132,7 @@ def _expected_pinhole_ground_distance(px: int, py: int) -> float: radius_d = float(np.hypot(x_d, y_d)) if radius_d > 0.0: - lower, upper = 0.0, radius_d - for _ in range(64): - radius_u = 0.5 * (lower + upper) - if radius_u * (1.0 + _PINHOLE_K1 * radius_u**2) < radius_d: - lower = radius_u - else: - upper = radius_u - scale = (0.5 * (lower + upper)) / radius_d + scale = undistorted_radius(radius_d) / radius_d x_u, y_u = x_d * scale, y_d * scale else: x_u, y_u = 0.0, 0.0 @@ -129,6 +147,7 @@ def _expected_pinhole_ground_distance(px: int, py: int) -> float: x_axis /= np.linalg.norm(x_axis) y_axis = np.cross(z_axis, x_axis) ray_world = np.column_stack((x_axis, y_axis, z_axis)) @ ray_camera + assert ray_world[2] < 0.0, "the sampled pixel must look at the ground" return float(-eye[2] / ray_world[2]) @@ -165,31 +184,26 @@ def _mean_abs_distance_diff(a: np.ndarray, b: np.ndarray) -> float: def test_opencv_distortion_changes_newton_render(): - """The Newton renderer must render the distorted and zero-coefficient cameras meaningfully differently.""" + """The Newton renderer applies the OpenCV pinhole and fisheye models and honors ``apply_lens_distortion``. + + Both distorted renders are checked against the analytic ground distance of the distorted ray at sample + pixels, and differ well beyond render noise from the undistorted pinhole reference. + """ distorted = _render_distance(_pinhole_distortion(True)) reference = _render_distance(_pinhole_distortion(False)) + fisheye = _render_distance(_fisheye_distortion(True)) - assert distorted.shape == (HEIGHT, WIDTH, 1) - assert np.isfinite(distorted).mean() > 0.9 - assert np.isfinite(reference).mean() > 0.9 + for image in (distorted, reference, fisheye): + assert image.shape == (HEIGHT, WIDTH, 1) + assert np.isfinite(image).mean() > 0.9 mean_abs_diff = _mean_abs_distance_diff(distorted, reference) assert mean_abs_diff > 0.01, f"distorted vs reference distance maps differ by only {mean_abs_diff:.4f} m" for px, py in ((0, 0), (WIDTH // 2, HEIGHT // 2), (WIDTH - 1, HEIGHT - 1)): - assert distorted[py, px, 0] == pytest.approx(_expected_pinhole_ground_distance(px, py), abs=2e-3) - - -def test_opencv_fisheye_distortion_renders_through_newton(): - """The Newton renderer honors the OpenCV fisheye model: its render differs meaningfully from the pinhole. - - The same calibrated camera is rendered under the OpenCV fisheye model and under an undistorted - pinhole. The fisheye equidistant projection bends the rays, so the two distance maps must differ - well beyond render noise. - """ - fisheye = _render_distance(_fisheye_distortion(True)) - pinhole = _render_distance(_pinhole_distortion(False)) + expected = _expected_ground_distance(px, py, _pinhole_undistorted_radius) + assert distorted[py, px, 0] == pytest.approx(expected, abs=2e-3) - assert fisheye.shape == (HEIGHT, WIDTH, 1) - assert np.isfinite(fisheye).mean() > 0.9 - assert np.isfinite(pinhole).mean() > 0.9 - mean_abs_diff = _mean_abs_distance_diff(fisheye, pinhole) + mean_abs_diff = _mean_abs_distance_diff(fisheye, reference) assert mean_abs_diff > 0.05, f"fisheye vs pinhole distance maps differ by only {mean_abs_diff:.4f} m" + for px, py in ((WIDTH // 4, 3 * HEIGHT // 4), (WIDTH // 2, HEIGHT // 2), (WIDTH - 1, HEIGHT - 1)): + expected = _expected_ground_distance(px, py, _fisheye_undistorted_radius) + assert fisheye[py, px, 0] == pytest.approx(expected, abs=2e-3) diff --git a/source/isaaclab_newton/test/sensors/test_contact_sensor.py b/source/isaaclab_newton/test/sensors/test_contact_sensor.py index 5242f0eb71c..9f160d276b9 100644 --- a/source/isaaclab_newton/test/sensors/test_contact_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_contact_sensor.py @@ -25,7 +25,6 @@ sys.path.insert(0, str(Path(__file__).resolve().parents[1])) import math -import re import pytest import torch @@ -220,13 +219,31 @@ def test_contact_lifecycle(device: str, use_mujoco_contacts: bool, shape_type: S assert no_contact_detected[env_idx], f"Env {env_idx}: Contact should stop after lift." -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("use_mujoco_contacts", COLLISION_PIPELINES) -@pytest.mark.parametrize( - "shape_type", - [ShapeType.SPHERE, ShapeType.MESH_CAPSULE], - ids=["sphere", "mesh_capsule"], -) +def _rotated_horizontal_collision_cases() -> list: + """Cover each shape and pipeline pair once while rotating through the devices. + + The last row pairs MuJoCo contacts with the mesh capsule on the second device (a GPU on the + default runtime) so the MuJoCo-Warp #1527 offset branch stays covered. + """ + devices = test_devices() + rows = [ + (ShapeType.SPHERE, COLLISION_PIPELINES[1]), + (ShapeType.SPHERE, COLLISION_PIPELINES[0]), + (ShapeType.MESH_CAPSULE, COLLISION_PIPELINES[0]), + (ShapeType.MESH_CAPSULE, COLLISION_PIPELINES[1]), + ] + cases = [] + for i, (shape_type, pipeline) in enumerate(rows): + device = devices[i % len(devices)] + cases.append( + pytest.param( + device, pipeline.values[0], shape_type, id=f"{shape_type_to_str(shape_type)}-{pipeline.id}-{device}" + ) + ) + return cases + + +@pytest.mark.parametrize("device, use_mujoco_contacts, shape_type", _rotated_horizontal_collision_cases()) def test_horizontal_collision_detects_contact(device: str, use_mujoco_contacts: bool, shape_type: ShapeType): """Test horizontal collision detection with varied velocities and separations. @@ -334,8 +351,14 @@ def test_horizontal_collision_detects_contact(device: str, use_mujoco_contacts: # =================================================================== -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("use_mujoco_contacts", COLLISION_PIPELINES) +# The pipelines select distinct contact paths; the device selects none, so it rotates across them. +@pytest.mark.parametrize( + "device, use_mujoco_contacts", + [ + pytest.param(device, pipeline.values[0], id=f"{pipeline.id}-{device}") + for device, pipeline in zip(test_devices() * len(COLLISION_PIPELINES), COLLISION_PIPELINES) + ], +) def test_resting_object_contact_force(device: str, use_mujoco_contacts: bool): """Test that resting object contact force equals weight and points upward. @@ -437,83 +460,6 @@ def test_resting_object_contact_force(device: str, use_mujoco_contacts: bool): assert not errs, "\n".join(errs) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("use_mujoco_contacts", COLLISION_PIPELINES) -def test_higher_drop_produces_larger_impact_force(device: str, use_mujoco_contacts: bool): - """Test that dropping from higher produces larger peak impact force. - - 8 environments with heights from 0.3m to 3.0m. - - Verifies: - - Peak impact force generally increases with height - - Overall trend: highest/lowest force ratio > 1.5 - """ - num_envs = 8 - min_height, max_height = 0.3, 3.0 - drop_heights = [min_height + (max_height - min_height) * i / (num_envs - 1) for i in range(num_envs)] - gravity_mag = 9.81 - object_radius = 0.25 - - sim_cfg = make_sim_cfg(use_mujoco_contacts=use_mujoco_contacts, device=device, gravity=(0.0, 0.0, -gravity_mag)) - - with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.object_a = create_shape_cfg( - ShapeType.SPHERE, - "{ENV_REGEX_NS}/Sphere", - pos=(0.0, 0.0, max_height + object_radius), - disable_gravity=False, - activate_contact_sensors=True, - ) - scene_cfg.contact_sensor_a = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Sphere", - update_period=0.0, - history_length=1, - ) - - scene = InteractiveScene(scene_cfg) - sim.reset() - scene.reset() - - obj: RigidObject = scene["object_a"] - contact_sensor: ContactSensor = scene["contact_sensor_a"] - - root_pose = obj.data.root_link_pose_w.torch.clone() - for env_idx in range(num_envs): - root_pose[env_idx, 2] = drop_heights[env_idx] + object_radius - obj.write_root_pose_to_sim_index(root_pose=root_pose) - - total_steps = int(((2 * max_height / gravity_mag) ** 0.5 + 0.5) / SIM_DT) - peak_forces = [0.0] * num_envs - contact_detected = [False] * num_envs - - for _ in range(total_steps): - perform_sim_step(sim, scene, SIM_DT) - force_magnitudes = torch.norm(contact_sensor.data.net_normal_forces_w.torch, dim=-1) - for env_idx in range(num_envs): - f = force_magnitudes[env_idx].max().item() - if f > 0.1: - contact_detected[env_idx] = True - peak_forces[env_idx] = max(peak_forces[env_idx], f) - - for env_idx in range(num_envs): - assert contact_detected[env_idx], f"Env {env_idx} (h={drop_heights[env_idx]:.2f}m): No contact" - - violations = [] - for i in range(num_envs - 1): - if peak_forces[i + 1] < peak_forces[i] * 0.95: - violations.append( - f"Env {i} (h={drop_heights[i]:.2f}m, F={peak_forces[i]:.2f}N) -> " - f"Env {i + 1} (h={drop_heights[i + 1]:.2f}m, F={peak_forces[i + 1]:.2f}N)" - ) - assert len(violations) <= 2, "Peak force should increase with height. Violations:\n" + "\n".join(violations) - - force_ratio = peak_forces[-1] / peak_forces[0] if peak_forces[0] > 0 else 0 - assert force_ratio > 1.5, f"Force ratio (highest/lowest) should be > 1.5. Got {force_ratio:.2f}" - - # =================================================================== # Priority 3: Filtering # =================================================================== @@ -535,22 +481,29 @@ def test_higher_drop_produces_larger_impact_force(device: str, use_mujoco_contac pytest.param(True, id="mujoco_contacts"), ], ) -def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): - """Test that filter_prim_paths_expr filters contacts and enables normal_force_matrix_w. +def test_filtered_contact_forces_and_points(device: str, use_mujoco_contacts: bool): + """Test that filter_prim_paths_expr enables per-filter forces and average contact positions. - Object A rests on ground, Object B stacked on A. - Sensor on A is filtered for B only (not ground). + Object A rests on ground, Object B stacked on A, Object C off to the side (never touching A). + Sensor on A is filtered for B and C with friction and contact point tracking enabled. Verifies: - - normal_force_matrix_w reports only filtered contact (A-B) + - normal_force_matrix_w reports only filtered contact: B-on-A ~ (0, 0, -m_B g), C column zero - net_normal_forces_w reports normal contact against all objects (ground + B) - the normal and friction outputs reconstruct Newton's total-force outputs - - filtered normal force is smaller than net normal force (ground contact excluded from matrix) + - contact_pos_w has shape (num_envs, num_sensors, num_filter_objects, 3) + - The B column reports a position at the A-B interface (top face of A, centered under B) + - The C column is NaN (no contact) + - Regression for #4970: ``scene.reset(env_ids)`` zeroes the reset env's forces and contact + positions without a physics step, and leaves the other envs untouched + - The B column returns to NaN once B is separated from A """ settle_steps = 240 num_envs = 4 - mass_b = 2.0 gravity = 9.81 + size_a = (0.5, 0.5, 0.3) + pos_a = (0.0, 0.0, 0.2) + mass_b = 2.0 expected_force_from_b = mass_b * gravity sim_cfg = make_sim_cfg(use_mujoco_contacts=use_mujoco_contacts, device=device, gravity=(0.0, 0.0, -gravity)) @@ -560,19 +513,18 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) - rigid_props_a = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=0.5, angular_damping=0.5)] + rigid_props = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=0.5, angular_damping=0.5)] scene_cfg.object_a = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/ObjectA", spawn=sim_utils.CuboidCfg( - size=(0.5, 0.5, 0.3), - rigid_props=rigid_props_a, + size=size_a, + rigid_props=rigid_props, collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), mass_props=sim_utils.MassCfg(mass=5.0), activate_contact_sensors=True, ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.2)), + init_state=RigidObjectCfg.InitialStateCfg(pos=pos_a), ) - rigid_props_b = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=2.0, angular_damping=2.0)] scene_cfg.object_b = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/ObjectB", @@ -585,13 +537,25 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.55)), ) + scene_cfg.object_c = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/ObjectC", + spawn=sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + rigid_props=rigid_props, + collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), + mass_props=sim_utils.MassCfg(mass=2.0), + activate_contact_sensors=True, + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(2.0, 0.0, 0.15)), + ) scene_cfg.contact_sensor_a = ContactSensorCfg( prim_path="{ENV_REGEX_NS}/ObjectA", update_period=0.0, history_length=0, - filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB"], + filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB", "{ENV_REGEX_NS}/ObjectC"], track_friction_forces=True, + track_contact_points=True, ) scene = InteractiveScene(scene_cfg) @@ -599,31 +563,44 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): scene.reset() contact_sensor: ContactSensor = scene["contact_sensor_a"] + object_a: RigidObject = scene["object_a"] + object_b: RigidObject = scene["object_b"] + + assert contact_sensor.filter_object_names == ["ObjectB", "ObjectC"], ( + f"unexpected filter_object_names: {contact_sensor.filter_object_names}" + ) + col_b = contact_sensor.filter_object_names.index("ObjectB") + col_c = contact_sensor.filter_object_names.index("ObjectC") # Average over the last `avg_window` ticks to reject per-step solver oscillation. avg_window = 20 + pos_samples: list[torch.Tensor] = [] matrix_samples: list[torch.Tensor] = [] net_samples: list[torch.Tensor] = [] for step in range(settle_steps): perform_sim_step(sim, scene, SIM_DT) if step >= settle_steps - avg_window: + pos_raw = contact_sensor.data.contact_pos_w matrix_raw = contact_sensor.data.normal_force_matrix_w matrix_history_raw = contact_sensor.data.normal_force_matrix_w_history net_raw = contact_sensor.data.net_normal_forces_w - if not matrix_samples: + if not pos_samples: + assert pos_raw is not None, "contact_pos_w should not be None when tracking is enabled" + assert pos_raw.torch.shape == (num_envs, 1, 2, 3), f"unexpected shape: {pos_raw.torch.shape}" assert matrix_raw is not None, "normal_force_matrix_w should not be None when filter is set" assert matrix_history_raw is not None, ( "normal_force_matrix_w_history should not be None when filter is set" ) - assert matrix_history_raw.torch.shape == (num_envs, 1, 1, 1, 3) + assert matrix_history_raw.torch.shape == (num_envs, 1, 1, 2, 3) torch.testing.assert_close(matrix_history_raw.torch[:, 0], matrix_raw.torch) assert contact_sensor.data.net_friction_forces_w is not None assert contact_sensor.data.friction_force_matrix_w is not None + pos_samples.append(pos_raw.torch.clone()) matrix_samples.append(matrix_raw.torch.clone()) net_samples.append(net_raw.torch.clone()) total_force = wp.to_torch(contact_sensor.contact_view.total_force).reshape(num_envs, 1, 3) - total_force_matrix = wp.to_torch(contact_sensor.contact_view.force_matrix).reshape(num_envs, 1, 1, 3) + total_force_matrix = wp.to_torch(contact_sensor.contact_view.force_matrix).reshape(num_envs, 1, 2, 3) torch.testing.assert_close( contact_sensor.data.net_normal_forces_w.torch + contact_sensor.data.net_friction_forces_w.torch, total_force, @@ -635,12 +612,11 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): force_matrix = torch.stack(matrix_samples).mean(dim=0) net_forces = torch.stack(net_samples).mean(dim=0) - expected_b_on_a = torch.tensor([0.0, 0.0, -expected_force_from_b], device=device) tolerance = 0.05 * expected_force_from_b errs: list[str] = [] for env_idx in range(num_envs): - b_on_a = force_matrix[env_idx, 0, 0] + b_on_a = force_matrix[env_idx, 0, col_b] net_contact = net_forces[env_idx, 0] error = torch.norm(b_on_a - expected_b_on_a).item() @@ -654,119 +630,10 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): f"Env {env_idx}: |B-on-A| should be < |net contact|. " f"B-on-A: {b_on_a.tolist()}, Net: {net_contact.tolist()}" ) + if torch.norm(force_matrix[env_idx, 0, col_c]).item() != 0.0: + errs.append(f"Env {env_idx}: C column should report no force. Got {force_matrix[env_idx, 0, col_c]}") assert not errs, "\n".join(errs) - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize( - "use_mujoco_contacts", - [ - pytest.param( - False, - id="newton_contacts", - marks=pytest.mark.skip( - reason=( - "Newton normal_force_matrix_w is non-deterministic across hardware (reports 0 or inflated values)" - ), - ), - ), - pytest.param(True, id="mujoco_contacts"), - ], -) -def test_track_contact_points_reports_average_position(device: str, use_mujoco_contacts: bool): - """Test that track_contact_points reports the average contact position per filter object. - - Object A rests on ground, Object B stacked on A, Object C off to the side (never touching A). - Sensor on A is filtered for B and C with contact point tracking enabled. - - Verifies: - - contact_pos_w is available and has shape (num_envs, num_sensors, num_filter_objects, 3) - - The B column reports a position at the A-B interface (top face of A, centered under B) - - The C column is NaN (no contact) - - The B column returns to NaN once B is separated from A - """ - settle_steps = 240 - num_envs = 4 - gravity = 9.81 - size_a = (0.5, 0.5, 0.3) - pos_a = (0.0, 0.0, 0.2) - - sim_cfg = make_sim_cfg(use_mujoco_contacts=use_mujoco_contacts, device=device, gravity=(0.0, 0.0, -gravity)) - - with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) - - rigid_props = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=0.5, angular_damping=0.5)] - scene_cfg.object_a = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/ObjectA", - spawn=sim_utils.CuboidCfg( - size=size_a, - rigid_props=rigid_props, - collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), - mass_props=sim_utils.MassCfg(mass=5.0), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=pos_a), - ) - scene_cfg.object_b = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/ObjectB", - spawn=sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - rigid_props=rigid_props, - collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), - mass_props=sim_utils.MassCfg(mass=2.0), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.55)), - ) - scene_cfg.object_c = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/ObjectC", - spawn=sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - rigid_props=rigid_props, - collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), - mass_props=sim_utils.MassCfg(mass=2.0), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(2.0, 0.0, 0.15)), - ) - - scene_cfg.contact_sensor_a = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/ObjectA", - update_period=0.0, - history_length=1, - filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB", "{ENV_REGEX_NS}/ObjectC"], - track_contact_points=True, - ) - - scene = InteractiveScene(scene_cfg) - sim.reset() - scene.reset() - - contact_sensor: ContactSensor = scene["contact_sensor_a"] - object_a: RigidObject = scene["object_a"] - object_b: RigidObject = scene["object_b"] - - assert contact_sensor.filter_object_names == ["ObjectB", "ObjectC"], ( - f"unexpected filter_object_names: {contact_sensor.filter_object_names}" - ) - col_b = contact_sensor.filter_object_names.index("ObjectB") - col_c = contact_sensor.filter_object_names.index("ObjectC") - - # Average over the last `avg_window` ticks to reject per-step solver oscillation. - avg_window = 20 - pos_samples: list[torch.Tensor] = [] - for step in range(settle_steps): - perform_sim_step(sim, scene, SIM_DT) - if step >= settle_steps - avg_window: - pos_raw = contact_sensor.data.contact_pos_w - if not pos_samples: - assert pos_raw is not None, "contact_pos_w should not be None when tracking is enabled" - assert pos_raw.torch.shape == (num_envs, 1, 2, 3), f"unexpected shape: {pos_raw.torch.shape}" - pos_samples.append(pos_raw.torch.clone()) - stacked = torch.stack(pos_samples) assert torch.isnan(stacked[:, :, :, col_c]).all(), "C column should be NaN (no contact with ObjectC)" assert torch.isfinite(stacked[:, :, :, col_b]).all(), "B column should be finite (in contact with ObjectB)" @@ -789,6 +656,23 @@ def test_track_contact_points_reports_average_position(device: str, use_mujoco_c ) assert not errs, "\n".join(errs) + # Mimic ``ManagerBasedRLEnv._reset_idx``: reset env 0 inside a step, without a physics step. + pre_reset_forces = contact_sensor.data.net_normal_forces_w.torch.clone() + pre_reset_force_mag = torch.linalg.norm(pre_reset_forces[0], dim=-1).item() + assert pre_reset_force_mag > 1.0, f"Expected non-zero contact force before reset; got {pre_reset_force_mag!r}" + scene.reset(env_ids=torch.tensor([0], device=object_a.device)) + post_reset_forces = contact_sensor.data.net_normal_forces_w.torch + post_reset_force_mag = torch.linalg.norm(post_reset_forces[0], dim=-1).item() + assert post_reset_force_mag == 0.0, ( + "Contact sensor returned stale pre-reset data after scene.reset(): " + f"got {post_reset_force_mag}, expected 0.0 (pre-reset value was {pre_reset_force_mag})." + ) + post_reset_contact_pos = contact_sensor.data.contact_pos_w.torch[0] + assert torch.isnan(post_reset_contact_pos).all(), ( + f"contact_pos_w should reset to NaN after scene.reset(); got {post_reset_contact_pos.tolist()}" + ) + torch.testing.assert_close(post_reset_forces[1:], pre_reset_forces[1:]) + # Separate B from A and verify the B column returns to NaN (not the last contact position). lifted_pose = object_b.data.root_link_pose_w.torch.clone() lifted_pose[:, 2] += 2.0 @@ -818,7 +702,6 @@ def test_track_contact_points_reports_average_position(device: str, use_mujoco_c } -@pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize( "use_mujoco_contacts", [ @@ -832,11 +715,12 @@ def test_track_contact_points_reports_average_position(device: str, use_mujoco_c pytest.param(True, id="mujoco_contacts"), ], ) +# The drop shape selects the collision path; the device selects none, so it rotates across the shapes. @pytest.mark.parametrize( - "drop_shape", + "device, drop_shape", [ - pytest.param(ShapeType.SPHERE, id="sphere"), - pytest.param(ShapeType.MESH_BOX, id="mesh_box"), + pytest.param(test_devices()[-1], ShapeType.SPHERE, id=f"sphere-{test_devices()[-1]}"), + pytest.param(test_devices()[0], ShapeType.MESH_BOX, id=f"mesh_box-{test_devices()[0]}"), ], ) @flaky(max_runs=3, min_passes=1) @@ -1014,7 +898,8 @@ def _make_two_box_scene_cfg(num_envs: int) -> ContactSensorTestSceneCfg: return scene_cfg -@pytest.mark.parametrize("device", test_devices()) +# Sensor metadata is host-side bookkeeping, identical on every device. +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_sensor_metadata(device: str): """Verify sensor_names and filter_object_names match the underlying sensing and counterpart configuration across body-mode, body-mode-with-filter, and shape-mode. @@ -1067,75 +952,6 @@ def test_sensor_metadata(device: str): assert shape_sensor.filter_object_names == [] -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_no_stale_data_after_scene_reset(device: str): - """Regression for #4970: ``scene.reset(env_ids)`` must not surface pre-reset contact data (Newton). - - Mirrors the PhysX equivalent (``test_contact_sensor_no_stale_data_after_reset``). Reproduces the - ``ManagerBasedRLEnv._reset_idx`` flow where reset runs inside a step without a subsequent - physics step; the contact sensor's lazy ``data`` accessor must not refetch from the Newton - contact buffer here (it still reflects the previous step). - """ - sim_cfg = make_sim_cfg(use_mujoco_contacts=False, device=device, gravity=(0.0, 0.0, -9.81)) - with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - scene_cfg = ContactSensorTestSceneCfg(num_envs=1, env_spacing=2.0, lazy_sensor_update=False) - scene_cfg.object_a = create_shape_cfg( - ShapeType.BOX, - "{ENV_REGEX_NS}/Object", - pos=(0.0, 0.0, 1.0), - disable_gravity=False, - activate_contact_sensors=True, - ) - # Object falls onto ObjectB so that contact_pos_w has real data to clear on reset. - scene_cfg.object_b = create_shape_cfg( - ShapeType.BOX, - "{ENV_REGEX_NS}/ObjectB", - pos=(0.0, 0.0, 0.25), - disable_gravity=False, - activate_contact_sensors=True, - ) - scene_cfg.contact_sensor_a = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Object", - update_period=0.0, - history_length=1, - filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB"], - track_contact_points=True, - ) - - scene = InteractiveScene(scene_cfg) - sim.reset() - scene.reset() - - sensor: ContactSensor = scene["contact_sensor_a"] - obj: RigidObject = scene["object_a"] - - for _ in range(200): - perform_sim_step(sim, scene, SIM_DT) - - pre_reset_force_mag = torch.linalg.norm(sensor.data.net_normal_forces_w.torch, dim=-1).item() - assert pre_reset_force_mag > 1.0, f"Expected non-zero contact force before reset; got {pre_reset_force_mag!r}" - - # Mimic ``ManagerBasedRLEnv._reset_idx``: write post-reset asset state, then scene.reset(). - env_ids = torch.tensor([0], device=obj.device) - new_root_pose = torch.tensor([[0.0, 0.0, 2.0, 1.0, 0.0, 0.0, 0.0]], device=obj.device) - new_root_vel = torch.zeros((1, 6), device=obj.device) - obj.write_root_pose_to_sim_index(root_pose=new_root_pose, env_ids=env_ids) - obj.write_root_velocity_to_sim_index(root_velocity=new_root_vel, env_ids=env_ids) - scene.reset(env_ids=env_ids) - - post_reset_force_mag = torch.linalg.norm(sensor.data.net_normal_forces_w.torch, dim=-1).item() - assert post_reset_force_mag == 0.0, ( - "Contact sensor returned stale pre-reset data after scene.reset(): " - f"got {post_reset_force_mag}, expected 0.0 (pre-reset value was {pre_reset_force_mag})." - ) - post_reset_contact_pos = sensor.data.contact_pos_w.torch - assert torch.isnan(post_reset_contact_pos).all(), ( - f"contact_pos_w should reset to NaN after scene.reset(); got {post_reset_contact_pos.tolist()}" - ) - - # =================================================================== # Selector patterns # =================================================================== @@ -1169,11 +985,13 @@ def test_alternation_resolves(): def test_segment_wildcard_does_not_cross_path_separators(): - """``[^/]*`` selects one segment, so nested links stay out.""" + """``[^/]*`` selects one segment, so nested links stay out; shape selectors carry no rule of their own.""" selected = _select(f"{_NS}/Robot/[^/]*", _LABELS) assert "/World/envs/env_0/Robot/base" in selected assert "/World/envs/env_0/Robot/Geometry/panda_link0" not in selected + assert _select(f"{_NS}/Box[^/]*", _SHAPE_LABELS) == [] + assert _select(f"{_NS}/Box[^/]*/.*", _SHAPE_LABELS) == _SHAPE_LABELS def test_expression_list_selects_the_union(): @@ -1192,29 +1010,10 @@ def test_expression_list_selects_the_union(): ] -def test_shape_expressions_match_on_the_same_terms_as_body_expressions(): - """Shape selectors carry no rule of their own.""" - assert _select(f"{_NS}/Box[^/]*", _SHAPE_LABELS) == [] - assert _select(f"{_NS}/Box[^/]*/.*", _SHAPE_LABELS) == _SHAPE_LABELS - - -@pytest.mark.parametrize("expr", [None, []]) -def test_absent_selector_compiles_to_no_pattern(expr): - """Nothing requested means unfiltered, not empty.""" - assert _compile_label_pattern(expr) is None - - -def test_invalid_expression_raises_regex_error(): - """Reject malformed selector expressions at contact sensor construction.""" - with pytest.raises(re.error): - _compile_label_pattern("foo(") - - # The clock is accumulated identically on every device; the largest age bounds the drift. @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("clock_age", [2.5, 30.0]) @pytest.mark.parametrize("history_length", [1, 0], ids=["substep_refresh", "lazy_refresh"]) -def test_first_transition_with_aged_clock(device: str, clock_age: float, history_length: int): +def test_first_transition_with_aged_clock(device: str, history_length: int): """Regression for #7283: transitions must still be reported once the sensor clock has aged. The sensor clock is a float32 accumulator whose rounding error grows with simulated time. On a @@ -1222,6 +1021,7 @@ def test_first_transition_with_aged_clock(device: str, clock_age: float, history tolerance of :meth:`compute_first_contact` has to absorb that error. A fixed 1e-8 tolerance is ~100x too small after a few seconds and silently drops touchdowns and lift-offs. """ + clock_age = 30.0 # With history, the sensor refreshes every physics step; without it, only when data is read. decimation = 1 if history_length > 0 else 4 poll_dt = decimation * SIM_DT diff --git a/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py b/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py index 6e4c8d3be77..1d01b29c2b0 100644 --- a/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py +++ b/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py @@ -19,14 +19,27 @@ def test_force_matrix_history_rolls_newest_first_and_honors_mask(): - """Test newest-first ordering without advancing masked environments.""" + """Test newest-first ordering of every history buffer without advancing masked environments. + + Newton's total-force properties return the totals without the base-class PhysX-limitation warning. + """ data = ContactSensorData() - data.create_buffers(2, 1, 1, 3, True, False, False, "cpu") + data.create_buffers(2, 1, 1, 3, True, False, False, "cpu", track_friction_forces=True) timestamp = wp.ones((2,), dtype=wp.float32, device="cpu") timestamp_last_update = wp.zeros((2,), dtype=wp.float32, device="cpu") + # Each buffer gets a distinct scale so a mis-wired history shows up as a wrong value. + buffers = { + "net_forces_w": 1.0, + "force_matrix_w": 10.0, + "normal_force_matrix_w": 100.0, + "net_friction_forces_w": 1000.0, + "friction_force_matrix_w": 10000.0, + "net_normal_forces_w": 100000.0, + } for value, mask_values in ((1.0, [True, True]), (2.0, [True, True]), (3.0, [True, False])): - wp.to_torch(data._normal_force_matrix_w).fill_(value) + for name, scale in buffers.items(): + wp.to_torch(getattr(data, f"_{name}")).fill_(scale * value) wp.launch( update_contact_sensor_kernel, dim=(2, 1), @@ -57,10 +70,20 @@ def test_force_matrix_history_rolls_newest_first_and_honors_mask(): device="cpu", ) - history = data.normal_force_matrix_w_history.torch - for env, values in enumerate(((3.0, 2.0, 1.0), (2.0, 1.0, 0.0))): - for history_index, value in enumerate(values): - torch.testing.assert_close(history[env, history_index], torch.full_like(history[env, history_index], value)) + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + histories = {name: getattr(data, f"{name}_history").torch for name in buffers} + torch.testing.assert_close(data.net_forces_w.torch, torch.full((2, 1, 3), 3.0)) + torch.testing.assert_close(data.force_matrix_w.torch, torch.full((2, 1, 1, 3), 30.0)) + assert data.friction_forces_w is data.net_friction_forces_w + assert not [item for item in caught if issubclass(item.category, UserWarning)] + + for name, scale in buffers.items(): + history = histories[name] + for env, values in enumerate(((3.0, 2.0, 1.0), (2.0, 1.0, 0.0))): + for history_index, value in enumerate(values): + expected = torch.full_like(history[env, history_index], scale * value) + torch.testing.assert_close(history[env, history_index], expected, msg=name) def test_copy_from_newton_decomposes_normal_and_friction_forces(): @@ -108,11 +131,11 @@ def test_copy_from_newton_decomposes_normal_and_friction_forces(): device="cpu", ) - torch.testing.assert_close(wp.to_torch(data._net_forces_w), torch.tensor([[[3.0, 4.0, 0.0]]])) + torch.testing.assert_close(data.net_forces_w.torch, torch.tensor([[[3.0, 4.0, 0.0]]])) torch.testing.assert_close(data.net_normal_forces_w.torch, torch.tensor([[[3.0, 0.0, 0.0]]])) torch.testing.assert_close(data.net_friction_forces_w.torch, torch.tensor([[[0.0, 4.0, 0.0]]])) torch.testing.assert_close( - wp.to_torch(data._force_matrix_w), + data.force_matrix_w.torch, torch.tensor([[[[1.0, 2.0, 0.0], [0.0, 0.0, 3.0]]]]), ) torch.testing.assert_close( @@ -123,76 +146,3 @@ def test_copy_from_newton_decomposes_normal_and_friction_forces(): data.friction_force_matrix_w.torch, torch.tensor([[[[0.0, 2.0, 0.0], [0.0, 0.0, 1.0]]]]), ) - - -def test_net_forces_w_is_newton_total_without_warning(): - """Test Newton total-force properties return totals without a warning.""" - data = ContactSensorData() - data.create_buffers(1, 1, 1, 1, True, False, False, "cpu", track_friction_forces=True) - wp.to_torch(data._net_forces_w).fill_(7.0) - wp.to_torch(data._force_matrix_w).fill_(8.0) - wp.to_torch(data._friction_force_matrix_w).fill_(9.0) - - with warnings.catch_warnings(record=True) as caught: - warnings.simplefilter("always") - total = data.net_forces_w - matrix = data.force_matrix_w - history = data.net_forces_w_history - matrix_history = data.force_matrix_w_history - friction = data.friction_forces_w - assert total is not None - assert matrix is not None - assert history is not None - assert matrix_history is not None - assert friction is data.net_friction_forces_w - assert not [item for item in caught if issubclass(item.category, UserWarning)] - - -def test_friction_force_history_rolls_newest_first(): - """Test friction history buffers roll newest-first.""" - data = ContactSensorData() - data.create_buffers(1, 1, 1, 3, True, False, False, "cpu", track_friction_forces=True) - timestamp = wp.ones((1,), dtype=wp.float32, device="cpu") - timestamp_last_update = wp.zeros((1,), dtype=wp.float32, device="cpu") - - for value in (1.0, 2.0, 3.0): - wp.to_torch(data._net_friction_forces_w).fill_(value) - wp.to_torch(data._friction_force_matrix_w).fill_(value) - wp.launch( - update_contact_sensor_kernel, - dim=(1, 1), - inputs=[ - 3, - 1, - 0.0, - wp.array([True], dtype=wp.bool, device="cpu"), - data._net_forces_w, - data._force_matrix_w, - data._net_normal_forces_w, - data._normal_force_matrix_w, - data._net_friction_forces_w, - data._friction_force_matrix_w, - timestamp, - timestamp_last_update, - data._net_forces_w_history, - data._force_matrix_w_history, - data._net_normal_forces_w_history, - data._normal_force_matrix_w_history, - data._net_friction_forces_w_history, - data._friction_force_matrix_w_history, - None, - None, - None, - None, - ], - device="cpu", - ) - - torch.testing.assert_close( - data.net_friction_forces_w_history.torch, - torch.tensor([[[[3.0, 3.0, 3.0]], [[2.0, 2.0, 2.0]], [[1.0, 1.0, 1.0]]]]), - ) - torch.testing.assert_close( - data.friction_force_matrix_w_history.torch, - torch.tensor([[[[[3.0, 3.0, 3.0]]], [[[2.0, 2.0, 2.0]]], [[[1.0, 1.0, 1.0]]]]]), - ) diff --git a/source/isaaclab_newton/test/sensors/test_frame_transformer.py b/source/isaaclab_newton/test/sensors/test_frame_transformer.py index ed87348b8bc..e63119b182a 100644 --- a/source/isaaclab_newton/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_newton/test/sensors/test_frame_transformer.py @@ -55,9 +55,6 @@ class MySceneCfg(InteractiveSceneCfg): # articulation - robot robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - # sensors - frame transformer (filled inside unit test) - frame_transformer: FrameTransformerCfg = None - # block cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/cube", @@ -98,487 +95,133 @@ def sim(): # Cleanup is handled by build_simulation_context -def test_frame_transformer_feet_wrt_base(sim): - """Test feet transformations w.r.t. base source frame. - - In this test, the source frame is the robot base. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="LF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), +def _feet_frames(prefixes: list[str]) -> list[FrameTransformerCfg.FrameCfg]: + """Foot frames offset from the ANYmal shanks, named ``_FOOT_USER``.""" + offsets = { + "LF": (-1, (0.08795, 0.01305, -0.33797)), + "RF": (1, (0.08795, -0.01305, -0.33797)), + "LH": (-1, (-0.08795, 0.01305, -0.33797)), + "RH": (1, (-0.08795, -0.01305, -0.33797)), + } + frames = [] + for prefix in prefixes: + sign, xyz = offsets[prefix] + frames.append( FrameTransformerCfg.FrameCfg( - name="LH_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LH_SHANK", + name=f"{prefix}_FOOT_USER", + prim_path=f"{{ENV_REGEX_NS}}/Robot/{prefix}_SHANK", offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(-0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RH_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RH_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(-0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() - - # Acquire the index of ground truth bodies - feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"]) - - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - - # Reorder the feet indices to match the order of the target frames with _USER suffix removed - target_frame_names = [name.split("_USER")[0] for name in target_frame_names] - - # Find the indices of the feet in the order of the target frames - reordering_indices = [feet_names.index(name) for name in target_frame_names] - feet_indices = [feet_indices[i] for i in reordering_indices] - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, + pos=euler_rpy_apply(rpy=(0, 0, sign * math.pi / 2), xyz=xyz), + rot=quat_from_euler_rpy(0, 0, sign * math.pi / 2), ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_pose_w.torch - feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w.torch[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w.torch[:, feet_indices] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch - feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch - - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) - - # check if relative transforms are same - feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - for index in range(len(feet_indices)): - # ground-truth - foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] ) - # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) - - -def test_frame_transformer_feet_wrt_thigh(sim): - """Test feet transformation w.r.t. thigh source frame.""" - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/LF_THIGH", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="LF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() + ) + return frames - # Acquire the index of ground truth bodies - source_frame_index = scene.articulations["robot"].find_bodies("LF_THIGH")[0][0] - feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT"]) - # Check names are parsed the same order - user_feet_names = [f"{name}_USER" for name in feet_names] - assert scene.sensors["frame_transformer"].data.target_frame_names == user_feet_names - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, - ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset buffers - scene.reset() +def _assert_relative_poses(source_pos_w, source_quat_w, target_pos_w, target_quat_w, pos_source, quat_source): + """Check the source-relative target poses against the world poses, target by target.""" + for index in range(target_pos_w.shape[1]): + target_pos_b, target_quat_b = math_utils.subtract_frame_transforms( + source_pos_w, source_quat_w, target_pos_w[:, index], target_quat_w[:, index] + ) + torch.testing.assert_close(pos_source[:, index], target_pos_b) + torch.testing.assert_close(quat_source[:, index], target_quat_b) - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - # check absolute frame transforms in world frame - # -- ground-truth - source_pose_w_gt = scene.articulations["robot"].data.body_state_w.torch[:, source_frame_index, :7] - feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w.torch[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w.torch[:, feet_indices] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch - feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch - # check if they are same - torch.testing.assert_close(source_pose_w_gt[:, :3], source_pos_w_tf) - torch.testing.assert_close(source_pose_w_gt[:, 3:], source_quat_w_tf) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) - - # check if relative transforms are same - feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - for index in range(len(feet_indices)): - # ground-truth - foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( - source_pose_w_gt[:, :3], source_pose_w_gt[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] - ) - # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) +def test_frame_transformer_sources_and_targets(sim): + """Frame transformers with different sources and targets track ground truth across scene resets. + One scene hosts five sensors, each checked against asset ground truth every step: -def test_frame_transformer_robot_body_to_external_cube(sim): - """Test transformation from robot body to a cube in the scene.""" - # Spawn things into stage + * ``ft_base``: offset foot frames on the shanks w.r.t. the robot base (root source). + * ``ft_thigh``: foot frames w.r.t. a non-root source; target names follow ``find_bodies`` order. + * ``ft_cube``: a separate rigid object as target of a robot body. + * ``ft_offsets``: +-0.1 m offset frames on the cube w.r.t. the cube itself. + * ``ft_all``: every robot body through a ``[^/]*`` wildcard, named after the bodies. + """ scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( + scene_cfg.ft_base = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", target_frames=_feet_frames(["LF", "RF", "LH", "RH"]) + ) + scene_cfg.ft_thigh = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/LF_THIGH", target_frames=_feet_frames(["LF", "RF"]) + ) + scene_cfg.ft_cube = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="CUBE_USER", - prim_path="{ENV_REGEX_NS}/cube", - ), - ], + target_frames=[FrameTransformerCfg.FrameCfg(name="CUBE_USER", prim_path="{ENV_REGEX_NS}/cube")], ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, - ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_pose_w.torch - cube_pos_w_gt = scene.rigid_objects["cube"].data.root_pos_w.torch - cube_quat_w_gt = scene.rigid_objects["cube"].data.root_quat_w.torch - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - cube_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch.squeeze() - cube_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch.squeeze() - - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(cube_pos_w_gt, cube_pos_w_tf) - torch.testing.assert_close(cube_quat_w_gt, cube_quat_w_tf) - - # check if relative transforms are same - cube_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - cube_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - # ground-truth - cube_pos_b, cube_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], cube_pos_w_tf, cube_quat_w_tf - ) - # check if they are same - torch.testing.assert_close(cube_pos_source_tf[:, 0], cube_pos_b) - torch.testing.assert_close(cube_quat_source_tf[:, 0], cube_quat_b) - - -def test_frame_transformer_offset_frames(sim): - """Test body transformation w.r.t. base source frame. - - In this test, the source frame is the cube frame. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( + scene_cfg.ft_offsets = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/cube", target_frames=[ - FrameTransformerCfg.FrameCfg( - name="CUBE_CENTER", - prim_path="{ENV_REGEX_NS}/cube", - ), + FrameTransformerCfg.FrameCfg(name="CUBE_CENTER", prim_path="{ENV_REGEX_NS}/cube"), FrameTransformerCfg.FrameCfg( name="CUBE_TOP", prim_path="{ENV_REGEX_NS}/cube", - offset=OffsetCfg( - pos=(0.0, 0.0, 0.1), - rot=(0.0, 0.0, 0.0, 1.0), - ), + offset=OffsetCfg(pos=(0.0, 0.0, 0.1), rot=(0.0, 0.0, 0.0, 1.0)), ), FrameTransformerCfg.FrameCfg( name="CUBE_BOTTOM", prim_path="{ENV_REGEX_NS}/cube", - offset=OffsetCfg( - pos=(0.0, 0.0, -0.1), - rot=(0.0, 0.0, 0.0, 1.0), - ), + offset=OffsetCfg(pos=(0.0, 0.0, -0.1), rot=(0.0, 0.0, 0.0, 1.0)), ), ], ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() - - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene["cube"].data.default_root_pose.torch, - scene["cube"].data.default_root_vel.torch, - ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - # -- set root state - # -- cube - scene["cube"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - # reset buffers - scene.reset() - - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - cube_pos_w_gt = scene["cube"].data.root_pos_w.torch - cube_quat_w_gt = scene["cube"].data.root_quat_w.torch - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - target_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch.squeeze() - target_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch.squeeze() - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - - cube_center_idx = target_frame_names.index("CUBE_CENTER") - cube_bottom_idx = target_frame_names.index("CUBE_BOTTOM") - cube_top_idx = target_frame_names.index("CUBE_TOP") - - # check if they are same - torch.testing.assert_close(cube_pos_w_gt, source_pos_w_tf) - torch.testing.assert_close(cube_quat_w_gt, source_quat_w_tf) - torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) - torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) - - # test offsets are applied correctly - # -- cube top - cube_pos_top = target_pos_w_tf[:, cube_top_idx] - cube_quat_top = target_quat_w_tf[:, cube_top_idx] - torch.testing.assert_close( - cube_pos_top, cube_pos_w_gt + torch.tensor([0.0, 0.0, 0.1], device=cube_pos_w_gt.device) - ) - torch.testing.assert_close(cube_quat_top, cube_quat_w_gt) - - # -- cube bottom - cube_pos_bottom = target_pos_w_tf[:, cube_bottom_idx] - cube_quat_bottom = target_quat_w_tf[:, cube_bottom_idx] - torch.testing.assert_close( - cube_pos_bottom, cube_pos_w_gt + torch.tensor([0.0, 0.0, -0.1], device=cube_pos_w_gt.device) - ) - torch.testing.assert_close(cube_quat_bottom, cube_quat_w_gt) - - -def test_frame_transformer_all_bodies(sim): - """Test transformation of all bodies w.r.t. base source frame. - - In this test, the source frame is the robot base. - - The target_frames are all bodies in the robot, implemented using .* pattern. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( + scene_cfg.ft_all = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/[^/]*", - ), - ], + target_frames=[FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/[^/]*")], ) scene = InteractiveScene(scene_cfg) # Play the simulator sim.reset() - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - articulation_body_names = scene.articulations["robot"].data.body_names - - reordering_indices = [target_frame_names.index(name) for name in articulation_body_names] + robot = scene.articulations["robot"] + cube = scene["cube"] + + # -- ft_base: reorder the feet indices to match the target frames with the _USER suffix removed + base_feet_indices, base_feet_names = robot.find_bodies(["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"]) + base_frame_names = [name.split("_USER")[0] for name in scene.sensors["ft_base"].data.target_frame_names] + base_feet_indices = [base_feet_indices[base_feet_names.index(name)] for name in base_frame_names] + # -- ft_thigh: names are parsed in the same order as the bodies + thigh_index = robot.find_bodies("LF_THIGH")[0][0] + thigh_feet_indices, thigh_feet_names = robot.find_bodies(["LF_FOOT", "RF_FOOT"]) + assert scene.sensors["ft_thigh"].data.target_frame_names == [f"{name}_USER" for name in thigh_feet_names] + # -- ft_all: wildcard frames are named after the bodies + all_frame_names = scene.sensors["ft_all"].data.target_frame_names + articulation_body_names = robot.data.body_names + all_reordering_indices = [all_frame_names.index(name) for name in articulation_body_names] # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() + default_actions = robot.data.default_joint_pos.torch.clone() # Define simulation stepping sim_dt = sim.get_physics_dt() # Simulate physics for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset + # reset every 25 steps so the sensors are checked across a scene reset if count % 25 == 0: - # reset root state + # -- robot root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, - ), - dim=-1, + (robot.data.default_root_pose.torch, robot.data.default_root_vel.torch), dim=-1 ).clone() root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) + robot.write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + robot.write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch) + robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch) + # -- cube + cube_state = torch.cat( + (cube.data.default_root_pose.torch, cube.data.default_root_vel.torch), dim=-1 + ).clone() + cube_state[:, :3] += scene.env_origins + cube.write_root_pose_to_sim_index(root_pose=cube_state[:, :7]) + cube.write_root_velocity_to_sim_index(root_velocity=cube_state[:, 7:]) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) + robot.set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -586,39 +229,94 @@ def test_frame_transformer_all_bodies(sim): # read data from sim scene.update(sim_dt) - # check absolute frame transforms in world frame # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_pose_w.torch - bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w.torch - bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w.torch - - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - bodies_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch - bodies_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch - - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices]) - torch.testing.assert_close(bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices]) - - bodies_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - bodies_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - - # Go through each body and check if relative transforms are same - for index in range(len(articulation_body_names)): - body_pos_b, body_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], bodies_pos_w_tf[:, index], bodies_quat_w_tf[:, index] - ) + root_pose_w = robot.data.root_pose_w.torch + body_pos_w = robot.data.body_pos_w.torch + body_quat_w = robot.data.body_quat_w.torch + cube_pos_w_gt = cube.data.root_pos_w.torch + cube_quat_w_gt = cube.data.root_quat_w.torch + + # -- ft_base: feet w.r.t. the robot base + data = scene.sensors["ft_base"].data + torch.testing.assert_close(root_pose_w[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(root_pose_w[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(body_pos_w[:, base_feet_indices], data.target_pos_w.torch) + torch.testing.assert_close(body_quat_w[:, base_feet_indices], data.target_quat_w.torch) + _assert_relative_poses( + root_pose_w[:, :3], + root_pose_w[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) - torch.testing.assert_close(bodies_pos_source_tf[:, index], body_pos_b) - torch.testing.assert_close(bodies_quat_source_tf[:, index], body_quat_b) + # -- ft_thigh: feet w.r.t. a thigh + data = scene.sensors["ft_thigh"].data + source_pose_w_gt = robot.data.body_state_w.torch[:, thigh_index, :7] + torch.testing.assert_close(source_pose_w_gt[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(source_pose_w_gt[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(body_pos_w[:, thigh_feet_indices], data.target_pos_w.torch) + torch.testing.assert_close(body_quat_w[:, thigh_feet_indices], data.target_quat_w.torch) + _assert_relative_poses( + source_pose_w_gt[:, :3], + source_pose_w_gt[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) + + # -- ft_cube: the cube w.r.t. the robot base + data = scene.sensors["ft_cube"].data + torch.testing.assert_close(root_pose_w[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(root_pose_w[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(cube_pos_w_gt, data.target_pos_w.torch.squeeze()) + torch.testing.assert_close(cube_quat_w_gt, data.target_quat_w.torch.squeeze()) + _assert_relative_poses( + root_pose_w[:, :3], + root_pose_w[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) + + # -- ft_offsets: offset frames w.r.t. the cube + data = scene.sensors["ft_offsets"].data + target_pos_w_tf = data.target_pos_w.torch + target_quat_w_tf = data.target_quat_w.torch + cube_center_idx = data.target_frame_names.index("CUBE_CENTER") + cube_bottom_idx = data.target_frame_names.index("CUBE_BOTTOM") + cube_top_idx = data.target_frame_names.index("CUBE_TOP") + torch.testing.assert_close(cube_pos_w_gt, data.source_pos_w.torch) + torch.testing.assert_close(cube_quat_w_gt, data.source_quat_w.torch) + torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) + torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) + offset = torch.tensor([0.0, 0.0, 0.1], device=cube_pos_w_gt.device) + torch.testing.assert_close(target_pos_w_tf[:, cube_top_idx], cube_pos_w_gt + offset) + torch.testing.assert_close(target_quat_w_tf[:, cube_top_idx], cube_quat_w_gt) + torch.testing.assert_close(target_pos_w_tf[:, cube_bottom_idx], cube_pos_w_gt - offset) + torch.testing.assert_close(target_quat_w_tf[:, cube_bottom_idx], cube_quat_w_gt) + + # -- ft_all: every body w.r.t. the robot base + data = scene.sensors["ft_all"].data + torch.testing.assert_close(root_pose_w[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(root_pose_w[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(body_pos_w, data.target_pos_w.torch[:, all_reordering_indices]) + torch.testing.assert_close(body_quat_w, data.target_quat_w.torch[:, all_reordering_indices]) + _assert_relative_poses( + root_pose_w[:, :3], + root_pose_w[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) -@pytest.mark.parametrize("source_robot", ["Robot", "Robot_1"]) -@pytest.mark.parametrize("path_prefix", ["{ENV_REGEX_NS}", "/World"]) +# Each source robot and each path prefix is covered once; the axes select independent branches. +@pytest.mark.parametrize(("source_robot", "path_prefix"), [("Robot", "{ENV_REGEX_NS}"), ("Robot_1", "/World")]) def test_frame_transformer_duplicate_body_names(sim, source_robot, path_prefix): """Test tracking bodies with same leaf name at different hierarchy levels. diff --git a/source/isaaclab_newton/test/sensors/test_imu.py b/source/isaaclab_newton/test/sensors/test_imu.py index 91f1b3a611d..1f584c97342 100644 --- a/source/isaaclab_newton/test/sensors/test_imu.py +++ b/source/isaaclab_newton/test/sensors/test_imu.py @@ -12,11 +12,10 @@ import pytest import torch -import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg import isaaclab.sim as sim_utils -from isaaclab.assets import RigidObject, RigidObjectCfg +from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors.imu import Imu, ImuCfg from isaaclab.sim import SimulationCfg @@ -65,8 +64,11 @@ def sim(): yield sim -def test_initialization_and_data_shapes(sim): - """The Newton IMU sensor initializes and exposes correctly shaped buffers after one step.""" +def test_at_rest_measures_gravity_and_zero_angular_velocity(sim): + """A settled IMU measures gravity (~9.81 m/s^2 upward) and near-zero angular velocity. + + While the cube still falls, the accelerometer reads near zero (proper, not coordinate, acceleration). + """ scene_cfg = ImuTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() @@ -74,25 +76,20 @@ def test_initialization_and_data_shapes(sim): imu: Imu = scene["imu"] assert imu.num_instances == 2 - sim.step() - scene.update(sim.get_physics_dt()) - - assert imu.data.ang_vel_b.torch.shape == (2, 3) - assert imu.data.lin_acc_b.torch.shape == (2, 3) + # The cube falls from z=1.0 and lands after ~86 steps; 10 steps are still in freefall. + for _ in range(10): + sim.step() + scene.update(sim.get_physics_dt()) - -def test_at_rest_measures_gravity_and_zero_angular_velocity(sim): - """A settled IMU measures gravity (~9.81 m/s^2 upward) and near-zero angular velocity.""" - scene_cfg = ImuTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() + # In freefall, gravity and inertial acceleration cancel. + acc_magnitude = torch.norm(imu.data.lin_acc_b.torch, dim=-1) + torch.testing.assert_close(acc_magnitude, torch.zeros_like(acc_magnitude), atol=0.5, rtol=0.0) # Step enough for the cube to settle on the ground - for _ in range(500): + for _ in range(490): sim.step() scene.update(sim.get_physics_dt()) - imu: Imu = scene["imu"] lin_acc = imu.data.lin_acc_b.torch ang_vel = imu.data.ang_vel_b.torch @@ -113,112 +110,41 @@ def test_at_rest_measures_gravity_and_zero_angular_velocity(sim): torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel), atol=0.1, rtol=0.0) -def test_reset(sim): - """Test that reset zeroes out IMU data.""" - scene_cfg = ImuTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - # Step enough for the cube to settle on the ground so the accelerometer reads gravity. - # The cube falls from z=1.0 (bottom at z=0.9) and reaches the ground in ~86 steps - # at 200 Hz; 200 steps gives time to settle after impact. - for _ in range(200): - sim.step() - scene.update(sim.get_physics_dt()) - - imu: Imu = scene["imu"] - - lin_acc = imu.data.lin_acc_b.torch - assert torch.any(lin_acc != 0), "Expected non-zero data before reset" - - imu.reset() - - # Access internal buffers directly: accessing imu.data triggers lazy re-evaluation - # which re-fills from the Newton sensor, so we check the raw buffers instead. - ang_vel_after = wp.to_torch(imu._data._ang_vel_b) - lin_acc_after = wp.to_torch(imu._data._lin_acc_b) - - torch.testing.assert_close(ang_vel_after, torch.zeros_like(ang_vel_after)) - torch.testing.assert_close(lin_acc_after, torch.zeros_like(lin_acc_after)) - - -@configclass -class FreefallSceneCfg(InteractiveSceneCfg): - """Scene with a rigid cube and IMU but no ground plane (freefall).""" - - env_spacing = 2.0 - cube = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), - mass_props=sim_utils.MassCfg(mass=1.0), - collision_props=sim_utils.UsdPhysicsCollisionCfg(), - physics_material=sim_utils.RigidBodyMaterialCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 0.0)), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)), - ) - - imu = ImuCfg( - prim_path="{ENV_REGEX_NS}/Cube", - ) - - -def test_freefall_acceleration(sim): - """Test that a freefalling IMU measures near-zero acceleration.""" - scene_cfg = FreefallSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - # Step a few times while the cube is in freefall (no ground contact) - for _ in range(10): - sim.step() - scene.update(sim.get_physics_dt()) - - imu: Imu = scene["imu"] - lin_acc = imu.data.lin_acc_b.torch - - # In freefall, accelerometer should read near zero (gravity and inertial acceleration cancel) - acc_magnitude = torch.norm(lin_acc, dim=-1) - torch.testing.assert_close( - acc_magnitude, - torch.zeros_like(acc_magnitude), - atol=0.5, - rtol=0.0, - ) - - def test_no_stale_data_after_scene_reset(sim): - """Regression for #4970: ``scene.reset(env_ids)`` must not surface pre-reset IMU values (Newton). + """Regression for #4970: resets must not surface pre-reset IMU values (Newton). - Mirrors the PhysX equivalent. Reproduces the ``ManagerBasedRLEnv._reset_idx`` flow where - reset runs inside a step without a subsequent physics step; the IMU sensor's lazy ``data`` - accessor must not refetch from the Newton rigid-body view here (the velocity buffer reflects - the previous step and would produce a spurious finite-difference acceleration). + Reproduces the ``ManagerBasedRLEnv._reset_idx`` flow, where reset runs inside a step without a + subsequent physics step: Newton's accelerometer still holds the pre-reset reading, so the public + ``data`` accessor must return the zeroed buffers instead of refetching it. The cube rests on the + ground first so the stale reading (gravity, ~9.81 m/s^2) is distinguishable from a reset one. """ - scene_cfg = ImuTestSceneCfg(num_envs=1) + scene_cfg = ImuTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() scene.reset() imu: Imu = scene["imu"] - cube: RigidObject = scene["cube"] - # Let the cube fall so the rigid-body view accumulates a non-zero velocity. - for _ in range(30): + # The cube falls from z=1.0 (bottom at z=0.9) and lands in ~86 steps at 200 Hz; 200 steps let it settle. + for _ in range(200): scene.write_data_to_sim() sim.step(render=False) scene.update(dt=sim.get_physics_dt()) - # Reset the scene without writing fresh velocity/transform. The Newton velocity - # buffer therefore still holds the pre-reset (falling) value. - env_ids = torch.tensor([0], device=cube.device) - scene.reset(env_ids=env_ids) - - # The public ``data`` accessor must not refetch a stale physics buffer; ``reset()`` zeroes - # ``_lin_acc_b`` / ``_ang_vel_b`` and those must be what comes out here. - post_reset_lin_acc = imu.data.lin_acc_b.torch - post_reset_ang_vel = imu.data.ang_vel_b.torch - torch.testing.assert_close(post_reset_lin_acc, torch.zeros_like(post_reset_lin_acc)) - torch.testing.assert_close(post_reset_ang_vel, torch.zeros_like(post_reset_ang_vel)) + pre_reset_lin_acc = imu.data.lin_acc_b.torch.clone() + assert (pre_reset_lin_acc[:, 2] > 5.0).all(), f"Expected a settled gravity reading, got {pre_reset_lin_acc}" + + # Partial reset: env 0 reads zeros, env 1 keeps its measurement. + scene.reset(env_ids=torch.tensor([0], device=imu.device)) + lin_acc = imu.data.lin_acc_b.torch + ang_vel = imu.data.ang_vel_b.torch + torch.testing.assert_close(lin_acc[0], torch.zeros_like(lin_acc[0])) + torch.testing.assert_close(ang_vel[0], torch.zeros_like(ang_vel[0])) + torch.testing.assert_close(lin_acc[1], pre_reset_lin_acc[1]) + + # Full reset zeroes every environment. + imu.reset() + lin_acc = imu.data.lin_acc_b.torch + ang_vel = imu.data.ang_vel_b.torch + torch.testing.assert_close(lin_acc, torch.zeros_like(lin_acc)) + torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel)) 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 dd54dbb9ea9..95b1f40de1d 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -92,16 +92,6 @@ class _SingleJointSceneCfg(InteractiveSceneCfg): wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") -@configclass -class _CartpoleSceneCfg(InteractiveSceneCfg): - """Scene with a cartpole (2-joint) articulation and the joint-wrench sensor.""" - - env_spacing = 4.0 - terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") - robot = _make_cartpole_articulation_cfg() - wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") - - @configclass class _CartpoleDampedSceneCfg(InteractiveSceneCfg): """Cartpole with pole damping for steady-state physics validation tests.""" @@ -152,46 +142,10 @@ def test_data_before_init_is_none(): # --------------------------------------------------------------------------- -# Initialization and shapes +# Initialization # --------------------------------------------------------------------------- -def test_initialization_and_shapes(sim): - """Sensor initializes on sim reset and exposes correctly-shaped buffers.""" - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) - sim.reset() - - robot: Articulation = scene["robot"] - sensor: JointWrenchSensor = scene["wrench"] - sim.step() - scene.update(sim.get_physics_dt()) - - # revolute_articulation has one joint whose child is "Arm". - num_envs = 2 - num_joints = 1 - assert sensor.data.force.torch.shape == (num_envs, num_joints, 3) - assert sensor.data.torque.torch.shape == (num_envs, num_joints, 3) - assert sensor.body_names == ["Arm"] - assert sensor._root_view is robot.root_view # noqa: SLF001 - - -def test_multi_body_articulation(sim): - """Cartpole (2 joints) exposes a wrench for each joint labelled by its child body.""" - scene = InteractiveScene(_CartpoleSceneCfg(num_envs=2)) - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - sim.step() - scene.update(sim.get_physics_dt()) - - num_envs = 2 - num_joints = 2 - assert sensor.data.force.torch.shape == (num_envs, num_joints, 3) - assert sensor.data.torque.torch.shape == (num_envs, num_joints, 3) - assert len(sensor.body_names) == 2 - assert "rail" not in [n.lower() for n in sensor.body_names] - - def test_nested_articulation_root_resolution(sim): """Sensor covers a nested articulation root from the configured asset prefix.""" scene = InteractiveScene(_NestedRootAntSceneCfg(num_envs=1)) @@ -210,163 +164,40 @@ def test_nested_articulation_root_resolution(sim): # --------------------------------------------------------------------------- # Physical correctness +# +# The joint-frame convention (orientation and anchor) is owned by the shared ``test_joint_wrench_frame`` +# contract. The tests below check load paths with frame-independent quantities from public asset data. # --------------------------------------------------------------------------- -def _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env: int, - joint: int, - gravity: torch.Tensor, - ext_force_b: torch.Tensor | None = None, - ext_torque_b: torch.Tensor | None = None, - descendant_body_names: list[str] | None = None, -): - """Compute the analytical joint-frame wrench for a single joint. - - Uses the same geometric data (body_com, joint_X_c, body_q) and frame - transformations as the kernel, but computes the wrench analytically from - known loads rather than reading body_parent_f. Computes the moment of - forces about the joint anchor and rotates the result into the child-side - joint frame. - - For terminal links, the wrench is due to the child body alone. For - interior joints, pass all bodies in the subtree below the joint via - ``descendant_body_names`` so the helper sums their gravitational - contributions. +def test_wrench_with_external_force_and_torque(sim): + """External loads applied through the wrench composer reach the reported joint wrench. - Args: - sensor: The JointWrenchSensor instance (used to read Newton model bindings). - robot: The Articulation asset (used for body mass lookup). - env: Environment index. - joint: Joint index within the sensor. - gravity: Gravity vector in world frame, shape (3,). - ext_force_b: External force on the child body in body frame [N], shape (3,). - ext_torque_b: External torque on the child body in body frame [N·m], shape (3,). - descendant_body_names: Bodies whose gravitational load acts through this - joint. Defaults to the child body only (correct for terminal links). - For an interior joint, pass all bodies in the subtree below the joint. - - Returns: - A tuple of (force, torque) tensors, each shape (3,), in the child-side - joint frame. + The arm first settles under gravity alone, then under an additional body-frame force and torque. Force + magnitudes are frame-independent, and so is ``dF . dtau``: with ``dF = -f`` and + ``dtau = -(tau + r x f)`` for any anchor offset ``r``, it equals ``f . tau``. """ - body_idx = wp.to_torch(sensor._joint_child)[joint].item() - - # Link transform in world (of the child body — defines the joint frame). - link_xform = wp.to_torch(sensor._sim_bind_body_q)[env, body_idx] # (7,) = pos(3) + quat(4) - link_pos = link_xform[:3] - link_quat = link_xform[3:] # wp.quatf = (x, y, z, w) - - # Joint anchor and orientation in world = link_xform * joint_X_c. - joint_X_c = wp.to_torch(sensor._sim_bind_joint_X_c)[env, joint] # (7,) - jxc_pos = joint_X_c[:3] - jxc_quat = joint_X_c[3:] - anchor_world = link_pos + math_utils.quat_apply(link_quat.unsqueeze(0), jxc_pos.unsqueeze(0)).squeeze(0) - joint_quat_world = math_utils.quat_mul(link_quat.unsqueeze(0), jxc_quat.unsqueeze(0)).squeeze(0) - - # Bodies whose weight contributes to the wrench at this joint. - if descendant_body_names is None: - descendant_body_names = [sensor.body_names[joint]] - - link_names = list(sensor._root_view.link_names) - - total_force_w = torch.zeros(3, device=gravity.device) - total_torque_w = torch.zeros(3, device=gravity.device) - - for body_name in descendant_body_names: - b_idx = link_names.index(body_name) - b_xform = wp.to_torch(sensor._sim_bind_body_q)[env, b_idx] - b_pos = b_xform[:3] - b_quat = b_xform[3:] - b_com_local = wp.to_torch(sensor._sim_bind_body_com)[env, b_idx] - b_com_world = b_pos + math_utils.quat_apply(b_quat.unsqueeze(0), b_com_local.unsqueeze(0)).squeeze(0) - - art_b_idx = robot.body_names.index(body_name) - mass = robot.data.body_mass.torch[env, art_b_idx].item() - weight_w = mass * gravity - - total_force_w = total_force_w + weight_w - r = b_com_world - anchor_world - total_torque_w = total_torque_w + torch.cross(r, weight_w, dim=-1) - - # External force/torque on the child body only (if provided). Actuator - # torque is intentionally omitted; see tolerance comment in calling tests. - if ext_force_b is not None: - ext_force_w = math_utils.quat_apply(link_quat.unsqueeze(0), ext_force_b.unsqueeze(0)).squeeze(0) - total_force_w = total_force_w + ext_force_w - # Moment of the external force about the joint anchor (applied at child COM). - child_com_local = wp.to_torch(sensor._sim_bind_body_com)[env, body_idx] - child_com_world = link_pos + math_utils.quat_apply( - link_quat.unsqueeze(0), child_com_local.unsqueeze(0) - ).squeeze(0) - total_torque_w = total_torque_w + torch.cross(child_com_world - anchor_world, ext_force_w, dim=-1) - if ext_torque_b is not None: - total_torque_w = total_torque_w + math_utils.quat_apply( - link_quat.unsqueeze(0), ext_torque_b.unsqueeze(0) - ).squeeze(0) - - # Reaction wrench = negation of total wrench (joint supports against all loads). - reaction_force_w = -total_force_w - reaction_torque_w = -total_torque_w - - # Rotate into joint frame. - expected_force = math_utils.quat_apply_inverse( - joint_quat_world.unsqueeze(0), reaction_force_w.unsqueeze(0) - ).squeeze(0) - expected_torque = math_utils.quat_apply_inverse( - joint_quat_world.unsqueeze(0), reaction_torque_w.unsqueeze(0) - ).squeeze(0) - - return expected_force, expected_torque - - -def test_force_and_torque_components_at_rest(sim): - """Component-level validation of force and torque against analytical expectations (gravity only).""" scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) sim.reset() sensor: JointWrenchSensor = scene["wrench"] robot: Articulation = scene["robot"] + arm_idx = robot.body_names.index("Arm") + gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + weight_w = robot.data.body_mass.torch[0, arm_idx] * gravity + for _ in range(400): sim.step() scene.update(sim.get_physics_dt()) + force_gravity = sensor.data.force.torch[0, 0].clone() + torque_gravity = sensor.data.torque.torch[0, 0].clone() + torch.testing.assert_close(force_gravity.norm(), weight_w.norm(), atol=1e-2, rtol=1e-3) - gravity = torch.tensor(sim.cfg.gravity, device=sim.device) - expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env=0, - joint=0, - gravity=gravity, - ) - - 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) - - -def test_wrench_with_external_force_and_torque(sim): - """Full analytical wrench validation with external force and torque applied. - - Mirrors the PhysX ``test_body_incoming_joint_wrench_b_single_joint`` pattern: - apply a known wrench, settle, compute the expected reaction wrench analytically, - and compare component-by-component. - """ - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - robot: Articulation = scene["robot"] - arm_idx = robot.body_names.index("Arm") - - # Apply 10 N in body-Y and 10 N·m in body-Z on the arm (matches PhysX test). + # Force on every axis makes the dot-product check sensitive to each torque component. ext_force_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) - ext_force_b[:, arm_idx, 1] = 10.0 + ext_force_b[:, arm_idx, :] = torch.tensor([5.0, 10.0, 10.0], device=sim.device) ext_torque_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) + ext_torque_b[:, arm_idx, 1] = 5.0 ext_torque_b[:, arm_idx, 2] = 10.0 for _ in range(800): @@ -375,24 +206,17 @@ def test_wrench_with_external_force_and_torque(sim): sim.step() scene.update(sim.get_physics_dt()) - gravity = torch.tensor(sim.cfg.gravity, device=sim.device) - expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env=0, - joint=0, - gravity=gravity, - ext_force_b=ext_force_b[0, arm_idx], - ext_torque_b=ext_torque_b[0, arm_idx], - ) - force = sensor.data.force.torch[0, 0] torque = sensor.data.torque.torch[0, 0] - - # The PD actuator contributes a small torque (~0.1 N·m) to body_parent_f that is - # not modelled in the analytical helper. Force is unaffected (actuator is pure torque). - torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) - torch.testing.assert_close(torque, expected_torque, atol=0.15, rtol=1e-2) + arm_quat_w = robot.data.body_link_quat_w.torch[0, arm_idx] + ext_force_w = math_utils.quat_apply(arm_quat_w.unsqueeze(0), ext_force_b[0, arm_idx].unsqueeze(0)).squeeze(0) + torch.testing.assert_close(force.norm(), (weight_w + ext_force_w).norm(), atol=1e-2, rtol=1e-3) + torch.testing.assert_close((force - force_gravity).norm(), ext_force_b[0, arm_idx].norm(), atol=1e-2, rtol=1e-3) + # The PD actuator adds a small torque (~0.1 N·m) along the joint axis that is not modelled here. + expected_dot = torch.dot(ext_force_b[0, arm_idx], ext_torque_b[0, arm_idx]) + torch.testing.assert_close( + torch.dot(force - force_gravity, torque - torque_gravity), expected_dot, atol=1.5, rtol=0.0 + ) @pytest.mark.parametrize("fixed_pole", [False, True]) @@ -421,13 +245,15 @@ def test_interior_joint_wrench_at_rest(sim, tmp_path, fixed_pole): sim.step() scene.update(sim.get_physics_dt()) + assert sensor.data.force.torch.shape == sensor.data.torque.torch.shape == (1, 2, 3) + # Each joint carries the weight of its subtree, whatever frame the force is expressed in. gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + masses = robot.data.body_mass.torch[0] 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 + subtree_mass = sum(masses[robot.body_names.index(name)] for name in descendants) + torch.testing.assert_close( + sensor.data.force.torch[0, joint].norm(), (subtree_mass * gravity).norm(), atol=1e-2, rtol=1e-3 ) - 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) # --------------------------------------------------------------------------- @@ -435,43 +261,15 @@ def test_interior_joint_wrench_at_rest(sim, tmp_path, fixed_pole): # --------------------------------------------------------------------------- -def test_reset_zeros_selected_then_all_envs(sim): - """Partial reset zeros only the selected envs; a full reset clears every force / torque buffer.""" - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=4)) - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - for _ in range(100): - sim.step() - scene.update(sim.get_physics_dt()) - - force_before = sensor.data.force.torch.clone() - assert torch.all(torch.any(force_before != 0, dim=(1, 2))), "Expected non-zero data in every env before reset" - - sensor.reset(env_ids=[0, 2]) - - # Access raw buffers to skip lazy re-population from the Newton view on the next data read. - force_after = wp.to_torch(sensor._data._force) - torch.testing.assert_close(force_after[0], torch.zeros_like(force_after[0])) - torch.testing.assert_close(force_after[2], torch.zeros_like(force_after[2])) - torch.testing.assert_close(force_after[1], force_before[1]) - torch.testing.assert_close(force_after[3], force_before[3]) - - sensor.reset() - - force_after = wp.to_torch(sensor._data._force) - torque_after = wp.to_torch(sensor._data._torque) - torch.testing.assert_close(force_after, torch.zeros_like(force_after)) - torch.testing.assert_close(torque_after, torch.zeros_like(torque_after)) - - def test_no_stale_data_after_scene_reset(sim): """Regression for #4970: ``scene.reset(env_ids)`` must not surface pre-reset wrenches (Newton). Mirrors the PhysX equivalent. The joint-wrench sensor's lazy ``data`` accessor must not refetch from the Newton articulation view here (the wrench buffer reflects the previous step). + A partial reset leaves the other envs untouched and a full sensor reset zeroes every env. """ - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) + num_envs = 4 + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=num_envs)) sim.reset() sensor: JointWrenchSensor = scene["wrench"] @@ -479,11 +277,23 @@ def test_no_stale_data_after_scene_reset(sim): sim.step() scene.update(sim.get_physics_dt()) + # revolute_articulation has one joint whose child is "Arm". + assert sensor.body_names == ["Arm"] + assert sensor.data.force.torch.shape == sensor.data.torque.torch.shape == (num_envs, 1, 3) pre_reset_force = sensor.data.force.torch.clone() - pre_reset_torque = sensor.data.torque.torch.clone() - assert torch.any(pre_reset_force != 0) or torch.any(pre_reset_torque != 0), "Expected non-zero wrench before reset" + assert torch.all(torch.any(pre_reset_force != 0, dim=(1, 2))), "Expected non-zero wrench in every env" + + scene.reset(env_ids=torch.tensor([0, 2], device=sensor.device)) - scene.reset(env_ids=torch.tensor([0], device=sensor.device)) + post_reset_force = sensor.data.force.torch + post_reset_torque = sensor.data.torque.torch + for env in (0, 2): + torch.testing.assert_close(post_reset_force[env], torch.zeros_like(post_reset_force[env])) + torch.testing.assert_close(post_reset_torque[env], torch.zeros_like(post_reset_torque[env])) + for env in (1, 3): + torch.testing.assert_close(post_reset_force[env], pre_reset_force[env]) + + sensor.reset() post_reset_force = sensor.data.force.torch post_reset_torque = sensor.data.torque.torch diff --git a/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py b/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py index 9cc18abc591..a28adf49236 100644 --- a/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py @@ -124,11 +124,19 @@ def _step_and_read(sim, scene) -> NewtonRaycastSensor: return scene["raycast"] -@pytest.mark.parametrize("global_world_only", [False, True]) -def test_rays_hit_ground_plane(sim, global_world_only): - """All rays of a downward grid pattern hit the global-world ground at the sensor height.""" - scene_cfg = RaycastTestSceneCfg(num_envs=2) - scene_cfg.raycast.global_world_only = global_world_only +# Graph mode and ``global_world_only`` select independent branches, so each value is covered once. +@pytest.mark.parametrize( + ("sim", "generic_cfg"), + [pytest.param(True, False, id="cuda_graph-newton_cfg"), pytest.param(False, True, id="eager-generic_cfg")], + indirect=["sim"], +) +def test_rays_hit_ground_plane(sim, generic_cfg): + """All rays of a downward grid pattern hit the global-world ground at the sensor height. + + The generic row uses the backend-dispatching :class:`RayCasterCfg` with ``global_world_only=True``, + which must select the Newton BVH implementation. + """ + scene_cfg = GenericRaycastTestSceneCfg(num_envs=2) if generic_cfg else RaycastTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) expected_bvh_flags = ShapeFlags.VISIBLE | ShapeFlags.COLLIDE_SHAPES assert NewtonManager._sensor_bvh_shape_flags == expected_bvh_flags @@ -144,31 +152,16 @@ def test_rays_hit_ground_plane(sim, global_world_only): torch.testing.assert_close(distances, torch.full_like(distances, RAY_START_HEIGHT), atol=1e-3, rtol=0) expected_normal = torch.tensor([0.0, 0.0, 1.0], device=normals.device).expand_as(normals) torch.testing.assert_close(normals, expected_normal, atol=1e-3, rtol=0) + if generic_cfg: + assert isinstance(sensor, NewtonRaycastSensor) + # Camera and multi-mesh factories retain their explicit legacy implementations. + assert RayCasterCamera.resolve_class() is LegacyRayCasterCamera + assert MultiMeshRayCaster.resolve_class() is LegacyMultiMeshRayCaster + assert MultiMeshRayCasterCamera.resolve_class() is LegacyMultiMeshRayCasterCamera -def test_generic_ray_caster_uses_newton_scene_bvh(sim): - """The backend-dispatching ray caster selects the Newton BVH implementation.""" - scene = InteractiveScene(GenericRaycastTestSceneCfg(num_envs=1)) - sim.reset() - sensor = _step_and_read(sim, scene) - - assert isinstance(sensor, NewtonRaycastSensor) - assert hasattr(sensor.data, "ray_distances") - torch.testing.assert_close( - sensor.data.ray_distances.torch, - torch.full_like(sensor.data.ray_distances.torch, RAY_START_HEIGHT), - atol=1e-3, - rtol=0, - ) - - -def test_remaining_warp_mesh_factories_select_legacy_newton_adapters(sim): - """Camera and multi-mesh factories retain their explicit legacy implementations.""" - assert RayCasterCamera.resolve_class() is LegacyRayCasterCamera - assert MultiMeshRayCaster.resolve_class() is LegacyMultiMeshRayCaster - assert MultiMeshRayCasterCamera.resolve_class() is LegacyMultiMeshRayCasterCamera - - +# The legacy adapter does not use the Newton manager graph. +@pytest.mark.parametrize("sim", [pytest.param(False, id="eager")], indirect=True) def test_legacy_multi_mesh_tracks_ad_hoc_regex_target(sim): """Tracked target registration remains valid when discovery returns concrete owner paths.""" obstacle_cfg = sim_utils.CuboidCfg( @@ -202,7 +195,10 @@ def test_legacy_multi_mesh_tracks_ad_hoc_regex_target(sim): def test_bvh_refit_tracks_moving_geometry(sim): - """Sliding a box under the sensor changes the hits, proving the BVH refits live.""" + """Sliding a box under the sensor changes the hits, proving the BVH refits live. + + After a carrier pose write, the first sensor read and the pose getter both resolve pending FK. + """ scene = InteractiveScene(RaycastTestSceneCfg(num_envs=1)) sim.reset() sensor = _step_and_read(sim, scene) @@ -224,13 +220,7 @@ def test_bvh_refit_tracks_moving_geometry(sim): distances = sensor.data.ray_distances.torch torch.testing.assert_close(distances, torch.full_like(distances, RAY_START_HEIGHT - 1.0), atol=1e-3, rtol=0) - -def test_sensor_reads_refresh_fk_after_carrier_pose_write(sim): - """The first sensor read and the pose getter both resolve pending FK after a carrier pose write.""" - scene = InteractiveScene(RaycastTestSceneCfg(num_envs=1)) - sim.reset() - sensor = _step_and_read(sim, scene) - initial_distances = sensor.data.ray_distances.torch.clone() + initial_distances = distances.clone() initial_positions = sensor.get_world_poses()[0].torch.clone() # Lift the carrier: the first data read after the write must see the refreshed body_q. diff --git a/source/isaaclab_newton/test/sensors/test_pva.py b/source/isaaclab_newton/test/sensors/test_pva.py index 805f0871724..b008ca3ad3d 100644 --- a/source/isaaclab_newton/test/sensors/test_pva.py +++ b/source/isaaclab_newton/test/sensors/test_pva.py @@ -12,7 +12,6 @@ import pytest import torch -import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg import isaaclab.sim as sim_utils @@ -65,135 +64,31 @@ def sim(): yield sim -def test_initialization_and_data_shapes(sim): - """The Newton PVA sensor initializes and exposes correctly shaped, populated buffers after one step.""" - scene_cfg = PvaTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - pva: Pva = scene["pva"] - assert pva.num_instances == 2 - - sim.step() - scene.update(sim.get_physics_dt()) - - assert pva.data.pos_w.torch.shape == (2, 3) - assert pva.data.quat_w.torch.shape == (2, 4) - assert pva.data.pose_w.torch.shape == (2, 7) - assert pva.data.lin_vel_b.torch.shape == (2, 3) - assert pva.data.ang_vel_b.torch.shape == (2, 3) - assert pva.data.lin_acc_b.torch.shape == (2, 3) - assert pva.data.ang_acc_b.torch.shape == (2, 3) - assert pva.data.projected_gravity_b.torch.shape == (2, 3) - # The cube starts above the ground plane, so the world position is populated rather than zero. - assert torch.all(pva.data.pos_w.torch[:, 2] > 0.0), ( - f"Expected positive z position, got {pva.data.pos_w.torch[:, 2]}" - ) - - def test_at_rest_reports_gravity_and_zero_velocity(sim): - """A settled PVA sensor reports unit gravity along body -Z and near-zero velocity.""" - scene_cfg = PvaTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - # Cube falls from z=1.0 (bottom at z=0.9), reaches ground in ~86 steps at 200 Hz. - for _ in range(200): - sim.step() - scene.update(sim.get_physics_dt()) + """A settled PVA sensor reports unit gravity along body -Z and near-zero velocity. - pva: Pva = scene["pva"] - proj_grav = pva.data.projected_gravity_b.torch - lin_vel = pva.data.lin_vel_b.torch - ang_vel = pva.data.ang_vel_b.torch - - expected = torch.tensor([[0.0, 0.0, -1.0]], dtype=proj_grav.dtype, device=proj_grav.device).repeat(2, 1) - torch.testing.assert_close(proj_grav, expected, atol=0.05, rtol=0.0) - torch.testing.assert_close(lin_vel, torch.zeros_like(lin_vel), atol=0.05, rtol=0.0) - torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel), atol=0.05, rtol=0.0) - - -def test_reset(sim): - """Test that reset zeroes out PVA data.""" + While the cube still falls, PVA reports coordinate acceleration (from body_qdd), not proper + acceleration: (0, 0, -9.81) in the body frame of the upright cube, with a growing downward speed. + """ scene_cfg = PvaTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() - for _ in range(10): - sim.step() - scene.update(sim.get_physics_dt()) - pva: Pva = scene["pva"] + assert pva.num_instances == 2 - pos = pva.data.pos_w.torch - assert torch.any(pos != 0), "Expected non-zero data before reset" - - pva.reset() - - # Access internal buffers directly to avoid lazy re-evaluation via pva.data - # (the data property triggers _update_buffers_impl which would overwrite reset values). - pos = wp.to_torch(pva._data._pos_w) - lin_vel = wp.to_torch(pva._data._lin_vel_b) - ang_vel = wp.to_torch(pva._data._ang_vel_b) - lin_acc = wp.to_torch(pva._data._lin_acc_b) - ang_acc = wp.to_torch(pva._data._ang_acc_b) - quat = wp.to_torch(pva._data._quat_w) - - torch.testing.assert_close(pos, torch.zeros_like(pos)) - torch.testing.assert_close(lin_vel, torch.zeros_like(lin_vel)) - torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel)) - torch.testing.assert_close(lin_acc, torch.zeros_like(lin_acc)) - torch.testing.assert_close(ang_acc, torch.zeros_like(ang_acc)) - expected_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=quat.dtype, device=quat.device).repeat(2, 1) - torch.testing.assert_close(quat, expected_quat) - - -@configclass -class FreefallSceneCfg(InteractiveSceneCfg): - """Scene with a rigid cube and PVA but no ground plane (freefall).""" - - env_spacing = 2.0 - cube = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), - mass_props=sim_utils.MassCfg(mass=1.0), - collision_props=sim_utils.UsdPhysicsCollisionCfg(), - physics_material=sim_utils.RigidBodyMaterialCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 0.0)), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)), - ) - - pva = PvaCfg( - prim_path="{ENV_REGEX_NS}/Cube", - ) - - -def test_freefall_reports_gravity_acceleration_and_growing_speed(sim): - """A freefalling body reports coordinate acceleration equal to gravity and a growing downward speed. - - PVA reports coordinate acceleration (from body_qdd), not proper acceleration. - In freefall, coordinate acceleration equals gravitational acceleration (~9.81 m/s^2 - downward). For an upright body, this is (0, 0, -9.81) in the body frame. - """ - scene_cfg = FreefallSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - + # Cube falls from z=1.0 (bottom at z=0.9), reaches ground in ~86 steps at 200 Hz. for _ in range(10): sim.step() scene.update(sim.get_physics_dt()) - pva: Pva = scene["pva"] + assert pva.data.quat_w.torch.shape == (2, 4) + torch.testing.assert_close(pva.data.pose_w.torch[:, :3], pva.data.pos_w.torch) + torch.testing.assert_close(pva.data.pose_w.torch[:, 3:], pva.data.quat_w.torch) lin_acc = pva.data.lin_acc_b.torch ang_acc = pva.data.ang_acc_b.torch - - # Coordinate acceleration in freefall should be ~(0, 0, -9.81) in body frame. expected_acc = torch.tensor([[0.0, 0.0, -9.81]], dtype=lin_acc.dtype, device=lin_acc.device).repeat(2, 1) torch.testing.assert_close(lin_acc, expected_acc, atol=0.5, rtol=0.0) - # Angular acceleration should be near zero (no torques in freefall). torch.testing.assert_close(ang_acc, torch.zeros_like(ang_acc), atol=0.05, rtol=0.0) @@ -201,11 +96,23 @@ def test_freefall_reports_gravity_acceleration_and_growing_speed(sim): for _ in range(40): sim.step() scene.update(sim.get_physics_dt()) - speed = torch.norm(pva.data.lin_vel_b.torch, dim=-1) assert torch.all(speed > 0.1), f"Expected non-zero velocity in freefall, got {speed}" assert torch.all(speed > early_speed), f"Expected speed to grow in freefall, got {early_speed} -> {speed}" + for _ in range(150): + sim.step() + scene.update(sim.get_physics_dt()) + + proj_grav = pva.data.projected_gravity_b.torch + lin_vel = pva.data.lin_vel_b.torch + ang_vel = pva.data.ang_vel_b.torch + + expected = torch.tensor([[0.0, 0.0, -1.0]], dtype=proj_grav.dtype, device=proj_grav.device).repeat(2, 1) + torch.testing.assert_close(proj_grav, expected, atol=0.05, rtol=0.0) + torch.testing.assert_close(lin_vel, torch.zeros_like(lin_vel), atol=0.05, rtol=0.0) + torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel), atol=0.05, rtol=0.0) + @configclass class OffsetRotatedSceneCfg(InteractiveSceneCfg): @@ -213,7 +120,7 @@ class OffsetRotatedSceneCfg(InteractiveSceneCfg): The cube is rotated 90 degrees about the X axis and the sensor has a +Z offset of 0.5 m in the body frame. This exercises the lever-arm - velocity/acceleration corrections and body-frame gravity projection. + position offset and body-frame gravity projection. """ env_spacing = 2.0 @@ -276,9 +183,10 @@ def test_no_stale_data_after_scene_reset(sim): Mirrors the PhysX equivalent. The PVA sensor's lazy ``data`` accessor must not refetch from the Newton rigid-body view here (the velocity buffer reflects the previous step and would - produce spurious finite-difference accelerations). + produce spurious finite-difference accelerations). A partial reset leaves other envs untouched + and a full sensor reset zeroes every output. """ - scene_cfg = PvaTestSceneCfg(num_envs=1) + scene_cfg = PvaTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() scene.reset() @@ -292,8 +200,9 @@ def test_no_stale_data_after_scene_reset(sim): sim.step(render=False) scene.update(dt=sim.get_physics_dt()) - pre_reset_vel_mag = torch.linalg.norm(pva.data.lin_vel_b.torch, dim=-1).item() - assert pre_reset_vel_mag > 0.05, f"Expected non-zero velocity before reset; got {pre_reset_vel_mag!r}" + pre_reset_vel = pva.data.lin_vel_b.torch.clone() + pre_reset_vel_mag = torch.linalg.norm(pre_reset_vel, dim=-1) + assert (pre_reset_vel_mag > 0.05).all(), f"Expected non-zero velocity before reset; got {pre_reset_vel_mag!r}" # Reset the scene without writing fresh velocity/transform. The Newton velocity buffer # therefore still holds the pre-reset (falling) value. @@ -302,5 +211,15 @@ def test_no_stale_data_after_scene_reset(sim): post_reset_vel = pva.data.lin_vel_b.torch post_reset_acc = pva.data.lin_acc_b.torch - torch.testing.assert_close(post_reset_vel, torch.zeros_like(post_reset_vel)) - torch.testing.assert_close(post_reset_acc, torch.zeros_like(post_reset_acc)) + torch.testing.assert_close(post_reset_vel[0], torch.zeros_like(post_reset_vel[0])) + torch.testing.assert_close(post_reset_acc[0], torch.zeros_like(post_reset_acc[0])) + torch.testing.assert_close(post_reset_vel[1], pre_reset_vel[1]) + + # Full reset zeroes every environment. + pva.reset() + for name in ("pos_w", "lin_vel_b", "ang_vel_b", "lin_acc_b", "ang_acc_b"): + value = getattr(pva.data, name).torch + torch.testing.assert_close(value, torch.zeros_like(value), msg=name) + quat = pva.data.quat_w.torch + expected_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=quat.dtype, device=quat.device).repeat(2, 1) + torch.testing.assert_close(quat, expected_quat) diff --git a/source/isaaclab_newton/test/sensors/test_site_injection.py b/source/isaaclab_newton/test/sensors/test_site_injection.py index 686086f6a03..7ef68b1ed8e 100644 --- a/source/isaaclab_newton/test/sensors/test_site_injection.py +++ b/source/isaaclab_newton/test/sensors/test_site_injection.py @@ -18,40 +18,12 @@ class TestTransformToVecQuat: - """Tests for the zero-copy view split utility.""" - - def test_1d_pos_quat_split(self): - """1D array: position is first 3 floats, quaternion is last 4.""" - t = wp.zeros(3, dtype=wp.transformf, device="cpu") - pos, quat = transform_to_vec_quat(t) - assert pos.shape == (3,) - assert quat.shape == (3,) - assert pos.dtype == wp.vec3f - assert quat.dtype == wp.quatf - - def test_2d_pos_quat_split(self): - """2D array: shapes are (N, M) with vec3f and quatf dtypes.""" - t = wp.zeros((2, 4), dtype=wp.transformf, device="cpu") - pos, quat = transform_to_vec_quat(t) - assert pos.shape == (2, 4) - assert quat.shape == (2, 4) - assert pos.dtype == wp.vec3f - assert quat.dtype == wp.quatf - - def test_zero_copy_1d(self): - """Writes through pos/quat views are reflected in the original transform array.""" - t = wp.zeros(1, dtype=wp.transformf, device="cpu") - pos, quat = transform_to_vec_quat(t) - # Write known values through the views - pos.numpy()[0] = (1.0, 2.0, 3.0) - quat.numpy()[0] = (0.0, 0.0, 0.0, 1.0) - floats = t.view(wp.float32).numpy() - assert list(floats[0]) == pytest.approx([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]) + """Error paths of the zero-copy view split utility; values are covered by the frame-transformer tests.""" def test_invalid_ndim_raises(self): - """Passing a 0D or 4D array raises an error.""" - with pytest.raises((ValueError, IndexError)): - transform_to_vec_quat(wp.zeros((), dtype=wp.transformf, device="cpu")) + """Passing a 4D array raises the documented ValueError rather than a Warp view error.""" + with pytest.raises(ValueError, match="ndim=4"): + transform_to_vec_quat(wp.zeros((1, 1, 1, 1), dtype=wp.transformf, device="cpu")) def test_wrong_dtype_raises(self): """Passing wrong dtype raises TypeError.""" @@ -93,36 +65,9 @@ def test_global_site_entry_is_int_none_tuple(self): global_idx, per_world = entry assert isinstance(global_idx, int) assert per_world is None - - def test_global_site_pending_cleared(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {(None, False, tuple(xform)): ("ft_0", xform)} - NewtonManager._cl_inject_sites_fallback() - assert len(NewtonManager._cl_pending_sites) == 0 -class TestFallbackLocalSingleBody: - """Single-body local site must produce a (None, [[idx]]) entry — one world.""" - - def setup_method(self): - NewtonManager.clear() - NewtonManager._builder = MockBuilder(["Robot/base", "Robot/hand"]) - - def test_single_body_entry_shape(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {("Robot/base", False, tuple(xform)): ("ft_0", xform)} - NewtonManager._cl_inject_sites_fallback() - - entry = NewtonManager._cl_site_index_map["ft_0"] - global_idx, per_world = entry - assert global_idx is None - assert isinstance(per_world, list) - assert len(per_world) == 1 # one world - assert len(per_world[0]) == 1 # one match - assert isinstance(per_world[0][0], int) - - class TestFallbackLocalWildcard: """Wildcard local site matching N bodies must produce (None, [[idx0..idxN-1]]) — one world.""" @@ -204,11 +149,6 @@ def _make_site_map( class TestSourceValidation: - def test_valid_source_one_per_env(self): - site_map = _make_site_map([[10], [20]], []) - indices, _ = FrameTransformer._validate_site_map("source", "/Robot/base", [], [], site_map, num_envs=2) - assert indices == [10, 20] - def test_source_wrong_env_count_raises(self): # site map has 1 world entry but num_envs=2 site_map = _make_site_map([[10]], []) @@ -227,20 +167,6 @@ def test_source_two_in_env_raises(self): class TestTargetValidation: - def test_valid_single_target_per_env(self): - site_map = _make_site_map([[10], [20]], [[[30], [40]]]) - _, tgt = FrameTransformer._validate_site_map( - "source", "/Robot/base", ["target_0"], ["/Robot/hand"], site_map, num_envs=2 - ) - assert tgt[0] == [[30], [40]] - - def test_valid_wildcard_two_bodies_per_env(self): - site_map = _make_site_map([[10], [20]], [[[30, 31], [40, 41]]]) - _, tgt = FrameTransformer._validate_site_map( - "source", "/Robot/base", ["target_0"], ["/Robot/foot.*"], site_map, num_envs=2 - ) - assert tgt[0] == [[30, 31], [40, 41]] - def test_target_zero_bodies_raises(self): site_map = _make_site_map([[10], [20]], [[[], []]]) with pytest.raises(ValueError, match="matched no bodies"): @@ -282,63 +208,3 @@ def test_zero_targets_shapes_refs(self): assert refs == [0, 0] assert names == [] assert tgt_per_tgt == [] - - -class TestSingleTarget: - def test_one_env_one_target(self): - """1 env, 1 target: [src, tgt] shapes, [world_orig, src] refs.""" - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10], - target_per_world=[[[20]]], - target_frame_body_names=["hand"], - shape_labels={}, - world_origin_idx=0, - num_envs=1, - ) - assert shapes == [10, 20] - assert refs == [0, 10] - assert names == ["hand"] - - def test_two_envs_two_targets(self): - """2 envs, 2 targets: stride-2 interleaved layout.""" - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10, 11], - target_per_world=[[[20], [21]], [[30], [31]]], - target_frame_body_names=["arm", "hand"], - shape_labels={}, - world_origin_idx=0, - num_envs=2, - ) - assert shapes == [10, 20, 30, 11, 21, 31] - assert refs == [0, 10, 10, 0, 11, 11] - assert names == ["arm", "hand"] - - -class TestWildcardExpansion: - def test_wildcard_two_bodies_per_env(self): - """Wildcard: 2 bodies per env expand to 2 target entries with names derived from shape_labels.""" - shape_labels = {20: "FL_foot/label_0", 21: "FL_foot/label_0", 22: "FR_foot/label_0", 23: "FR_foot/label_0"} - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10, 11], - target_per_world=[[[20, 22], [21, 23]]], - target_frame_body_names=["foot"], - shape_labels=shape_labels, - world_origin_idx=0, - num_envs=2, - ) - assert shapes == [10, 20, 22, 11, 21, 23] - assert refs == [0, 10, 10, 0, 11, 11] - assert tgt_per_tgt == [[20, 21], [22, 23]] - assert names == ["FL_foot", "FR_foot"] - - def test_wildcard_single_body_uses_config_name(self): - """Single body match: config name is used regardless of shape_labels.""" - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10, 11], - target_per_world=[[[20], [21]]], - target_frame_body_names=["foot"], - shape_labels={}, - world_origin_idx=0, - num_envs=2, - ) - assert names == ["foot"] diff --git a/source/isaaclab_newton/test/sim/test_cable_usd_import.py b/source/isaaclab_newton/test/sim/test_cable_usd_import.py index 1ddf6b04d04..79d36c9145c 100644 --- a/source/isaaclab_newton/test/sim/test_cable_usd_import.py +++ b/source/isaaclab_newton/test/sim/test_cable_usd_import.py @@ -32,17 +32,11 @@ def _import_cable_joint_stiffness(**material_kwargs) -> list[float]: return [builder.joint_target_ke[dof0 + offset] for offset in range(4)] -def test_newton_cable_shear_and_twist_fall_back_when_unset(): - """Test that unset shear/twist reuse the stretch/bend stiffness.""" - stiffness = _import_cable_joint_stiffness() - - assert stiffness[_SHEAR] == pytest.approx(stiffness[_STRETCH]) - assert stiffness[_TWIST] == pytest.approx(stiffness[_BEND]) - - def test_newton_cable_authored_shear_and_twist_override_fallbacks(): - """Test that authored moduli decouple shear from stretch and twist from bend.""" + """Test that unset shear/twist reuse stretch/bend and authored moduli decouple them.""" fallback = _import_cable_joint_stiffness() + assert fallback[_SHEAR] == pytest.approx(fallback[_STRETCH]) + assert fallback[_TWIST] == pytest.approx(fallback[_BEND]) stiffness = _import_cable_joint_stiffness(shear_stiffness=9.0e9, twist_stiffness=7.0e7) # Stretch and bend are untouched; only the newly authored degrees of freedom move. diff --git a/source/isaaclab_newton/test/sim/test_mpm_visualization.py b/source/isaaclab_newton/test/sim/test_mpm_visualization.py index d201c6eae78..3b7e283e098 100644 --- a/source/isaaclab_newton/test/sim/test_mpm_visualization.py +++ b/source/isaaclab_newton/test/sim/test_mpm_visualization.py @@ -44,7 +44,10 @@ def _create_visualization(monkeypatch, visual_material=None): def test_each_environment_renders_its_own_particle_slice(monkeypatch): - """Every environment gets a points prim carrying its own positions, plus the shared widths and color.""" + """Every environment gets a world-frame points prim with its own positions, plus the shared widths and color. + + Points are authored in the world frame, so the prims must reset the environment's xform stack. + """ stage, prim_paths = _create_visualization(monkeypatch) assert prim_paths == _PRIM_PATHS @@ -53,14 +56,7 @@ def test_each_environment_renders_its_own_particle_slice(monkeypatch): np.testing.assert_array_equal(np.asarray(points.GetPointsAttr().Get()), _POSITIONS[env_idx]) np.testing.assert_array_equal(np.asarray(points.GetWidthsAttr().Get()), _WIDTHS) assert points.GetDisplayColorAttr().Get() == [Gf.Vec3f(*_COLOR)] - - -def test_particle_clouds_ignore_the_inherited_environment_transform(monkeypatch): - """Points are authored in the world frame, so the prims must reset the environment's xform stack.""" - stage, prim_paths = _create_visualization(monkeypatch) - - for prim_path in prim_paths: - assert UsdGeom.Points(stage.GetPrimAtPath(prim_path)).GetResetXformStack() + assert points.GetResetXformStack() def test_prim_path_count_must_match_environment_count(monkeypatch): diff --git a/source/isaaclab_newton/test/sim/test_newton_schemas.py b/source/isaaclab_newton/test/sim/test_newton_schemas.py index 6619792aae6..9e07c1caa3b 100644 --- a/source/isaaclab_newton/test/sim/test_newton_schemas.py +++ b/source/isaaclab_newton/test/sim/test_newton_schemas.py @@ -19,11 +19,9 @@ MujocoJointDrivePropertiesCfg, MujocoRigidBodyPropertiesCfg, NewtonArticulationRootPropertiesCfg, - NewtonCollisionPropertiesCfg, NewtonJointDrivePropertiesCfg, NewtonMaterialPropertiesCfg, NewtonMeshCollisionPropertiesCfg, - NewtonRigidBodyPropertiesCfg, NewtonSDFCollisionPropertiesCfg, ) @@ -64,15 +62,6 @@ def _has_authored_api_schema(prim, schema_name: str) -> bool: # --------------------------------------------------------------------------- -@pytest.mark.isaacsim_ci -def test_newton_rigid_body_inherits_field_routing(setup_sim): - """Inherited disable_gravity must use the PhysX namespace consumed by Newton.""" - prim = sim_utils.create_prim("/World/newton_body", prim_type="Cube") - schemas.define_rigid_body_properties("/World/newton_body", NewtonRigidBodyPropertiesCfg(disable_gravity=True)) - assert prim.GetAttribute("physxRigidBody:disableGravity").Get() is True - assert not prim.GetAttribute("physics:disableGravity").IsValid() - - @pytest.mark.isaacsim_ci def test_mujoco_gravcomp_authored_only_when_set(setup_sim): """gravcomp=0.5 writes mjc:gravcomp=0.5; gravcomp=None leaves the attribute unauthored.""" @@ -128,32 +117,6 @@ def test_joint_drive_max_velocity_routes_to_physx_namespace(setup_sim, cfg_type) assert joint.GetPrim().GetAttribute("physxJoint:maxJointVelocity").Get() == pytest.approx(math.degrees(5.0)) -# --------------------------------------------------------------------------- -# Newton collision -# --------------------------------------------------------------------------- - - -@pytest.mark.isaacsim_ci -def test_newton_collision_schema_applied_only_when_set(setup_sim): - """contact_margin=0.01 writes newton:contactMargin and applies NewtonCollisionAPI; all-None applies nothing.""" - stage = sim_utils.get_current_stage() - sim_utils.create_prim("/World/col_newton", prim_type="Cube", translation=(2.0, 0.0, 0.5)) - schemas.define_collision_properties( - "/World/col_newton", NewtonCollisionPropertiesCfg(contact_margin=0.01, contact_offset=0.02, rest_offset=0.01) - ) - prim = stage.GetPrimAtPath("/World/col_newton") - assert prim.GetAttribute("physxCollision:contactOffset").Get() == pytest.approx(0.02) - assert prim.GetAttribute("physxCollision:restOffset").Get() == pytest.approx(0.01) - assert not prim.GetAttribute("physics:contactOffset").IsValid() - assert not prim.GetAttribute("physics:restOffset").IsValid() - assert prim.GetAttribute("newton:contactMargin").Get() == pytest.approx(0.01) - assert "NewtonCollisionAPI" in prim.GetAppliedSchemas() - - sim_utils.create_prim("/World/col_newton2", prim_type="Cube", translation=(3.0, 0.0, 0.5)) - schemas.define_collision_properties("/World/col_newton2", NewtonCollisionPropertiesCfg()) - assert "NewtonCollisionAPI" not in stage.GetPrimAtPath("/World/col_newton2").GetAppliedSchemas() - - # --------------------------------------------------------------------------- # Newton material # --------------------------------------------------------------------------- @@ -161,42 +124,28 @@ def test_newton_collision_schema_applied_only_when_set(setup_sim): @pytest.mark.isaacsim_ci def test_newton_material_schema_applied_only_when_set(setup_sim): - """Newton friction fields write newton:* attributes and apply NewtonMaterialAPI; all-None applies nothing.""" - mat_cfg = NewtonMaterialPropertiesCfg(torsional_friction=0.3, rolling_friction=0.001) + """Newton material fields write newton:* attributes and apply NewtonMaterialAPI; all-None applies nothing.""" + mat_cfg = NewtonMaterialPropertiesCfg( + torsional_friction=0.3, + rolling_friction=0.001, + contact_stiffness=1.0e4, + contact_damping=250.0, + contact_friction_gain=40.0, + contact_adhesion=0.02, + ) prim = spawn_rigid_body_material("/World/newton_mat", mat_cfg) assert prim.GetAttribute("newton:torsionalFriction").Get() == pytest.approx(0.3) assert prim.GetAttribute("newton:rollingFriction").Get() == pytest.approx(0.001) + assert prim.GetAttribute("newton:contactStiffness").Get() == pytest.approx(1.0e4) + assert prim.GetAttribute("newton:contactDamping").Get() == pytest.approx(250.0) + assert prim.GetAttribute("newton:contactFrictionGain").Get() == pytest.approx(40.0) + assert prim.GetAttribute("newton:contactAdhesion").Get() == pytest.approx(0.02) assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() prim = spawn_rigid_body_material("/World/newton_mat2", NewtonMaterialPropertiesCfg()) assert "NewtonMaterialAPI" not in prim.GetAppliedSchemas() -@pytest.mark.isaacsim_ci -def test_newton_material_fragment_composes_with_usd_physics_fragment(setup_sim): - """NewtonMaterialCfg is a rigid-body material fragment (backend symmetry with the PhysX - fragment): it must compose in a fragment list with UsdPhysicsRigidBodyMaterialCfg and author - both the ``newton:*`` and solver-common ``physics:*`` namespaces on the same material prim.""" - from isaaclab_newton.sim.spawners.materials import NewtonMaterialCfg - - from isaaclab.sim.spawners.materials.physics_materials import spawn_rigid_body_material_from_fragments - from isaaclab.sim.spawners.materials.physics_materials_cfg import UsdPhysicsRigidBodyMaterialCfg - - prim = spawn_rigid_body_material_from_fragments( - "/World/newton_mat_frag", - [ - UsdPhysicsRigidBodyMaterialCfg(static_friction=0.6, dynamic_friction=0.5), - NewtonMaterialCfg(torsional_friction=0.3, rolling_friction=0.001), - ], - ) - assert bool(UsdPhysics.MaterialAPI(prim)) - assert prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(0.6) - assert prim.GetAttribute("physics:dynamicFriction").Get() == pytest.approx(0.5) - assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() - assert prim.GetAttribute("newton:torsionalFriction").Get() == pytest.approx(0.3) - assert prim.GetAttribute("newton:rollingFriction").Get() == pytest.approx(0.001) - - @pytest.mark.isaacsim_ci def test_newton_material_fragment_authors_all_six_newton_attrs(setup_sim): """Regression test: Newton's USD material schema resolver (``SchemaResolverNewton``) reads six @@ -204,23 +153,29 @@ def test_newton_material_fragment_authors_all_six_newton_attrs(setup_sim): (``contactStiffness``/``contactDamping``/``contactFrictionGain``/``contactAdhesion``) that replace the deprecated per-shape ``ke``/``kd``/``kf``/``ka`` parameters. All six must round-trip through :class:`~isaaclab_newton.sim.spawners.materials.NewtonMaterialCfg`, even though the - generated ``NewtonMaterialAPI`` schema currently only declares the two friction attributes.""" + generated ``NewtonMaterialAPI`` schema currently only declares the two friction attributes. + + The fragment also composes with :class:`UsdPhysicsRigidBodyMaterialCfg` on the same prim.""" from isaaclab_newton.sim.spawners.materials import NewtonMaterialCfg from newton._src.usd.schema_resolver import PrimType from newton._src.usd.schemas import SchemaResolverNewton from isaaclab.sim.spawners.materials import spawn_rigid_body_material_from_fragments + from isaaclab.sim.spawners.materials.physics_materials_cfg import UsdPhysicsRigidBodyMaterialCfg prim = spawn_rigid_body_material_from_fragments( "/World/newton_mat_contact", - NewtonMaterialCfg( - torsional_friction=0.3, - rolling_friction=0.001, - contact_stiffness=2500.0, - contact_damping=100.0, - contact_friction_gain=1000.0, - contact_adhesion=0.01, - ), + [ + UsdPhysicsRigidBodyMaterialCfg(static_friction=0.6, dynamic_friction=0.5), + NewtonMaterialCfg( + torsional_friction=0.3, + rolling_friction=0.001, + contact_stiffness=2500.0, + contact_damping=100.0, + contact_friction_gain=1000.0, + contact_adhesion=0.01, + ), + ], ) expected = { "mu_torsional": 0.3, @@ -231,6 +186,9 @@ def test_newton_material_fragment_authors_all_six_newton_attrs(setup_sim): "ka": 0.01, } + assert bool(UsdPhysics.MaterialAPI(prim)) + assert prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(0.6) + assert prim.GetAttribute("physics:dynamicFriction").Get() == pytest.approx(0.5) assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() resolver = SchemaResolverNewton() assert set(resolver.mapping[PrimType.MATERIAL]) == set(expected) @@ -264,32 +222,6 @@ def test_newton_articulation_root_schema_applied_only_when_set(setup_sim): assert "NewtonArticulationRootAPI" not in stage.GetPrimAtPath("/World/nart2").GetAppliedSchemas() -# --------------------------------------------------------------------------- -# Newton mesh collision (max_hull_vertices, NewtonMeshCollisionAPI) -# --------------------------------------------------------------------------- - - -@pytest.mark.isaacsim_ci -def test_newton_mesh_collision_schema_applied_only_when_set(setup_sim): - """max_hull_vertices=64 writes newton:maxHullVertices and applies NewtonMeshCollisionAPI; None applies nothing.""" - stage = sim_utils.get_current_stage() - sim_utils.create_prim("/World/mesh_col", prim_type="Cube", translation=(4.0, 0.0, 0.5)) - schemas.define_mesh_collision_properties( - "/World/mesh_col", - NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull", max_hull_vertices=64), - ) - prim = stage.GetPrimAtPath("/World/mesh_col") - assert prim.GetAttribute("newton:maxHullVertices").Get() == 64 - assert "NewtonMeshCollisionAPI" in prim.GetAppliedSchemas() - - sim_utils.create_prim("/World/mesh_col2", prim_type="Cube", translation=(5.0, 0.0, 0.5)) - schemas.define_mesh_collision_properties( - "/World/mesh_col2", - NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull"), - ) - assert "NewtonMeshCollisionAPI" not in stage.GetPrimAtPath("/World/mesh_col2").GetAppliedSchemas() - - # --------------------------------------------------------------------------- # Newton SDF collision # --------------------------------------------------------------------------- @@ -351,24 +283,6 @@ def test_newton_sdf_collision_schema_not_applied_without_sdf_fields(setup_sim): assert not _has_authored_api_schema(prim, "NewtonSDFCollisionAPI") -# --------------------------------------------------------------------------- -# Class hierarchy contract: Mujoco IS-A Newton -# --------------------------------------------------------------------------- - - -def test_mujoco_isinstance_newton(): - """MujocoXxxCfg instances must be isinstance of their Newton parent. - - The auto-enable spawner logic and any future polymorphic dispatch on - ``isinstance(cfg, NewtonRigidBodyPropertiesCfg)`` depends on this contract. - """ - mjc_rigid = MujocoRigidBodyPropertiesCfg(gravcomp=0.5) - assert isinstance(mjc_rigid, NewtonRigidBodyPropertiesCfg) - - mjc_joint = MujocoJointDrivePropertiesCfg(actuatorgravcomp=True) - assert isinstance(mjc_joint, NewtonJointDrivePropertiesCfg) - - # --------------------------------------------------------------------------- # Multi-namespace mixed write — verify per-declaring-class MRO routing keeps # fields owned by different classes in different namespaces on the same prim. @@ -404,15 +318,10 @@ def test_newton_mesh_collision_mixed_namespace_write(setup_sim): assert "NewtonCollisionAPI" in applied assert "NewtonMeshCollisionAPI" in applied - -@pytest.mark.isaacsim_ci -def test_newton_legacy_cfg_authors_contact_attrs(setup_sim): - """The legacy Newton material cfg authors all newton:* attributes the fragment authors.""" - mat_cfg = NewtonMaterialPropertiesCfg( - contact_stiffness=1.0e4, contact_damping=250.0, contact_friction_gain=40.0, contact_adhesion=0.02 + # Without max_hull_vertices the mesh collision schema stays unapplied. + sim_utils.create_prim("/World/mesh_col2", prim_type="Cube", translation=(5.0, 0.0, 0.5)) + schemas.define_mesh_collision_properties( + "/World/mesh_col2", + NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull"), ) - prim = spawn_rigid_body_material("/World/newton_mat_contact", mat_cfg) - assert prim.GetAttribute("newton:contactStiffness").Get() == pytest.approx(1.0e4) - assert prim.GetAttribute("newton:contactDamping").Get() == pytest.approx(250.0) - assert prim.GetAttribute("newton:contactFrictionGain").Get() == pytest.approx(40.0) - assert prim.GetAttribute("newton:contactAdhesion").Get() == pytest.approx(0.02) + assert "NewtonMeshCollisionAPI" not in stage.GetPrimAtPath("/World/mesh_col2").GetAppliedSchemas() diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index 831a154ed7b..0182c073459 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -13,7 +13,7 @@ import sys from pathlib import Path -from isaaclab.test.utils import test_devices +from isaaclab.test.utils import DeviceScope, test_devices sys.path.insert(0, str(Path(__file__).resolve().parents[1])) sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sim")) @@ -109,26 +109,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: # ================================================================== -@pytest.mark.parametrize("device", test_devices()) -def test_reject_body_and_shape_paths(device): - """FrameView rejects prim paths that resolve to a Newton physics body or collision shape.""" - ctx = _sim_context(device, num_envs=2) - sim = ctx.__enter__() - sim._app_control_on_stop_handle = None - InteractiveScene(_SceneCfg(num_envs=2, env_spacing=2.0)) - sim.reset() - - with pytest.raises(ValueError, match="physics body"): - FrameView("/World/envs/env_[^/]+/Cube", device=device) - - shape_labels = list(NewtonManager.get_model().shape_label) - assert shape_labels, "scene must contribute at least one collision shape" - with pytest.raises(ValueError, match="collision shape"): - FrameView(shape_labels[0], device=device) - ctx.__exit__(None, None, None) - - -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_non_colliding_shapes_after_finalize(device): """Non-colliding site and visual shapes remain valid after finalization.""" ctx = _sim_context(device, num_envs=1) @@ -161,13 +142,13 @@ def test_non_colliding_shapes_after_finalize(device): ctx.__exit__(None, None, None) -@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) -def test_body_local_frame_resolves_before_and_after_reset(device): - """A body-local site resolves through the ClonePlan before reset and from Newton body labels after it. +@pytest.mark.parametrize("device", test_devices()) +def test_body_local_frame_resolves_from_body_labels_after_reset(device): + """A body-local site created after reset resolves from the finalized Newton body labels. - Only the prototype env authors the child prim on the stage; the view created before ``sim.reset`` - expands it through the ClonePlan, while the view created afterwards resolves the same frame directly - from the finalized Newton body labels. Both must agree with the parent body poses. + Only the prototype env authors the child prim on the stage, so the view must expand it through the + Newton body labels rather than the stage. The ClonePlan path before reset is covered by the shared + contract tests. """ num_envs = 3 ctx = _sim_context(device, num_envs=num_envs) @@ -180,22 +161,22 @@ def test_body_local_frame_resolves_before_and_after_reset(device): assert not stage.GetPrimAtPath("/World/envs/env_1/Cube").IsValid() sim_utils.create_prim("/World/envs/env_0/Cube/CameraMount", translation=CHILD_OFFSET) - clone_plan_view = FrameView("/World/envs/env_[^/]+/Cube/CameraMount", device=device) sim.reset() label_view = FrameView("/World/envs/env_[^/]+/Cube/CameraMount", device=device) - assert clone_plan_view.count == num_envs assert label_view.count == num_envs assert not stage.GetPrimAtPath("/World/envs/env_1/Cube/CameraMount").IsValid() expected = _get_body_positions(num_envs, device) + torch.tensor(CHILD_OFFSET, device=device) - torch.testing.assert_close(clone_plan_view.get_world_poses()[0].torch, expected, atol=1e-5, rtol=0) torch.testing.assert_close(label_view.get_world_poses()[0].torch, expected, atol=1e-5, rtol=0) ctx.__exit__(None, None, None) -@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_close_before_reset_cancels_deferred_initialization(device): - """A view closed before the Newton model exists must not initialize on ``PHYSICS_READY``.""" + """A view closed before the Newton model exists must not initialize on ``PHYSICS_READY``. + + After reset, new views over Newton bodies or collision shapes are rejected. + """ num_envs = 3 ctx = _sim_context(device, num_envs=num_envs) sim = ctx.__enter__() @@ -210,6 +191,14 @@ def test_close_before_reset_cancels_deferred_initialization(device): sim.reset() assert view.count == 0, "a closed view still initialized from the physics-ready callback" + + # FrameView rejects prim paths that resolve to a Newton physics body or collision shape. + with pytest.raises(ValueError, match="physics body"): + FrameView("/World/envs/env_[^/]+/Cube", device=device) + shape_labels = list(NewtonManager.get_model().shape_label) + assert shape_labels, "scene must contribute at least one collision shape" + with pytest.raises(ValueError, match="collision shape"): + FrameView(shape_labels[0], device=device) ctx.__exit__(None, None, None) From 24fd31827ee878c6fe68850aa06a7a60f490e209 Mon Sep 17 00:00:00 2001 From: Mustafa H <34825877+StafaH@users.noreply.github.com> Date: Fri, 25 Sep 2026 06:17:39 -0700 Subject: [PATCH 7/7] [Core] Refactor and cleanup core subpackage for 3.0 [6/N] (#7986) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Description Sixth PR in a series that splits the core cleanup in #7949 into small, reviewable pieces. This one fixes two bugs in `isaaclab.benchmark`. Each fix has a regression test that fails against `develop` and passes here. ## Fixes - `benchmark.measurements.TestPhase.from_json`: phases written with `TestPhaseEncoder` store `"metadata"` as a list, but `from_json` passed `m["metadata"]` to `metadata_from_dict`, which indexes `["metadata"]` again. Phases with measurements raised `TypeError`, and phases without measurements silently lost their metadata because the assignment sat inside the measurement loop. It now passes the phase dict once, after the loop. This also fixes `TestPhase.aggregate_json_files`. - `benchmark.recorders.record_cpu_info.CPUInfoRecorder.get_data`: it read the mean/std/count from a dict that is only filled by `update()`, so calling it first raised `KeyError`. It now reads the running statistics directly, which start at zero. ## Tests - `test/benchmark/test_formatters.py`: `test_phase_json_round_trip_keeps_metadata` (with and without measurements). - `test/benchmark/test_recorders.py`: `TestCPUInfoRecorder.test_get_data_measurement_names` also calls `get_data()` before the first update. Validation: 25 formatter/recorder tests passed after merging `develop`. All three regression cases fail against the unfixed production code (metadata decoding, metadata without measurements, and CPU data before update). Formatting, lint, and changelog checks passed. ## 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 ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` Co-authored-by: Octi Zhang --- .../changelog.d/core-cleanup-benchmark-fixes.rst | 9 +++++++++ .../isaaclab/isaaclab/benchmark/measurements.py | 2 +- .../benchmark/recorders/record_cpu_info.py | 6 +++--- source/isaaclab/test/benchmark/test_formatters.py | 15 ++++++++++++++- source/isaaclab/test/benchmark/test_recorders.py | 3 ++- 5 files changed, 29 insertions(+), 6 deletions(-) create mode 100644 source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst diff --git a/source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst b/source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst new file mode 100644 index 00000000000..12d2287a600 --- /dev/null +++ b/source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst @@ -0,0 +1,9 @@ +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab.benchmark.measurements.TestPhase.from_json` raising ``TypeError`` for phases + serialized with :class:`~isaaclab.benchmark.measurements.TestPhaseEncoder` that have measurements, and + dropping the metadata of phases without measurements. This also affected + :meth:`~isaaclab.benchmark.measurements.TestPhase.aggregate_json_files`. +* Fixed :meth:`~isaaclab.benchmark.recorders.record_cpu_info.CPUInfoRecorder.get_data` raising ``KeyError`` + when called before the first :meth:`~isaaclab.benchmark.recorders.record_cpu_info.CPUInfoRecorder.update`. diff --git a/source/isaaclab/isaaclab/benchmark/measurements.py b/source/isaaclab/isaaclab/benchmark/measurements.py index 5004a694277..d43ace55d3a 100644 --- a/source/isaaclab/isaaclab/benchmark/measurements.py +++ b/source/isaaclab/isaaclab/benchmark/measurements.py @@ -291,7 +291,7 @@ def from_json(cls, m: dict) -> "TestPhase": curr_meas = BooleanMeasurement(name=meas["name"], bvalue=meas["bvalue"]) curr_run.measurements.append(curr_meas) - curr_run.metadata = TestPhase.metadata_from_dict(m["metadata"]) + curr_run.metadata = TestPhase.metadata_from_dict(m) return curr_run @classmethod diff --git a/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py b/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py index 13cf3225d37..e0f2aa263b0 100644 --- a/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py +++ b/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py @@ -70,9 +70,9 @@ def get_runtime_data(self) -> dict: def get_data(self) -> MeasurementData: return MeasurementData( measurements=[ - SingleMeasurement(name="CPU Utilization", value=self._cpu_runtime_info["mean"], unit="%"), - SingleMeasurement(name="CPU Utilization std", value=self._cpu_runtime_info["std"], unit="%"), - SingleMeasurement(name="CPU Utilization n", value=self._cpu_runtime_info["n"], unit=""), + SingleMeasurement(name="CPU Utilization", value=self._mean, unit="%"), + SingleMeasurement(name="CPU Utilization std", value=self._std, unit="%"), + SingleMeasurement(name="CPU Utilization n", value=self._n, unit=""), ], metadata=[ StringMetadata(name="cpu_name", data=self._cpu_hardware_info["name"]), diff --git a/source/isaaclab/test/benchmark/test_formatters.py b/source/isaaclab/test/benchmark/test_formatters.py index 8f8198f6e1d..c4ecc953d83 100644 --- a/source/isaaclab/test/benchmark/test_formatters.py +++ b/source/isaaclab/test/benchmark/test_formatters.py @@ -14,7 +14,7 @@ import pytest from isaaclab.benchmark import formatters -from isaaclab.benchmark.measurements import SingleMeasurement, StringMetadata, TestPhase +from isaaclab.benchmark.measurements import SingleMeasurement, StringMetadata, TestPhase, TestPhaseEncoder from isaaclab.benchmark.schema import ( GpuDeviceInfo, Hardware, @@ -29,6 +29,19 @@ ) +@pytest.mark.parametrize("measurements", [[SingleMeasurement(name="fps", value=60.0, unit="Hz")], []]) +def test_phase_json_round_trip_keeps_metadata(measurements) -> None: + """A phase serialized with TestPhaseEncoder reads back with its metadata, with or without measurements.""" + phase = TestPhase( + phase_name="runtime", measurements=measurements, metadata=[StringMetadata(name="gpu", data="A10")] + ) + + restored = TestPhase.from_json(json.loads(json.dumps(phase, cls=TestPhaseEncoder))) + + assert [(m.name, m.value) for m in restored.measurements] == [(m.name, m.value) for m in measurements] + assert [(m.name, m.data) for m in restored.metadata] == [("gpu", "A10")] + + def test_default_output_filenames_are_unique_with_identical_timestamps(monkeypatch) -> None: class FixedDatetime: @classmethod diff --git a/source/isaaclab/test/benchmark/test_recorders.py b/source/isaaclab/test/benchmark/test_recorders.py index 0706a59dc25..cd5c4d39a90 100644 --- a/source/isaaclab/test/benchmark/test_recorders.py +++ b/source/isaaclab/test/benchmark/test_recorders.py @@ -48,7 +48,8 @@ def test_get_runtime_data_after_updates(self, recorder): assert isinstance(data["cpu_utilization"]["n"], int) def test_get_data_measurement_names(self, recorder): - """Test that get_data returns measurements with correct names.""" + """Test get_data before the first update and the measurement names after updates.""" + assert len(recorder.get_data().measurements) == 3 for _ in range(3): recorder.update()