diff --git a/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-results.md b/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-results.md new file mode 100644 index 000000000000..70a72b04a21a --- /dev/null +++ b/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-results.md @@ -0,0 +1,300 @@ + + +# Asset Test Suite Redesign Results + +## Outcome + +The comparable asset and WrenchComposer scopes now finish in **134.11 s** of +subprocess wall time, down from **1,197.60 s**: an **8.93x wall-time speed-up**. +The test-runner model is unchanged: every selected file runs in a fresh +subprocess. The final scopes collect 1,519 focused cases: 1,416 pass and 103 +skip with an explicit capability reason. + +The reduction comes from replacing backend-independent solver matrices with a +shared contract, moving backend-only branches into tiny unit/kernel tests, and +retaining one local real-solver seam per supported asset family. Newton's real +asset tests are kitless and use no Nucleus assets. Cable and MPM remain +deliberately excluded. + +## Measurement environment + +- Final branch base: `6aad90be0a` from `origin/develop`, rebased before the + final measurements. +- Original baseline base: `d7033a5a1a207f1d4284edb60d72d7838984413b`. +- Worktree-local environment: `env_isaaclab`, installed with + `UV_PROJECT_ENVIRONMENT=env_isaaclab uv sync --frozen --inexact --extra test + --extra isaacsim --extra ovphysx`. +- IsaacSim 6.0.1.0, Kit 110.1.2, Warp 1.16.0, OVPhysX 0.5.10. +- NVIDIA GeForce RTX 5090, driver 590.48.01, 32,607 MiB. +- Commands ran with `OMNI_KIT_ACCEPT_EULA=YES` and a warmed writable Warp cache + at `/tmp/isaaclab-task8-warp`. +- Times in the comparison are the repository orchestrator's aggregate pytest + and subprocess wall times, not the outer shell duration. + +## Copy-ready PR performance section + +The asset-test redesign reduced the five comparable CI-style scopes from +19m57.60s to 2m14.11s wall time (**8.93x faster**). This comparison uses the +same repository test orchestrator before and after, with one process per test +file. Controller-owner and OV manager lifecycle checks are reported separately +and are not included in the denominator. + +| Scope | Before files / cases | After files / outcomes | Before pytest / wall | After pytest / wall | Wall speed-up | +|---|---:|---:|---:|---:|---:| +| Shared assets | 8 / 4,331 | 6 / 1,238 pass, 103 skip | 64.70 / 84.02 s | 3.81 / 17.92 s | **4.69x** | +| Newton assets, no cable/MPM | 6 / 644 | 15 / 59 pass | 590.94 / 608.44 s | 16.17 / 50.57 s | **12.03x** | +| PhysX assets | 7 / 486 | 12 / 44 pass | 236.80 / 255.30 s | 6.12 / 31.92 s | **8.00x** | +| OV assets | 9 / 492 | 12 / 60 pass | 196.94 / 220.05 s | 7.07 / 28.80 s | **7.64x** | +| WrenchComposer | 3 / 412 | 2 / 15 pass | 23.34 / 29.79 s | 0.89 / 4.90 s | **6.08x** | +| **Aggregate** | **33 / 6,365** | **47 / 1,416 pass, 103 skip** | **1,112.72 / 1,197.60 s** | **34.06 / 134.11 s** | **8.93x** | + +The articulation-only change in the final iteration more than doubled the +number of real backend probes while reducing their aggregate runtime. Each +backend now creates one composite scene, resets once, and keeps isolated actor +islands alive until every test node in the module has run. Every articulation +island contains two environments so the real backends prove partial writes +without restoring the old per-case scene startup cost. + +| Articulation backend | Before cases | After cases | Before pytest / wall | After pytest / wall | +|---|---:|---:|---:|---:| +| Newton | 3 | 4 | 3.04 / 4.65 s | 2.42 / 4.01 s | +| PhysX | 2 | 5 | 4.94 / 5.75 s | 3.28 / 4.02 s | +| OVPhysX | 2 | 6 | 2.38 / 3.46 s | 2.44 / 3.58 s | +| **Aggregate** | **7** | **15** | **10.36 / 13.86 s** | **8.14 / 11.61 s** | + +Focused gate and ownership timings: + +| Gate or owner | Result | Pytest / wall | +|---|---:|---:| +| Shared contract, one process | 1,134 pass, 103 skip | 7.46 / 8.90 s | +| Shared contract + adjacent units, file-isolated gate | 1,238 pass, 103 skip | 3.81 / 17.92 s | +| Newton backend units/kernels, including executable kitless guard | 49 pass | 11.96 / 13.06 s | +| PhysX backend units | 33 pass | 1.69 / 2.76 s | +| OV backend units | 50 pass | 1.62 / 2.70 s | +| Newton minimal real integration | 4 files, 10 pass | 4.58 / 16.42 s | +| PhysX minimal real integration | 6 files, 11 pass | 4.42 / 20.37 s | +| OV minimal real integration | 4 files, 10 pass | 4.71 / 12.08 s | +| WrenchComposer real delivery | 1 file, 1 pass | 0.87 / 3.33 s | +| Newton task-space controller owner | 3 pass | 3.03 / 4.57 s | +| PhysX actuator-runtime and termination owners | 6 pass | 1.13 / 2.00 s | +| OV mixed CPU/CUDA lifecycle owner | 1 pass | 2.01 / 2.43 s | + +All warmed contract/backend-unit gates are below the 30-second target. + +## Exact comparable-scope commands + +`TEST_INCLUDE_FILES` matches **basenames recursively** below +`TEST_FILTER_PATTERN`. The comparable commands therefore enumerate every +integration and unit basename explicitly; backend-qualified OV and PhysX unit +filenames keep combined pytest collection collision-free. + +```bash +WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ +OMNI_KIT_ACCEPT_EULA=YES \ +TEST_FILTER_PATTERN=/source/isaaclab/test/assets/ \ +TEST_INCLUDE_FILES=test_asset_contract_api.py,test_asset_contract_data.py,test_asset_contract_writes.py,test_articulation_ordering.py,test_articulation_ordering_kernels.py,test_asset_selector_cache.py \ +TEST_RESULT_FILE=task8-final-assets-shared.xml \ +./isaaclab.sh -p -m pytest tools -q + +WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ +OMNI_KIT_ACCEPT_EULA=YES \ +TEST_FILTER_PATTERN=/source/isaaclab_newton/test/assets/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_articulation_fk_cache.py,test_articulation_joint_staging.py,test_articulation_ordering.py,test_articulation_ordering_kernels.py,test_newton_actuator_adaptation.py,test_newton_actuators_newton.py,test_rigid_assets_import.py,test_rigid_object.py,test_rigid_object_collection.py,test_rigid_object_collection_model_indices.py,test_rigid_object_fk_cache.py,test_rigid_object_inertial_staging.py,test_rigid_object_setter_notifications.py,test_wrench_kernels.py \ +TEST_RESULT_FILE=task8-final-assets-newton.xml \ +./isaaclab.sh -p -m pytest tools -q + +WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ +OMNI_KIT_ACCEPT_EULA=YES \ +TEST_FILTER_PATTERN=/source/isaaclab_physx/test/assets/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_deformable_object.py,test_newton_actuators_physx.py,test_rigid_object.py,test_rigid_object_collection.py,test_surface_gripper.py,test_physx_actuator_control.py,test_physx_articulation.py,test_physx_deformable_object.py,test_physx_rigid_object.py,test_physx_rigid_object_collection.py,test_physx_surface_gripper.py \ +TEST_RESULT_FILE=task8-final-assets-physx.xml \ +./isaaclab.sh -p -m pytest tools -q + +WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ +OMNI_KIT_ACCEPT_EULA=YES \ +TEST_FILTER_PATTERN=/source/isaaclab_ov/test/assets/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_articulation_helpers.py,test_articulation_kernels.py,test_deformable_object.py,test_deformable_views.py,test_ovphysx_actuator_control.py,test_ovphysx_articulation.py,test_ovphysx_deformable_object.py,test_ovphysx_rigid_object.py,test_ovphysx_rigid_object_collection.py,test_rigid_object.py,test_rigid_object_collection.py \ +TEST_RESULT_FILE=task8-final-assets-ov.xml \ +./isaaclab.sh -p -m pytest tools -q + +WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ +OMNI_KIT_ACCEPT_EULA=YES \ +TEST_FILTER_PATTERN=/source/isaaclab/test/utils/ \ +TEST_INCLUDE_FILES=test_wrench_composer.py,test_wrench_composer_integration.py \ +TEST_RESULT_FILE=task8-final-wrench-composer.xml \ +./isaaclab.sh -p -m pytest tools -q +``` + +## Fast and real gate commands + +Run contract definitions together, but keep the adjacent legacy-ordering unit +module as its own file. That unit intentionally controls import stubs at module +collection time; the repository orchestrator above gives it the required clean +process. + +```bash +# Shared contract and backend-specific fast gates. +export WARP_CACHE_PATH=/tmp/isaaclab-task8-warp +export OMNI_KIT_ACCEPT_EULA=YES + +./isaaclab.sh -p -m pytest source/isaaclab/test/assets/contract -q +./isaaclab.sh -p -m pytest \ + source/isaaclab_newton/test/assets/unit \ + source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py -q +./isaaclab.sh -p -m pytest source/isaaclab_physx/test/assets/unit -q +./isaaclab.sh -p -m pytest source/isaaclab_ov/test/assets/unit -q +``` + +Real-only subprocess gates exclude `/assets/unit/` so their scope remains +explicitly limited to live backend tests. + +```bash +export WARP_CACHE_PATH=/tmp/isaaclab-task8-warp +export OMNI_KIT_ACCEPT_EULA=YES + +TEST_FILTER_PATTERN=/source/isaaclab_newton/test/assets/ \ +TEST_EXCLUDE_PATTERN=/assets/unit/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_newton_actuators_newton.py,test_rigid_object.py,test_rigid_object_collection.py \ +TEST_RESULT_FILE=task8-final-integration-newton.xml \ +./isaaclab.sh -p -m pytest tools -q + +TEST_FILTER_PATTERN=/source/isaaclab_physx/test/assets/ \ +TEST_EXCLUDE_PATTERN=/assets/unit/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_deformable_object.py,test_newton_actuators_physx.py,test_rigid_object.py,test_rigid_object_collection.py,test_surface_gripper.py \ +TEST_RESULT_FILE=task8-final-integration-physx.xml \ +./isaaclab.sh -p -m pytest tools -q + +TEST_FILTER_PATTERN=/source/isaaclab_ov/test/assets/ \ +TEST_EXCLUDE_PATTERN=/assets/unit/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_deformable_object.py,test_rigid_object.py,test_rigid_object_collection.py \ +TEST_RESULT_FILE=task8-final-integration-ov.xml \ +./isaaclab.sh -p -m pytest tools -q +``` + +## Old-module disposition + +### Shared Isaac Lab and WrenchComposer + +| Old module | Disposition | Coverage owner | +|---|---|---| +| `test_articulation_iface.py` | Replaced/renamed | Contract API/data/write entries; assertion cases remain in `_articulation_contract_cases.py`. | +| `test_articulation_ordering_iface.py` | Moved | `_articulation_ordering_contract_cases.py`, exposed by the three contract entries. | +| `test_rigid_object_iface.py` | Replaced/renamed | Contract API/data/write entries and `_rigid_object_contract_cases.py`. | +| `test_rigid_object_collection_iface.py` | Replaced/renamed | Contract API/data/write entries and `_rigid_object_collection_contract_cases.py`. | +| `test_iface_test_utils.py` | Replaced | Explicit backend capabilities and public-base-surface classification tests. | +| `test_articulation_ordering.py` | Retained | Solver-independent name/order/map units in its own clean process. | +| `test_articulation_ordering_kernels.py` | Retained | Tiny gather/scatter/write kernels and selector-width coverage. | +| `test_asset_selector_cache.py` | Retained | Selector identity/domain/LRU semantics. | +| `test_wrench_composer.py` | Replaced/focused | Fourteen literal `2 x 2` arithmetic, selection, reset, lazy, merge, validation, and compatibility cases. | +| `test_wrench_composer_integration.py` | Consolidated | One rotated global force-at-position delivery parity test. | +| `test_wrench_composer_vs_physx.py` | Removed as redundant | The unique rotated force/induced-torque seam is in the retained integration test; the matrix moved to literal units. | + +### Newton, excluding cable and MPM + +| Old module | Disposition | Coverage owner | +|---|---|---| +| `test_articulation.py` | Retained and consolidated | One kitless CPU scene contains duplicated floating and fixed actor islands across identical Newton worlds. Four aligned nodes cover partial state, joint/body properties, drive/dynamics, and floating-root/wrench behavior without rebuilding the model. FK, staging, and ordering remain in units; IK/OSC/gravity remain in the controller owner. | +| `test_newton_actuators_newton.py` | Retained and reduced | One real Lab/native execution-path equivalence; adaptation and target-mode branches moved to units. | +| `test_rigid_object.py` | Retained and reduced | Local CPU property/state/wrench seam plus CUDA smoke; selection, inverse inertia, FK, and notification branches moved to units. | +| `test_rigid_object_collection.py` | Retained and reduced | Local `N=2, B=2` selection/property seam; model-index mapping moved to units. | +| `test_articulation_ordering_kernels.py` | Retained | Tiny Newton ordering-kernel coverage. | +| `test_wrench_kernels.py` | Moved | `assets/unit/test_wrench_kernels.py`. | + +`test_cable_object.py` and `test_mpm_object.py` are excluded, unchanged, and +absent from every benchmark command. + +### PhysX + +| Old module | Disposition | Coverage owner | +|---|---|---| +| `test_articulation.py` | Retained and consolidated | One local composite scene contains two-environment ordered/fixed, floating, and spatial-tendon islands. Five nodes cover partial state, raw joint/body properties, drive/dynamics, floating-root/wrench behavior, and tendon writes after one reset. | +| `test_articulation_kernels.py` | Moved/expanded | `assets/unit/test_physx_articulation.py`. | +| `test_deformable_object.py` | Replaced | Two working local surface/volume probes plus focused classification/material/target/kernel units; the former all-skipped startup is gone. | +| `test_newton_actuators_physx.py` | Retained and reduced | One real ordered Lab/native dispatch seam; dispatch and graph branches moved to backend and shared actuator units. | +| `test_rigid_object.py` | Retained and reduced | Local state, raw mass/COM/inertia/material, and wrench delivery; staging/cache/import isolation moved to units. | +| `test_rigid_object_collection.py` | Retained and reduced | Local nontrivial body-major selection and raw property/material readback; ID/layout/staging moved to units. | +| `test_surface_gripper.py` | Replaced | Local two-cube open/close seam; filtering, partial properties, and CUDA rejection moved to units. | + +### OVPhysX + +| Old module | Disposition | Coverage owner | +|---|---|---| +| `test_articulation.py` | Retained and consolidated | One CUDA composite scene contains two-environment ordered/fixed, floating, spatial-tendon, and native-actuator islands. Six nodes cover partial state, raw joint/body properties, drive/dynamics, floating-root/wrench behavior, tendon writes, and native effort after one reset. | +| `test_articulation_helpers.py` | Moved | `assets/unit/test_articulation_helpers.py`. | +| `test_articulation_kernels.py` | Moved | `assets/unit/test_articulation_kernels.py`. | +| `test_deformable_object.py` | Retained and reduced | One volume and one surface CUDA seam, including forced rewarm isolation. | +| `test_deformable_object_helpers.py` | Moved/expanded | `assets/unit/test_ovphysx_deformable_object.py`. | +| `test_deformable_views.py` | Moved | `assets/unit/test_deformable_views.py`. | +| `test_rigid_object.py` | Retained and reduced | Local partial state, raw inertial property, and real wrench delivery. | +| `test_rigid_object_collection.py` | Retained and reduced | Local nonidentity selection plus raw fused inertial/material mapping. | +| `test_rigid_object_helpers.py` | Moved/expanded | Backend-qualified rigid and fused-collection unit modules. | + +## Case-family disposition and coverage holes closed + +| Old case family | Disposition | Current evidence | +|---|---|---| +| Generic initialization, names, shapes, aliases, defaults, finders | Replaced | Shared API/data contracts with canonical CPU `N=2, B=3, J=4`. | +| Root/body/joint writers, partial selection, invalid shapes | Replaced | Shared write contracts with literal index/mask cases. | +| Cache timestamps and invalidation | Moved | Shared contracts plus backend-specific cache/FK units. | +| Broad environment/body/joint/device Cartesian products | Removed as redundant | One canonical case, targeted singleton/order cases, and one genuine CUDA smoke per device-specific path. | +| Repeated wrench arithmetic, frames, masks, offsets, and long rollouts | Replaced | Literal WrenchComposer units plus one real delivery per backend. | +| Remote ANYmal/Panda/ShadowHand/humanoid fixtures | Replaced | Small local authored primitives and branching articulations. | +| Backend model/view ordering and selector translation | Moved/expanded | Newton model-index/order units, PhysX body-major units, and OV fused-layout units, each backed by one real nonidentity seam. | +| Mass, COM, inertia, friction, and restitution setters | Retained and strengthened | Raw backend view/binding readback per rigid, collection, and articulation family. | +| Newton FK freshness and model notifications | Newly focused | Dedicated units with exact literal state/model flags. | +| PhysX collection COM/inertia layout | Newly covered/fixed | Real body-major write/read exposed and guards flattened COM and 3-D inertia TensorAPI layouts. | +| PhysX deformable surface/volume distinction and material fallback | Newly covered | Focused units plus two non-skipping real probes. | +| OV fused layout, CPU staging, view rewarm, and manager device reuse | Newly covered | Focused units, forced-rewarm deformable seam, and CPU-CUDA-CPU lifecycle owner. | +| IK, OSC, gravity compensation, graph capture, and termination | Moved | Newton task-space controller owner; shared PhysX actuator-runtime and termination owners. | +| Newton kinematic rigid-object parameters | Removed unsupported skips | Newton does not support these modes; the old cases executed no behavior. | +| Cable and MPM | Excluded | Assigned outside this project and unchanged. | + +The public-base-surface inventory classifies every member declared by `AssetBase`, +`BaseRigidObject`, `BaseRigidObjectData`, `BaseRigidObjectCollection`, +`BaseRigidObjectCollectionData`, `BaseArticulation`, and +`BaseArticulationData` by contract kind or as reasoned out of scope; it detects +API taxonomy drift rather than claiming that a generic class import proves +per-member behavioral coverage. Direct tests cover actuator-collection binding +and the deprecated friction-writer forwarding seam. Factory manager +replacements are scoped to one contract test and exact +PhysX/Newton production bindings are restored; forward and reverse +cross-backend factory order is covered. + +## Unsupported capabilities and expected outcomes + +- Newton spatial tendons, CoM-orientation writes, and extended fixed-tendon + data/write paths are explicitly unsupported. Contract parameters stay visible + as reasoned capability skips rather than disappearing from collection. +- Contract fixtures without spatial tendons skip spatial-tendon data probes + with `No spatial tendons configured`. +- OV surface deformable kinematic targets remain unsupported and raise the + asserted `ValueError` in unit and real coverage. +- The installed PhysX wheel reports an invalid surface-view `check()` flag + while still accepting/returning nodal data. The real test asserts the stable + supported operations rather than the inconsistent flag. + +No retained backend asset file is entirely skipped. All Newton, PhysX, OV, and +Wrench real integration files executed on the reference GPU. + +## Isolation and residual warnings + +Final isolation checks included: + +- contract factory manager access/restoration in PhysX-Newton and + Newton-PhysX order; +- Newton rigid/collection/articulation forward and reverse repetition: + 12 passed, with four CUDA cases skipped only in the sandboxed repeat probe; +- PhysX deformables repeated in one process: 4 passed; +- OV deformable/rigid/articulation forward and reverse repetition: 8 passed; +- OV CPU-CUDA-CPU manager lifecycle: 1 passed. + +Residual output is from existing runtime behavior: Torch JIT deprecations, +Isaac Lab schema deprecations, Newton shape-color and coordinate-layout future +warnings, headless Kit display/IOMMU messages, OVPhysX automatic warmup and +USD synchronization messages, and the PhysX surface-view warning described +above. None caused a skip or failure in the retained real integration gates. diff --git a/source/isaaclab/changelog.d/asset-contract-infrastructure.skip b/source/isaaclab/changelog.d/asset-contract-infrastructure.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab/test/actuators/test_physx_actuator_runtime.py b/source/isaaclab/test/actuators/test_physx_actuator_runtime.py new file mode 100644 index 000000000000..2fa1558d01fb --- /dev/null +++ b/source/isaaclab/test/actuators/test_physx_actuator_runtime.py @@ -0,0 +1,146 @@ +# 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 + +"""Focused unit tests for the shared host-side PhysX actuator runtime.""" + +from types import SimpleNamespace +from unittest.mock import Mock, call + +import pytest +import warp as wp + +from isaaclab.actuators.newton.physx_runtime import PhysxActuatorRuntime + + +def _runtime() -> PhysxActuatorRuntime: + return PhysxActuatorRuntime(SimpleNamespace(device="cuda:0"), logger=Mock()) + + +def test_graph_capture_builds_two_graphs_and_restores_adapter_state(monkeypatch: pytest.MonkeyPatch) -> None: + """Capture both alternating state graphs without leaking the capture-time state swap.""" + graphs = [object(), object()] + + class _Capture: + def __init__(self, *args, **kwargs): + self.graph = graphs.pop(0) + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + return False + + state_a, state_b = object(), object() + runtime = _runtime() + runtime.adapter = SimpleNamespace(_states_a=state_a, _states_b=state_b) + monkeypatch.setattr(wp, "ScopedCapture", _Capture) + monkeypatch.setattr(runtime, "_run_native_actuator_kernels", Mock()) + + runtime._capture_native_actuator_graphs(SimpleNamespace(), 0.01) + + assert len(runtime.native_actuator_graphs) == 2 + assert runtime.adapter._states_a is state_a + assert runtime.adapter._states_b is state_b + assert runtime._native_actuator_graph_index == 0 + + +def test_graph_capture_failure_falls_back_to_eager(monkeypatch: pytest.MonkeyPatch) -> None: + """A partial capture failure must restore adapter state before eager execution.""" + + class _FailingCapture: + capture_count = 0 + + def __init__(self, *args, **kwargs): + self.capture_index = self.capture_count + type(self).capture_count += 1 + self.graph = object() + + def __enter__(self): + if self.capture_index == 1: + raise RuntimeError("second capture unavailable") + return self + + def __exit__(self, exc_type, exc_value, traceback): + return False + + state_a, state_b = object(), object() + runtime = _runtime() + runtime.adapter = SimpleNamespace(_states_a=state_a, _states_b=state_b) + + def _swap_adapter_state(*args, **kwargs) -> None: + runtime.adapter._states_a, runtime.adapter._states_b = runtime.adapter._states_b, runtime.adapter._states_a + + monkeypatch.setattr(wp, "ScopedCapture", _FailingCapture) + monkeypatch.setattr(runtime, "_run_native_actuator_kernels", _swap_adapter_state) + + runtime._capture_native_actuator_graphs(SimpleNamespace(), 0.01) + + assert runtime.native_actuator_graphs == () + assert runtime.adapter._states_a is state_a + assert runtime.adapter._states_b is state_b + runtime._logger.warning.assert_called_once() + + +def test_compute_falls_back_to_eager_after_graph_capture_failure(monkeypatch: pytest.MonkeyPatch) -> None: + """A failed host capture during compute must execute the current command eagerly.""" + + class _FailingCapture: + def __init__(self, *args, **kwargs): + pass + + def __enter__(self): + raise RuntimeError("capture unavailable") + + def __exit__(self, exc_type, exc_value, traceback): + return False + + runtime = _runtime() + runtime.adapter = SimpleNamespace( + is_stateful=False, + is_all_graphable=True, + _states_a=object(), + _states_b=object(), + ) + eager_compute = Mock() + monkeypatch.setattr(wp, "get_device", lambda device: SimpleNamespace(is_cuda=True, is_capturing=False)) + monkeypatch.setattr(wp, "ScopedCapture", _FailingCapture) + monkeypatch.setattr(runtime, "_run_native_actuator_kernels", eager_compute) + collection = SimpleNamespace() + + runtime.compute(collection, 0.01) + + eager_compute.assert_called_once_with(collection, 0.01) + + +def test_compute_launches_alternating_graphs_and_swaps_state(monkeypatch: pytest.MonkeyPatch) -> None: + """Successive graphable computes must ping-pong graphs and adapter state exactly once.""" + graph_a, graph_b = object(), object() + swap_state = Mock() + runtime = _runtime() + runtime.adapter = SimpleNamespace(is_stateful=True, is_all_graphable=True, _swap_state_buffers=swap_state) + runtime.native_actuator_graphs = (graph_a, graph_b) + eager_compute = Mock() + launch_graph = Mock() + monkeypatch.setattr(wp, "get_device", lambda device: SimpleNamespace(is_cuda=True, is_capturing=False)) + monkeypatch.setattr(wp, "capture_launch", launch_graph) + monkeypatch.setattr(runtime, "_run_native_actuator_kernels", eager_compute) + + runtime.compute(SimpleNamespace(), 0.01) + runtime.compute(SimpleNamespace(), 0.01) + + assert launch_graph.call_args_list == [call(graph_a), call(graph_b)] + assert swap_state.call_count == 2 + assert runtime._native_actuator_graph_index == 0 + eager_compute.assert_not_called() + + +def test_stateful_actuator_rejects_outer_cuda_capture(monkeypatch: pytest.MonkeyPatch) -> None: + """Stateful adapters cannot safely mutate their buffers inside an outer CUDA capture.""" + runtime = _runtime() + runtime.adapter = SimpleNamespace(is_stateful=True) + monkeypatch.setattr(wp, "get_device", lambda device: SimpleNamespace(is_cuda=True, is_capturing=True)) + + with pytest.raises(RuntimeError, match="stateful Newton actuators cannot run inside an outer CUDA graph capture"): + runtime.compute(SimpleNamespace(), 0.01) diff --git a/source/isaaclab/test/assets/_iface_test_boot.py b/source/isaaclab/test/assets/_iface_test_boot.py deleted file mode 100644 index 96d3847b1483..000000000000 --- a/source/isaaclab/test/assets/_iface_test_boot.py +++ /dev/null @@ -1,31 +0,0 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Shared Kit and kitless bootstrap for backend interface tests.""" - -import os -import sys -from unittest.mock import MagicMock - -_kitless = "ovphysx" in os.environ.get("LD_PRELOAD", "") or ( - os.environ.get("LD_PRELOAD", "") == "" and "EXP_PATH" not in os.environ -) - -if not _kitless: - from isaaclab.app import AppLauncher - - simulation_app = AppLauncher(headless=True).app -else: - simulation_app = None - # ``omni`` is a real namespace package in OvPhysX kitless runs. Install - # missing submodules in both ``sys.modules`` and the namespace attributes. - import omni as _omni - - for _mod in ("physics", "physics.tensors", "physx", "timeline", "usd"): - _stub = MagicMock() - sys.modules[f"omni.{_mod}"] = _stub - setattr(_omni, _mod.split(".", 1)[0], _stub) - for _mod in ("isaacsim.core", "isaacsim.core.simulation_manager"): - sys.modules.setdefault(_mod, MagicMock()) diff --git a/source/isaaclab/test/assets/contract/__init__.py b/source/isaaclab/test/assets/contract/__init__.py new file mode 100644 index 000000000000..0257e2759638 --- /dev/null +++ b/source/isaaclab/test/assets/contract/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Shared asset contract test infrastructure.""" diff --git a/source/isaaclab/test/assets/test_articulation_iface.py b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py similarity index 91% rename from source/isaaclab/test/assets/test_articulation_iface.py rename to source/isaaclab/test/assets/contract/_articulation_contract_cases.py index f4e5b41b1d0a..6983c2bbb940 100644 --- a/source/isaaclab/test/assets/test_articulation_iface.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py @@ -7,8 +7,8 @@ # pyright: reportPrivateUsage=none """ -Checks that the articulation interfaces are consistent across backends, and are providing the exact same data as what -the base articulation class advertises. All articulation interfaces need to comply with the same interface contract. +Checks that articulation implementations are consistent across backends and provide the exact same data as the base +articulation class advertises. All articulation implementations need to comply with the same shared contract. The setup is a bit convoluted so that we can run these tests without requiring Isaac Sim or GPU simulation. """ @@ -19,10 +19,8 @@ import pytest import torch import warp as wp -from _articulation_iface_test_utils import BACKENDS, get_articulation - -pytestmark = pytest.mark.integration - +from ._articulation_contract_utils import BACKENDS, get_articulation +from .capabilities import backend_parameters, contract_backend, require_backend_capability @pytest.fixture def articulation_iface(request): @@ -58,21 +56,17 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name assert arr.dtype == expected_dtype, f"{name}: expected dtype {expected_dtype}, got {arr.dtype}" -# Common parametrize decorator for all interface tests -_backends = pytest.mark.parametrize("backend", BACKENDS, indirect=False) +# Common parametrize decorator for API contracts. +_backends = contract_backend("api") # We also need to provide the fixture params that articulation_iface reads: _default_dims = pytest.mark.parametrize( "num_instances, num_joints, num_bodies", - [(1, 1, 1), (1, 2, 2), (2, 6, 7), (100, 8, 13)], + [(2, 4, 3)], ) -_default_devices = pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -_index_resolution_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton") if backend in BACKENDS], indirect=False -) -_production_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False -) +_default_devices = pytest.mark.parametrize("device", ["cpu"]) +_index_resolution_backends = contract_backend("index_resolution", names=("physx", "newton")) +_production_backends = contract_backend("api") # --------------------------------------------------------------------------- @@ -274,9 +268,7 @@ def test_finder_returns_legacy_list_or_cached_proxy(self, backend, finder_name): # --------------------------------------------------------------------------- -_non_mock_backends = pytest.mark.parametrize( - "backend", [backend for backend in BACKENDS if backend.lower() != "mock"], indirect=False -) +_non_mock_backends = contract_backend("api") class TestResolveMatchingNamesCache: @@ -341,6 +333,8 @@ def test_find_with_preserve_order(self, backend, num_instances, num_joints, num_ # Tests: ArticulationData root state properties # --------------------------------------------------------------------------- +_backends = contract_backend("data") + class TestArticulationDataRootState: """Test data properties for root rigid body state.""" @@ -651,8 +645,6 @@ def test_body_com_acc_w(self, backend, num_instances, num_joints, num_bodies, de @_default_dims @_default_devices def test_body_com_pose_b(self, backend, num_instances, num_joints, num_bodies, device, articulation_iface): - if backend == "newton": - pytest.xfail("Newton only stores CoM position, not orientation") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -932,8 +924,6 @@ def test_body_com_pos_b(self, backend, num_instances, num_joints, num_bodies, de @_default_dims @_default_devices def test_body_com_quat_b(self, backend, num_instances, num_joints, num_bodies, device, articulation_iface): - if backend == "newton": - pytest.xfail("Newton only stores CoM position, not orientation") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -1207,6 +1197,24 @@ def test_actuator_compatibility_projections_are_stable( assert soft_joint_vel_limits_data.warp.ptr == art.actuators._soft_joint_vel_limits.ptr assert not [warning for warning in caught_warnings if warning.category is DeprecationWarning] + @_production_backends + @pytest.mark.parametrize("num_instances, num_joints, num_bodies", [(2, 4, 5)]) + @pytest.mark.parametrize("device", ["cpu"]) + def test_bind_actuator_collection_exposes_collection_buffers( + self, backend, num_instances, num_joints, num_bodies, device, articulation_iface + ): + art, _ = articulation_iface + + art.data.bind_actuator_collection(art.actuators) + + assert art.data._actuator_collection is art.actuators + with pytest.warns(DeprecationWarning): + assert art.data.joint_pos_target is art.actuators.target_command.position + assert art.data.joint_vel_target is art.actuators.target_command.velocity + assert art.data.joint_effort_target is art.actuators.target_command.effort + assert art.data.computed_torque is art.actuators.computed_effort + assert art.data.applied_torque is art.actuators.applied_effort + # --------------------------------------------------------------------------- # Writer/setter test helpers @@ -1321,6 +1329,39 @@ def _make_item_mask(total: int, selected: list[int], device: str) -> wp.array: return wp.array(mask_np, dtype=wp.bool, device=device) +def _read_articulation_root_pose(backend: str, art, raw_backend) -> np.ndarray: + """Read articulation root poses from backend storage.""" + if backend == "physx": + return raw_backend.get_root_transforms().numpy().reshape(art.num_instances, 7) + if backend == "newton": + return raw_backend.get_root_transforms(None).numpy().reshape(art.num_instances, -1, 7)[:, 0] + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.ROOT_POSE]._data.copy() + + +def _read_articulation_joint_positions(backend: str, art, raw_backend) -> np.ndarray: + """Read articulation joint positions from backend storage.""" + if backend == "physx": + return raw_backend.get_dof_positions().numpy() + if backend == "newton": + return art.data._sim_bind_joint_pos.numpy().reshape(art.num_instances, art.num_joints) + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.DOF_POSITION]._data.copy() + + +def _read_articulation_masses(backend: str, art, raw_backend) -> np.ndarray: + """Read articulation masses from backend storage.""" + if backend == "physx": + return raw_backend.get_masses().numpy() + if backend == "newton": + return art.data._sim_bind_body_mass.numpy().reshape(art.num_instances, art.num_bodies) + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.BODY_MASS]._data.copy() + + # --------------------------------------------------------------------------- # Tests: Articulation operations # --------------------------------------------------------------------------- @@ -1330,6 +1371,9 @@ def _make_item_mask(total: int, selected: list[int], device: str) -> wp.array: # Tests: Root writers — torch/warp × index/mask × all/subset × negative # --------------------------------------------------------------------------- +_backends = contract_backend("writes") +_production_backends = contract_backend("writes") + _ROOT_POSE_METHODS = ["root_pose", "root_link_pose", "root_com_pose"] _ROOT_VEL_METHODS = ["root_velocity", "root_link_velocity", "root_com_velocity"] @@ -1337,6 +1381,27 @@ def _make_item_mask(total: int, selected: list[int], device: str) -> wp.array: class TestArticulationWritersRoot: """Test root pose/velocity writers with all input combinations.""" + @_backends + @pytest.mark.parametrize("selection", ["index", "mask"]) + def test_root_pose_write_preserves_unselected_backend_row(self, backend, selection): + art, raw_backend = get_articulation( + backend, num_instances=2, num_joints=3, num_bodies=4, device="cpu" + ) + before = torch.from_numpy(_read_articulation_root_pose(backend, art, raw_backend)).clone() + root_pose = torch.tensor( + [[111.0, 112.0, 113.0, 0.0, 0.0, 0.0, 1.0], [121.0, 122.0, 123.0, 0.0, 0.0, 0.0, 1.0]], + dtype=torch.float32, + ) + + if selection == "index": + art.write_root_link_pose_to_sim_index(root_pose=root_pose[0:1], env_ids=_make_env_ids("cpu", True)) + else: + art.write_root_link_pose_to_sim_mask(root_pose=root_pose, env_mask=_make_env_mask(2, "cpu", True)) + after = torch.from_numpy(_read_articulation_root_pose(backend, art, raw_backend)) + + torch.testing.assert_close(after[0], root_pose[0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1], before[1], rtol=0.0, atol=0.0) + @_production_backends @pytest.mark.parametrize( "body_ordering", @@ -1609,6 +1674,54 @@ def test_write_root_velocity_to_sim_mask( class TestArticulationWritersJoint: """Test joint writers/setters with all input combinations.""" + def test_deprecated_friction_writer_forwards_to_canonical_method(self): + from isaaclab.assets.articulation import BaseArticulation + + forwarded = [] + + class Recorder: + def write_joint_friction_coefficient_to_sim(self, joint_friction, joint_ids=None, env_ids=None): + forwarded.append((joint_friction, joint_ids, env_ids)) + + joint_friction = torch.tensor([[0.25, 0.5]], dtype=torch.float32) + joint_ids = [1, 3] + env_ids = torch.tensor([0], dtype=torch.long) + + with pytest.warns(DeprecationWarning, match="write_joint_friction_coefficient_to_sim"): + BaseArticulation.write_joint_friction_to_sim( + Recorder(), joint_friction=joint_friction, joint_ids=joint_ids, env_ids=env_ids + ) + + assert len(forwarded) == 1 + assert forwarded[0][0] is joint_friction + assert forwarded[0][1] is joint_ids + assert forwarded[0][2] is env_ids + + @_backends + @pytest.mark.parametrize("selection", ["index", "mask"]) + def test_joint_position_write_preserves_unselected_backend_cells(self, backend, selection): + art, raw_backend = get_articulation( + backend, num_instances=2, num_joints=3, num_bodies=4, device="cpu" + ) + before = torch.from_numpy(_read_articulation_joint_positions(backend, art, raw_backend)).clone() + position = torch.tensor([[131.0, 132.0, 133.0], [141.0, 142.0, 143.0]], dtype=torch.float32) + + if selection == "index": + art.write_joint_position_to_sim_index( + position=position[0:1, 1:2], env_ids=_make_env_ids("cpu", True), joint_ids=[1] + ) + else: + art.write_joint_position_to_sim_mask( + position=position, + env_mask=_make_env_mask(2, "cpu", True), + joint_mask=_make_item_mask(3, [1], "cpu"), + ) + after = torch.from_numpy(_read_articulation_joint_positions(backend, art, raw_backend)) + + torch.testing.assert_close(after[0, 1], position[0, 1], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[0, 0], before[0, 0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1, 1], before[1, 1], rtol=0.0, atol=0.0) + @_backends @_default_dims @_default_devices @@ -1742,6 +1855,31 @@ def test_joint_writer_mask( class TestArticulationWritersBody: """Test body property writers/setters with all input combinations.""" + @_backends + @pytest.mark.parametrize("selection", ["index", "mask"]) + def test_mass_write_preserves_unselected_backend_cells(self, backend, selection): + art, raw_backend = get_articulation( + backend, num_instances=2, num_joints=3, num_bodies=4, device="cpu" + ) + before = torch.from_numpy(_read_articulation_masses(backend, art, raw_backend)).clone() + masses = torch.tensor( + [[151.0, 152.0, 153.0, 154.0], [161.0, 162.0, 163.0, 164.0]], dtype=torch.float32 + ) + + if selection == "index": + art.set_masses_index(masses=masses[0:1, 1:2], env_ids=_make_env_ids("cpu", True), body_ids=[1]) + else: + art.set_masses_mask( + masses=masses, + env_mask=_make_env_mask(2, "cpu", True), + body_mask=_make_item_mask(4, [1], "cpu"), + ) + after = torch.from_numpy(_read_articulation_masses(backend, art, raw_backend)) + + torch.testing.assert_close(after[0, 1], masses[0, 1], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[0, 0], before[0, 0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1, 1], before[1, 1], rtol=0.0, atol=0.0) + @_backends @_default_dims @_default_devices @@ -1763,8 +1901,8 @@ def test_body_writer_index( wp_dtype, trailing, ): - if backend == "newton" and method_base == "set_coms": - pytest.xfail("Newton only stores CoM position, not orientation") + if method_base == "set_coms": + require_backend_capability(backend, "com_orientation_write") art, _ = articulation_iface art.data.update(dt=0.01) method = getattr(art, f"{method_base}_index") @@ -1847,8 +1985,8 @@ def test_body_writer_mask( wp_dtype, trailing, ): - if backend == "newton" and method_base == "set_coms": - pytest.xfail("Newton only stores CoM position, not orientation") + if method_base == "set_coms": + require_backend_capability(backend, "com_orientation_write") art, _ = articulation_iface art.data.update(dt=0.01) method = getattr(art, f"{method_base}_mask") @@ -1907,6 +2045,8 @@ def _make_warp(n_envs, n_bods): # Tests: Alias/shorthand properties # --------------------------------------------------------------------------- +_backends = contract_backend("data") + class TestArticulationDataAliases: """Test that alias properties return the same shape/dtype as their canonical counterparts.""" @@ -1961,15 +2101,15 @@ def test_joint_aliases(self, backend, num_instances, num_joints, num_bodies, dev # Tendon tests — parametrize, properties, finders, data, writers # --------------------------------------------------------------------------- -# Newton does not support tendons (always 0), so exclude it from tendon tests. -_tendon_backends = pytest.mark.parametrize("backend", [b for b in BACKENDS if b != "newton"], indirect=False) +# Fixed and spatial tendon behavior have distinct backend capability declarations. +_tendon_backends = contract_backend("fixed_tendons") +_spatial_tendon_backends = contract_backend("spatial_tendons") _tendon_dims = pytest.mark.parametrize( "num_instances, num_joints, num_bodies, num_fixed_tendons, num_spatial_tendons", [ - (1, 2, 2, 1, 0), # fixed only - (2, 6, 7, 3, 2), # both types - (100, 8, 13, 4, 3), # large, both types + (1, 4, 3, 1, 0), # singleton fixed-only edge case + (2, 4, 3, 2, 2), # canonical case with both tendon types ], ) @@ -1994,7 +2134,7 @@ def test_num_fixed_tendons( art, _ = articulation_iface assert art.num_fixed_tendons == num_fixed_tendons - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_num_spatial_tendons( @@ -2031,7 +2171,7 @@ def test_fixed_tendon_names( assert len(names) == num_fixed_tendons assert all(isinstance(n, str) for n in names) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_spatial_tendon_names( @@ -2101,7 +2241,7 @@ def test_find_fixed_tendons_single( assert indices == [0] assert names == [first] - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_find_spatial_tendons_all( @@ -2123,7 +2263,7 @@ def test_find_spatial_tendons_all( assert len(indices) == num_spatial_tendons assert len(names) == num_spatial_tendons - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_find_spatial_tendons_single( @@ -2211,6 +2351,7 @@ def test_fixed_tendon_limit_stiffness( device, articulation_iface, ): + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2234,6 +2375,7 @@ def test_fixed_tendon_rest_length( device, articulation_iface, ): + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2257,6 +2399,7 @@ def test_fixed_tendon_offset( device, articulation_iface, ): + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2280,6 +2423,7 @@ def test_fixed_tendon_pos_limits( device, articulation_iface, ): + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) from isaaclab.utils.warp import ProxyArray @@ -2297,7 +2441,7 @@ def test_fixed_tendon_pos_limits( # -- Spatial tendon data properties -- - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_spatial_tendon_stiffness( @@ -2322,7 +2466,7 @@ def test_spatial_tendon_stiffness( name="spatial_tendon_stiffness", ) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_spatial_tendon_damping( @@ -2347,7 +2491,7 @@ def test_spatial_tendon_damping( name="spatial_tendon_damping", ) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_spatial_tendon_limit_stiffness( @@ -2372,7 +2516,7 @@ def test_spatial_tendon_limit_stiffness( name="spatial_tendon_limit_stiffness", ) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_spatial_tendon_offset( @@ -2441,6 +2585,8 @@ def test_fixed_tendon_writer_index( wp_dtype, accepts_float, ): + if method_base not in {"set_fixed_tendon_stiffness", "set_fixed_tendon_damping"}: + require_backend_capability(backend, "fixed_tendon_extended_write_index") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2506,6 +2652,7 @@ def test_fixed_tendon_writer_mask( wp_dtype, accepts_float, ): + require_backend_capability(backend, "fixed_tendon_write_mask") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2559,7 +2706,7 @@ def test_fixed_tendon_writer_mask( class TestArticulationWritersSpatialTendon: """Test spatial tendon writers/setters with all input combinations.""" - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices @pytest.mark.parametrize( @@ -2620,7 +2767,7 @@ def test_spatial_tendon_writer_index( with pytest.raises((AssertionError, RuntimeError)): method(**{kwarg: _make_bad_data_warp((num_instances, num_spatial_tendons), device, wp_dtype)}) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices @pytest.mark.parametrize( @@ -2703,6 +2850,7 @@ def test_write_fixed_tendon_properties_to_sim_index( device, articulation_iface, ): + require_backend_capability(backend, "fixed_tendon_write_to_sim_index") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2727,6 +2875,7 @@ def test_write_fixed_tendon_properties_to_sim_mask( device, articulation_iface, ): + require_backend_capability(backend, "fixed_tendon_write_to_sim_mask") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2737,7 +2886,7 @@ def test_write_fixed_tendon_properties_to_sim_mask( art.write_fixed_tendon_properties_to_sim_mask(env_mask=_make_env_mask(num_instances, device, True)) art.write_fixed_tendon_properties_to_sim_mask(fixed_tendon_mask=_make_item_mask(num_fixed_tendons, [0], device)) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_write_spatial_tendon_properties_to_sim_index( @@ -2763,7 +2912,7 @@ def test_write_spatial_tendon_properties_to_sim_index( spatial_tendon_ids=wp.array([0], dtype=wp.int64, device=device) ) - @_tendon_backends + @_spatial_tendon_backends @_tendon_dims @_default_devices def test_write_spatial_tendon_properties_to_sim_mask( diff --git a/source/isaaclab/test/assets/_articulation_iface_test_utils.py b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py similarity index 83% rename from source/isaaclab/test/assets/_articulation_iface_test_utils.py rename to source/isaaclab/test/assets/contract/_articulation_contract_utils.py index 0b2caf776390..2054093fac1d 100644 --- a/source/isaaclab/test/assets/_articulation_iface_test_utils.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py @@ -6,11 +6,12 @@ # ignore private usage of variables warning # pyright: reportPrivateUsage=none -"""Shared mocked articulation backend factories for interface tests.""" +"""Shared mocked articulation backend factories for contract tests.""" from unittest.mock import MagicMock -from _iface_test_boot import simulation_app +from ._contract_boot import simulation_app +from .capabilities import available_backends, backend_unavailable_reasons import numpy as np import torch @@ -19,43 +20,79 @@ from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg from isaaclab.utils.wrench_composer import WrenchComposer -BACKENDS: list[str] = [] -BACKEND_UNAVAILABLE_REASONS: dict[str, str] = {} +from ._manager_patch_scope import patch_contract_manager -try: +BACKENDS = available_backends("api") +BACKEND_UNAVAILABLE_REASONS = backend_unavailable_reasons() + +if "physx" in BACKENDS: from isaaclab_physx.assets.articulation.articulation import Articulation as PhysXArticulation from isaaclab_physx.assets.articulation.articulation_data import ArticulationData as PhysXArticulationData from isaaclab_physx.physics import PhysxManager as SimulationManager from isaaclab_physx.test.fixtures.views import MockArticulationViewWarp as PhysXMockArticulationViewWarp -except ImportError as error: - BACKEND_UNAVAILABLE_REASONS["physx"] = f"{type(error).__name__}: {error}" -else: - # PhysX data classes need gravity even though interface tests do not create a physics scene. - _mock_physics_sim_view = MagicMock() - _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) - SimulationManager.get_physics_sim_view = MagicMock(return_value=_mock_physics_sim_view) - - BACKENDS.append("physx") -try: +if "newton" in BACKENDS: from isaaclab_newton.assets.articulation.articulation import Articulation as NewtonArticulation from isaaclab_newton.assets.articulation.articulation_data import ArticulationData as NewtonArticulationData from isaaclab_newton.test.fixtures.views import MockNewtonArticulationView as NewtonMockArticulationView -except ImportError as error: - BACKEND_UNAVAILABLE_REASONS["newton"] = f"{type(error).__name__}: {error}" -else: - BACKENDS.append("newton") -try: +if "ovphysx" in BACKENDS: import ovphysx # noqa: F401 from isaaclab_ov.assets.articulation.articulation import Articulation as OvPhysxArticulation from isaaclab_ov.assets.articulation.articulation_data import ArticulationData as OvPhysxArticulationData from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet -except ImportError as error: - BACKEND_UNAVAILABLE_REASONS["ovphysx"] = f"{type(error).__name__}: {error}" -else: - BACKENDS.append("ovphysx") + + +def _install_physx_recording_setters(mock_view) -> None: + """Install contract-local PhysX setters that record and apply staged rows.""" + storage_by_method = { + "set_root_transforms": "_root_transforms", + "set_root_velocities": "_root_velocities", + "set_dof_positions": "_dof_positions", + "set_dof_velocities": "_dof_velocities", + "set_dof_position_targets": "_dof_position_targets", + "set_dof_velocity_targets": "_dof_velocity_targets", + "set_dof_actuation_forces": "_dof_actuation_forces", + "set_dof_limits": "_dof_limits", + "set_dof_stiffnesses": "_dof_stiffnesses", + "set_dof_dampings": "_dof_dampings", + "set_dof_max_forces": "_dof_max_forces", + "set_dof_max_velocities": "_dof_max_velocities", + "set_dof_armatures": "_dof_armatures", + "set_dof_friction_coefficients": "_dof_friction_coefficients", + "set_dof_friction_properties": "_dof_friction_properties", + "set_masses": "_masses", + "set_coms": "_coms", + "set_inertias": "_inertias", + } + mock_view._contract_write_calls = [] + + def make_setter(method_name: str, storage_name: str): + def setter(values: wp.array, indices: wp.array | None = None) -> None: + values_np = values.numpy().copy() + indices_np = None if indices is None else indices.numpy().astype(np.int64, copy=True) + mock_view._contract_write_calls.append((method_name, values_np.copy(), indices_np)) + stored = getattr(mock_view, storage_name, None) + if stored is None: + shape = (mock_view._count, *values_np.shape[1:]) + stored = wp.zeros(shape, dtype=wp.float32, device=values.device) + setattr(mock_view, storage_name, stored) + stored_np = stored.numpy() + if indices_np is None: + stored_np[...] = values_np.reshape(stored_np.shape) + else: + if values_np.size == stored_np.size: + normalized = values_np.reshape(stored_np.shape) + else: + normalized = values_np.reshape(len(indices_np), *stored_np.shape[1:]) + staged_rows = normalized[indices_np] if normalized.shape[0] == stored_np.shape[0] else normalized + stored_np[indices_np] = staged_rows + + return setter + + for method_name, storage_name in storage_by_method.items(): + setattr(mock_view, method_name, make_setter(method_name, storage_name)) def create_physx_articulation( @@ -70,6 +107,11 @@ def create_physx_articulation( body_ordering: tuple[str, ...] | None = None, ): """Create a test Articulation instance with mocked dependencies.""" + mock_physics_sim_view = MagicMock() + mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) + patch_contract_manager( + SimulationManager, "get_physics_sim_view", MagicMock(return_value=mock_physics_sim_view) + ) joint_names = [f"joint_{i}" for i in range(num_joints)] body_names = [f"body_{i}" for i in range(num_bodies)] fixed_tendon_names = [f"fixed_tendon_{i}" for i in range(num_fixed_tendons)] @@ -96,7 +138,8 @@ def create_physx_articulation( max_spatial_tendons=num_spatial_tendons, ) mock_view.set_random_mock_data() - mock_view._noop_setters = True + mock_view._noop_setters = False + _install_physx_recording_setters(mock_view) # Set up the mock view's metatype for accessing names/counts mock_metatype = MagicMock() @@ -188,7 +231,9 @@ def create_physx_articulation( object.__setattr__(articulation, "_sim_env_ids_views", {}) cpu_env_ids = wp.array(np.arange(N, dtype=np.int32), device="cpu") object.__setattr__(articulation, "_cpu_env_ids_all", cpu_env_ids) - object.__setattr__(articulation, "_cpu_env_ids", wp.empty(N, dtype=wp.int32, device="cpu", pinned=True)) + object.__setattr__( + articulation, "_cpu_env_ids", wp.empty(N, dtype=wp.int32, device="cpu", pinned=wp.is_cuda_available()) + ) object.__setattr__(articulation, "_cpu_env_ids_views", {}) object.__setattr__(articulation, "_cpu_joint_stiffness", wp.zeros((N, J), dtype=wp.float32, device="cpu")) object.__setattr__(articulation, "_cpu_joint_damping", wp.zeros((N, J), dtype=wp.float32, device="cpu")) @@ -322,6 +367,7 @@ def create_newton_articulation( num_instances=num_instances, num_bodies=num_bodies, num_joints=num_joints, + num_tendons=num_fixed_tendons, device=device, is_fixed_base=is_fixed_base, joint_names=joint_names, @@ -329,7 +375,13 @@ def create_newton_articulation( tendon_names=fixed_tendon_names, ) mock_view.set_random_mock_data() - mock_view._noop_setters = True + mock_view._noop_setters = False + mock_view._attributes["mujoco.tendon_stiffness"] = wp.zeros( + (num_instances, 1, num_fixed_tendons), dtype=wp.float32, device=device + ) + mock_view._attributes["mujoco.tendon_damping"] = wp.zeros( + (num_instances, 1, num_fixed_tendons), dtype=wp.float32, device=device + ) # Mock NewtonManager (aliased as SimulationManager in Newton modules) mock_model = MagicMock() @@ -358,15 +410,8 @@ def create_newton_articulation( mock_manager.get_control.return_value = mock_control # Patch SimulationManager in the Newton data module - original_sim_manager = newton_data_module.SimulationManager - newton_data_module.SimulationManager = mock_manager - - try: - data = NewtonArticulationData(mock_view, device) - finally: - newton_data_module.SimulationManager = original_sim_manager - mock_view._tendon_count = num_fixed_tendons - + patch_contract_manager(newton_data_module, "SimulationManager", mock_manager) + data = NewtonArticulationData(mock_view, device) # Create Articulation shell (bypass __init__) articulation = object.__new__(NewtonArticulation) diff --git a/source/isaaclab/test/assets/test_articulation_ordering_iface.py b/source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py similarity index 99% rename from source/isaaclab/test/assets/test_articulation_ordering_iface.py rename to source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py index 85aef24c6d76..623f32d9bd2d 100644 --- a/source/isaaclab/test/assets/test_articulation_ordering_iface.py +++ b/source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py @@ -5,7 +5,7 @@ # pyright: reportPrivateUsage=none -"""Mocked cross-backend articulation ordering interface tests.""" +"""Mocked cross-backend articulation ordering contract cases.""" from unittest.mock import MagicMock @@ -13,7 +13,8 @@ import pytest import torch import warp as wp -from _articulation_iface_test_utils import BACKEND_UNAVAILABLE_REASONS, BACKENDS, get_articulation +from ._articulation_contract_utils import BACKEND_UNAVAILABLE_REASONS, BACKENDS, get_articulation +from .capabilities import backend_parameters, contract_backend from _pytest.mark.structures import ParameterSet from isaaclab.utils.buffers import TimestampedBufferWarp @@ -891,9 +892,10 @@ def _get_backend_body_property_tensors(backend: str, art, raw_backend) -> dict[s def _backend_param(backend: str, *values, **kwargs) -> ParameterSet: - """Build a backend parameter that skips unavailable plugins at collection time.""" + """Build an ordering-capability parameter with optional additional values.""" + declared_parameter = backend_parameters("ordering", names=(backend,))[0] marks = list(kwargs.pop("marks", ())) - marks.append(pytest.mark.skipif(backend not in BACKENDS, reason=_backend_unavailable_reason(backend))) + marks.extend(declared_parameter.marks) return pytest.param(backend, *values, marks=marks, **kwargs) @@ -909,15 +911,9 @@ def _backend_unavailable_reason(backend: str) -> str: _requires_physx = pytest.mark.skipif("physx" not in BACKENDS, reason=_backend_unavailable_reason("physx")) _requires_ovphysx = pytest.mark.skipif("ovphysx" not in BACKENDS, reason=_backend_unavailable_reason("ovphysx")) _requires_newton = pytest.mark.skipif("newton" not in BACKENDS, reason=_backend_unavailable_reason("newton")) -_all_backends = pytest.mark.parametrize( - "backend", [_backend_param(backend) for backend in ("physx", "ovphysx", "newton")], indirect=False -) -_physx_ovphysx_backends = pytest.mark.parametrize( - "backend", [_backend_param(backend) for backend in ("physx", "ovphysx")], indirect=False -) -_dynamics_ordering_backends = pytest.mark.parametrize( - "backend", [_backend_param(backend) for backend in ("physx", "newton")], indirect=False -) +_all_backends = contract_backend("ordering") +_physx_ovphysx_backends = contract_backend("ordering", names=("physx", "ovphysx")) +_dynamics_ordering_backends = contract_backend("ordering", names=("physx", "newton")) _non_mock_backends = _all_backends @@ -955,7 +951,7 @@ def test_reversed_body_ordering_reorders_public_body_quantities( _assert_proxy_close(ordered_art.data.root_link_vel_w, identity_root_link_vel_w) @_non_mock_backends - @pytest.mark.parametrize("ordering_mode", ["reversed", "cyclic"]) + @pytest.mark.parametrize("ordering_mode", ["reversed"]) @pytest.mark.parametrize("num_instances, num_joints, num_bodies", [(2, 1, 3)]) @pytest.mark.parametrize("device", ["cpu"]) def test_body_ordering_reorders_public_body_properties( @@ -1986,7 +1982,7 @@ def test_legacy_position_target_accepts_joint_slice(self, backend: str) -> None: torch.testing.assert_close(art.actuators.target_command.position.torch, expected) @_non_mock_backends - @pytest.mark.parametrize("ordering_mode", ["none", "reversed", "cyclic"]) + @pytest.mark.parametrize("ordering_mode", ["none", "reversed"]) @pytest.mark.parametrize("is_fixed_base", [False, True], ids=["floating", "fixed"]) @pytest.mark.parametrize("device", ["cpu"]) def test_external_wrenches_are_written_in_backend_body_order(self, backend, ordering_mode, is_fixed_base, device): @@ -2059,7 +2055,7 @@ def test_ovphysx_configured_defaults_use_public_joint_names(self): np.testing.assert_array_equal(art.data.default_joint_pos.warp.numpy(), expected) @_requires_ovphysx - @pytest.mark.parametrize("ordering_mode", ["reversed", "cyclic"]) + @pytest.mark.parametrize("ordering_mode", ["reversed"]) def test_ovphysx_implicit_targets_are_written_in_backend_order(self, ordering_mode: str) -> None: """Write implicit position and velocity targets under their matching backend joint names.""" from isaaclab_ov import tensor_types as TT @@ -2137,7 +2133,7 @@ def strict_set_attribute(name, values, *, indices=None, mask=None): np.testing.assert_array_equal(raw_backend.bindings[TT.DOF_ACTUATION_FORCE]._data, expected) @_requires_physx - @pytest.mark.parametrize("ordering_mode", ["reversed", "cyclic"]) + @pytest.mark.parametrize("ordering_mode", ["reversed"]) def test_physx_newton_actuator_forces_are_written_in_backend_order(self, ordering_mode: str): """Write Newton-actuator PhysX forces in backend joint order.""" num_instances = 2 @@ -2320,7 +2316,7 @@ class TestArticulationWritersBody: """Test body property writers/setters with all input combinations.""" @_non_mock_backends - @pytest.mark.parametrize("ordering_mode", ["reversed", "cyclic"]) + @pytest.mark.parametrize("ordering_mode", ["reversed"]) @pytest.mark.parametrize("selection", ["index", "mask"]) def test_body_ordering_routes_property_writes_to_backend(self, backend: str, ordering_mode: str, selection: str): """Route partial public property writes to matching backend bodies.""" diff --git a/source/isaaclab/test/assets/contract/_contract_boot.py b/source/isaaclab/test/assets/contract/_contract_boot.py new file mode 100644 index 000000000000..90991c955eb1 --- /dev/null +++ b/source/isaaclab/test/assets/contract/_contract_boot.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Shared Kit and kitless bootstrap for backend contract tests.""" + +import os +import sys +from importlib.machinery import ModuleSpec +from unittest.mock import MagicMock + +_kitless = "ovphysx" in os.environ.get("LD_PRELOAD", "") or ( + os.environ.get("LD_PRELOAD", "") == "" and "EXP_PATH" not in os.environ +) + +if not _kitless: + from isaaclab.app import AppLauncher + + simulation_app = AppLauncher(headless=True).app +else: + simulation_app = None + + def _install_stub(module_name: str, *, is_package: bool = False) -> MagicMock: + """Install one faithful import-boundary module double.""" + if module_name in sys.modules: + return sys.modules[module_name] + stub = MagicMock() + stub.__spec__ = ModuleSpec(module_name, loader=None, is_package=is_package) + if is_package: + stub.__path__ = [] + sys.modules[module_name] = stub + if "." in module_name: + parent_name, attribute = module_name.rsplit(".", 1) + parent = sys.modules[parent_name] + setattr(parent, attribute, stub) + return stub + + # Normal worktree installs include the real ``isaaclab_physx`` package but + # not Kit's Python runtime. Stub only that external boundary so contracts + # still import the real PhysX asset/data classes and fixture views. + _install_stub("carb") + + # ``omni`` is a real namespace package in kitless runs. Install missing + # submodules in both ``sys.modules`` and the namespace attributes. + import omni as _omni + + for _module_name, _is_package in ( + ("omni.kit", True), + ("omni.kit.app", False), + ("omni.physics", True), + ("omni.physics.tensors", False), + ("omni.physx", False), + ("omni.timeline", False), + ("omni.usd", False), + ): + _install_stub(_module_name, is_package=_is_package) + sys.modules["omni.kit.app"].get_app.return_value = None + + for _module_name, _is_package in ( + ("isaacsim", True), + ("isaacsim.core", True), + ("isaacsim.core.simulation_manager", False), + ): + _install_stub(_module_name, is_package=_is_package) diff --git a/source/isaaclab/test/assets/contract/_manager_patch_scope.py b/source/isaaclab/test/assets/contract/_manager_patch_scope.py new file mode 100644 index 000000000000..774fc6b4f502 --- /dev/null +++ b/source/isaaclab/test/assets/contract/_manager_patch_scope.py @@ -0,0 +1,45 @@ +# 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 + +"""Test-local backend-manager patching for mocked asset factories.""" + +from collections.abc import Iterator +from contextlib import contextmanager +import inspect + + +_MISSING = object() +_patch_scopes: list[dict[tuple[object, str], object]] = [] + + +@contextmanager +def contract_manager_patch_scope() -> Iterator[None]: + """Restore backend manager attributes changed by factories in this scope.""" + patches: dict[tuple[object, str], object] = {} + _patch_scopes.append(patches) + try: + yield + finally: + for (owner, name), original in reversed(patches.items()): + if original is _MISSING: + delattr(owner, name) + else: + setattr(owner, name, original) + popped = _patch_scopes.pop() + assert popped is patches + + +def patch_contract_manager(owner: object, name: str, replacement: object) -> None: + """Patch a manager binding until the current contract test finishes.""" + if not _patch_scopes: + raise RuntimeError("contract factories must run inside contract_manager_patch_scope()") + patches = _patch_scopes[-1] + key = (owner, name) + if key not in patches: + try: + patches[key] = inspect.getattr_static(owner, name) + except AttributeError: + patches[key] = _MISSING + setattr(owner, name, replacement) diff --git a/source/isaaclab/test/assets/test_rigid_object_collection_iface.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py similarity index 90% rename from source/isaaclab/test/assets/test_rigid_object_collection_iface.py rename to source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py index 039140c3f229..2cd4db73b274 100644 --- a/source/isaaclab/test/assets/test_rigid_object_collection_iface.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py @@ -7,9 +7,8 @@ # pyright: reportPrivateUsage=none """ -Checks that the rigid object collection interfaces are consistent across backends, and are providing -the exact same data as what the base rigid object collection class advertises. All rigid object -collection interfaces need to comply with the same interface contract. +Checks that rigid object collection implementations are consistent across backends and provide the exact same data as +the base rigid object collection class advertises. All rigid object collections need to comply with the shared contract. The setup is a bit convoluted so that we can run these tests without requiring Isaac Sim or GPU simulation. """ @@ -18,10 +17,8 @@ import pytest import torch import warp as wp -from _rigid_object_collection_iface_test_utils import BACKENDS, get_rigid_object_collection - -pytestmark = pytest.mark.integration - +from ._rigid_object_collection_contract_utils import BACKENDS, get_rigid_object_collection +from .capabilities import contract_backend, require_backend_capability @pytest.fixture def collection_iface(request): @@ -60,21 +57,15 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name # Common parametrize decorators -_backends = pytest.mark.parametrize("backend", BACKENDS, indirect=False) -_default_dims = pytest.mark.parametrize("num_instances", [1, 2, 100]) +_backends = contract_backend("api") +_default_dims = pytest.mark.parametrize("num_instances", [2]) -_default_bodies = pytest.mark.parametrize("num_bodies", [1, 3]) +_default_bodies = pytest.mark.parametrize("num_bodies", [3]) -_default_devices = pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -_index_resolution_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton") if backend in BACKENDS], indirect=False -) -_reshape_3d_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False -) -_production_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False -) +_default_devices = pytest.mark.parametrize("device", ["cpu"]) +_index_resolution_backends = contract_backend("index_resolution", names=("physx", "newton")) +_reshape_3d_backends = contract_backend("data") +_production_backends = contract_backend("api") # --------------------------------------------------------------------------- @@ -179,6 +170,30 @@ def _make_item_mask(total: int, selected: list[int], device: str) -> wp.array: return wp.array(mask_np, dtype=wp.bool, device=device) +def _read_collection_root_poses( + backend: str, raw_backend, num_instances: int, num_bodies: int +) -> np.ndarray: + """Read collection root poses in public ``(env, body)`` order.""" + if backend == "physx": + return raw_backend.get_transforms().numpy().reshape(num_bodies, num_instances, 7).transpose(1, 0, 2) + if backend == "newton": + return raw_backend.get_root_transforms(None).numpy().reshape(num_instances, num_bodies, 7) + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.LINK_POSE]._data.copy() + + +def _read_collection_masses(backend: str, raw_backend, num_instances: int, num_bodies: int) -> np.ndarray: + """Read collection masses in public ``(env, body)`` order.""" + if backend == "physx": + return raw_backend.get_masses().numpy().reshape(num_bodies, num_instances).transpose(1, 0) + if backend == "newton": + return raw_backend.get_attribute("body_mass", None).numpy().reshape(num_instances, num_bodies) + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.BODY_MASS]._data.copy() + + # --------------------------------------------------------------------------- # Tests: Index resolution helpers # --------------------------------------------------------------------------- @@ -395,6 +410,9 @@ def test_find_objects_forwards_return_mode_with_alias_warning(self, backend): # --------------------------------------------------------------------------- +_backends = contract_backend("data") + + class TestCollectionDataBodyState: """Test data properties for body state.""" @@ -829,6 +847,9 @@ def test_default_body_vel(self, backend, num_instances, num_bodies, device, coll _BODY_VEL_METHODS = ["body_velocity", "body_com_velocity", "body_link_velocity"] +_production_backends = contract_backend("writes") + + class TestCollectionCacheInvalidation: @_production_backends def test_pose_write_invalidates_pose_dependent_caches(self, backend): @@ -909,9 +930,43 @@ def set_coms() -> None: _assert_buffers_stale(obj.data, buffers) +_backends = contract_backend("writes") + + class TestCollectionWritersPose: """Test body pose/velocity writers with all input combinations.""" + @_backends + @pytest.mark.parametrize("selection", ["index", "mask"]) + def test_body_pose_write_preserves_unselected_backend_cells(self, backend, selection): + num_instances, num_bodies = 2, 3 + obj, raw_backend = get_rigid_object_collection(backend, num_instances, num_bodies, "cpu") + before = torch.from_numpy( + _read_collection_root_poses(backend, raw_backend, num_instances, num_bodies) + ).clone() + body_poses = torch.zeros((num_instances, num_bodies, 7), dtype=torch.float32) + body_poses[..., 6] = 1.0 + body_poses[0, 1, :3] = torch.tensor([71.0, 72.0, 73.0]) + body_poses[1, 1, :3] = torch.tensor([81.0, 82.0, 83.0]) + + if selection == "index": + obj.write_body_pose_to_sim_index( + body_poses=body_poses[0:1, 1:2], + env_ids=_make_env_ids("cpu", True), + body_ids=_make_body_ids("cpu", [1]), + ) + else: + obj.write_body_pose_to_sim_mask( + body_poses=body_poses, + env_mask=_make_env_mask(num_instances, "cpu", True), + body_mask=_make_item_mask(num_bodies, [1], "cpu"), + ) + after = torch.from_numpy(_read_collection_root_poses(backend, raw_backend, num_instances, num_bodies)) + + torch.testing.assert_close(after[0, 1], body_poses[0, 1], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[0, 0], before[0, 0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1, 1], before[1, 1], rtol=0.0, atol=0.0) + # -- index variants for pose -- @_backends @@ -1112,6 +1167,32 @@ def test_write_body_velocity_to_sim_mask( class TestCollectionWritersBody: """Test body property writers/setters with all input combinations.""" + @_backends + @pytest.mark.parametrize("selection", ["index", "mask"]) + def test_mass_write_preserves_unselected_backend_cells(self, backend, selection): + num_instances, num_bodies = 2, 3 + obj, raw_backend = get_rigid_object_collection(backend, num_instances, num_bodies, "cpu") + before = torch.from_numpy(_read_collection_masses(backend, raw_backend, num_instances, num_bodies)).clone() + masses = torch.tensor([[91.0, 92.0, 93.0], [101.0, 102.0, 103.0]], dtype=torch.float32) + + if selection == "index": + obj.set_masses_index( + masses=masses[0:1, 1:2], + env_ids=_make_env_ids("cpu", True), + body_ids=_make_body_ids("cpu", [1]), + ) + else: + obj.set_masses_mask( + masses=masses, + env_mask=_make_env_mask(num_instances, "cpu", True), + body_mask=_make_item_mask(num_bodies, [1], "cpu"), + ) + after = torch.from_numpy(_read_collection_masses(backend, raw_backend, num_instances, num_bodies)) + + torch.testing.assert_close(after[0, 1], masses[0, 1], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[0, 0], before[0, 0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1, 1], before[1, 1], rtol=0.0, atol=0.0) + @_backends @_default_dims @_default_bodies @@ -1124,8 +1205,8 @@ class TestCollectionWritersBody: def test_body_writer_index( self, backend, num_instances, num_bodies, device, collection_iface, method_base, kwarg, wp_dtype, trailing ): - if backend == "newton" and method_base == "set_coms": - pytest.xfail("Newton set_coms expects vec3f (position only), not transformf (pose)") + if method_base == "set_coms": + require_backend_capability(backend, "com_orientation_write") obj, _ = collection_iface obj.data.update(dt=0.01) method = getattr(obj, f"{method_base}_index") @@ -1191,8 +1272,8 @@ def _make_warp(n_envs, n_bods): def test_body_writer_mask( self, backend, num_instances, num_bodies, device, collection_iface, method_base, kwarg, wp_dtype, trailing ): - if backend == "newton" and method_base == "set_coms": - pytest.xfail("Newton set_coms expects vec3f (position only), not transformf (pose)") + if method_base == "set_coms": + require_backend_capability(backend, "com_orientation_write") obj, _ = collection_iface obj.data.update(dt=0.01) method = getattr(obj, f"{method_base}_mask") @@ -1252,6 +1333,9 @@ def _make_warp(n_envs, n_bods): # --------------------------------------------------------------------------- +_backends = contract_backend("data") + + class TestCollectionDataAliases: """Test that alias properties return the same shape/dtype as their canonical counterparts.""" diff --git a/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py similarity index 80% rename from source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py rename to source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py index 602e4949729f..624a4c9716bb 100644 --- a/source/isaaclab/test/assets/_rigid_object_collection_iface_test_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py @@ -6,11 +6,12 @@ # ignore private usage of variables warning # pyright: reportPrivateUsage=none -"""Shared mocked rigid-object-collection backend factories for interface tests.""" +"""Shared mocked rigid-object-collection backend factories for contract tests.""" from unittest.mock import MagicMock -from _iface_test_boot import simulation_app +from ._contract_boot import simulation_app +from .capabilities import available_backends import numpy as np import warp as wp @@ -19,9 +20,11 @@ from isaaclab.assets.rigid_object_collection.rigid_object_collection_cfg import RigidObjectCollectionCfg from isaaclab.utils.wrench_composer import WrenchComposer -BACKENDS: list[str] = [] +from ._manager_patch_scope import patch_contract_manager -try: +BACKENDS = available_backends("api") + +if "physx" in BACKENDS: from isaaclab_physx.assets.rigid_object_collection.rigid_object_collection import ( RigidObjectCollection as PhysXRigidObjectCollection, ) @@ -30,17 +33,8 @@ ) from isaaclab_physx.physics import PhysxManager as SimulationManager from isaaclab_physx.test.fixtures.views import MockRigidBodyViewWarp as PhysXMockRigidBodyViewWarp -except ImportError: - pass -else: - # PhysX data classes need gravity even though interface tests do not create a physics scene. - _mock_physics_sim_view = MagicMock() - _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) - SimulationManager.get_physics_sim_view = MagicMock(return_value=_mock_physics_sim_view) - - BACKENDS.append("physx") -try: +if "newton" in BACKENDS: from isaaclab_newton.assets.rigid_object_collection.rigid_object_collection import ( RigidObjectCollection as NewtonRigidObjectCollection, ) @@ -48,12 +42,8 @@ RigidObjectCollectionData as NewtonRigidObjectCollectionData, ) from isaaclab_newton.test.fixtures.views import MockNewtonCollectionView as NewtonMockCollectionView -except ImportError: - pass -else: - BACKENDS.append("newton") -try: +if "ovphysx" in BACKENDS: import ovphysx # noqa: F401 from isaaclab_ov.assets.rigid_object_collection.rigid_object_collection import ( @@ -63,11 +53,39 @@ RigidObjectCollectionData as OvPhysxRigidObjectCollectionData, ) from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet -except ImportError: - pass -else: - if hasattr(OvPhysxRigidObjectCollection, "_create_buffers"): - BACKENDS.append("ovphysx") + + +def _install_physx_recording_setters(mock_view) -> None: + """Install contract-local PhysX setters that record and apply staged rows.""" + storage_by_method = { + "set_transforms": "_transforms", + "set_velocities": "_velocities", + "set_masses": "_masses", + "set_coms": "_coms", + "set_inertias": "_inertias", + } + mock_view._contract_write_calls = [] + + def make_setter(method_name: str, storage_name: str): + def setter(values: wp.array, indices: wp.array | None = None) -> None: + values_np = values.numpy().copy() + indices_np = None if indices is None else indices.numpy().astype(np.int64, copy=True) + mock_view._contract_write_calls.append((method_name, values_np.copy(), indices_np)) + stored_np = getattr(mock_view, storage_name).numpy() + if indices_np is None: + stored_np[...] = values_np.reshape(stored_np.shape) + else: + if values_np.size == stored_np.size: + normalized = values_np.reshape(stored_np.shape) + else: + normalized = values_np.reshape(len(indices_np), *stored_np.shape[1:]) + staged_rows = normalized[indices_np] if normalized.shape[0] == stored_np.shape[0] else normalized + stored_np[indices_np] = staged_rows + + return setter + + for method_name, storage_name in storage_by_method.items(): + setattr(mock_view, method_name, make_setter(method_name, storage_name)) def create_physx_rigid_object_collection( @@ -76,6 +94,11 @@ def create_physx_rigid_object_collection( device: str = "cuda:0", ): """Create a test RigidObjectCollection instance with mocked dependencies.""" + mock_physics_sim_view = MagicMock() + mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) + patch_contract_manager( + SimulationManager, "get_physics_sim_view", MagicMock(return_value=mock_physics_sim_view) + ) collection = object.__new__(PhysXRigidObjectCollection) rigid_objects = {f"object_{i}": RigidObjectCfg(prim_path=f"/World/Object_{i}") for i in range(num_bodies)} @@ -87,7 +110,8 @@ def create_physx_rigid_object_collection( device=device, ) mock_view.set_random_mock_data() - mock_view._noop_setters = True + mock_view._noop_setters = False + _install_physx_recording_setters(mock_view) object.__setattr__(collection, "_root_view", mock_view) object.__setattr__(collection, "_device", device) @@ -122,10 +146,12 @@ def create_physx_rigid_object_collection( object.__setattr__(collection, "_ALL_VIEW_INDICES", all_view_ids) object.__setattr__(collection, "_sim_view_ids", wp.empty(num_view_ids, dtype=wp.int32, device=device)) object.__setattr__(collection, "_sim_view_ids_views", {}) - cpu_all_view_ids = wp.empty(num_view_ids, dtype=wp.int32, device="cpu", pinned=True) + cpu_all_view_ids = wp.empty(num_view_ids, dtype=wp.int32, device="cpu", pinned=wp.is_cuda_available()) wp.copy(cpu_all_view_ids, all_view_ids) object.__setattr__(collection, "_cpu_all_view_ids", cpu_all_view_ids) - object.__setattr__(collection, "_cpu_view_ids", wp.empty(num_view_ids, dtype=wp.int32, device="cpu", pinned=True)) + object.__setattr__( + collection, "_cpu_view_ids", wp.empty(num_view_ids, dtype=wp.int32, device="cpu", pinned=wp.is_cuda_available()) + ) object.__setattr__(collection, "_cpu_view_ids_views", {}) return collection, mock_view @@ -150,7 +176,7 @@ def create_newton_rigid_object_collection( body_names=body_names, ) mock_view.set_random_mock_data() - mock_view._noop_setters = True + mock_view._noop_setters = False # Mock NewtonManager (aliased as SimulationManager in Newton modules) mock_model = MagicMock() @@ -170,16 +196,9 @@ def create_newton_rigid_object_collection( mock_manager.get_control.return_value = mock_control # Patch SimulationManager in both data and collection modules - original_data_manager = newton_data_module.SimulationManager - original_coll_manager = newton_coll_module.SimulationManager - newton_data_module.SimulationManager = mock_manager - newton_coll_module.SimulationManager = mock_manager - - try: - data = NewtonRigidObjectCollectionData(mock_view, num_bodies, device) - finally: - newton_data_module.SimulationManager = original_data_manager - newton_coll_module.SimulationManager = original_coll_manager + patch_contract_manager(newton_data_module, "SimulationManager", mock_manager) + patch_contract_manager(newton_coll_module, "SimulationManager", mock_manager) + data = NewtonRigidObjectCollectionData(mock_view, num_bodies, device) # Create collection shell (bypass __init__) collection = object.__new__(NewtonRigidObjectCollection) diff --git a/source/isaaclab/test/assets/test_rigid_object_iface.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py similarity index 86% rename from source/isaaclab/test/assets/test_rigid_object_iface.py rename to source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py index e8a6147b7b4f..5474db9028c9 100644 --- a/source/isaaclab/test/assets/test_rigid_object_iface.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py @@ -7,8 +7,8 @@ # pyright: reportPrivateUsage=none """ -Checks that the rigid object interfaces are consistent across backends, and are providing the exact same data as what -the base rigid object class advertises. All rigid object interfaces need to comply with the same interface contract. +Checks that rigid object implementations are consistent across backends and provide the exact same data as the base +rigid object class advertises. All rigid object implementations need to comply with the same shared contract. The setup is a bit convoluted so that we can run these tests without requiring Isaac Sim or GPU simulation. """ @@ -17,10 +17,8 @@ import pytest import torch import warp as wp -from _rigid_object_iface_test_utils import BACKENDS, get_rigid_object - -pytestmark = pytest.mark.integration - +from ._rigid_object_contract_utils import BACKENDS, get_rigid_object +from .capabilities import contract_backend, require_backend_capability @pytest.fixture def rigid_object_iface(request): @@ -45,16 +43,12 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name # Common parametrize decorators -_backends = pytest.mark.parametrize("backend", BACKENDS, indirect=False) -_default_dims = pytest.mark.parametrize("num_instances", [1, 2, 100]) +_backends = contract_backend("api") +_default_dims = pytest.mark.parametrize("num_instances", [2]) -_default_devices = pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -_index_resolution_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton") if backend in BACKENDS], indirect=False -) -_production_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False -) +_default_devices = pytest.mark.parametrize("device", ["cpu"]) +_index_resolution_backends = contract_backend("index_resolution", names=("physx", "newton")) +_production_backends = contract_backend("api") # --------------------------------------------------------------------------- @@ -172,6 +166,8 @@ def test_find_bodies_returns_legacy_list_or_cached_proxy(self, backend): # Tests: RigidObjectData root state properties # --------------------------------------------------------------------------- +_backends = contract_backend("data") + class TestRigidObjectDataRootState: """Test data properties for root rigid body state.""" @@ -735,6 +731,28 @@ def _make_item_mask(total: int, selected: list[int], device: str) -> wp.array: return wp.array(mask_np, dtype=wp.bool, device=device) +def _read_rigid_root_pose(backend: str, raw_backend) -> np.ndarray: + """Read rigid-object root poses from backend storage.""" + if backend == "physx": + return raw_backend.get_transforms().numpy() + if backend == "newton": + return raw_backend.get_root_transforms(None).numpy().reshape(2, -1, 7)[:, 0] + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.RIGID_BODY_POSE]._data.copy() + + +def _read_rigid_masses(backend: str, raw_backend) -> np.ndarray: + """Read rigid-object masses from backend storage.""" + if backend == "physx": + return raw_backend.get_masses().numpy().reshape(2, 1) + if backend == "newton": + return raw_backend.get_attribute("body_mass", None).numpy().reshape(2, -1)[:, :1] + from isaaclab_ov import tensor_types as TT + + return raw_backend.bindings[TT.RIGID_BODY_MASS]._data.reshape(2, 1).copy() + + # --------------------------------------------------------------------------- # Tests: Root writers — torch/warp × index/mask × all/subset × negative # --------------------------------------------------------------------------- @@ -743,6 +761,9 @@ def _make_item_mask(total: int, selected: list[int], device: str) -> wp.array: _ROOT_VEL_METHODS = ["root_velocity", "root_link_velocity", "root_com_velocity"] +_production_backends = contract_backend("writes") + + class TestRigidObjectCacheInvalidation: @_production_backends def test_pose_write_invalidates_pose_dependent_caches(self, backend): @@ -823,9 +844,103 @@ def set_coms() -> None: _assert_buffers_stale(obj.data, buffers) +_backends = contract_backend("writes") + + class TestRigidObjectWritersRoot: """Test root pose/velocity writers with all input combinations.""" + @_backends + def test_root_pose_index_forwards_selected_literal(self, backend): + obj, raw_backend = get_rigid_object(backend, num_instances=2, device="cpu") + if backend == "physx": + def read_backend(): + return raw_backend.get_transforms().numpy() + elif backend == "newton": + def read_backend(): + return raw_backend.get_root_transforms(None).numpy().reshape(2, -1, 7)[:, 0] + else: + from isaaclab_ov import tensor_types as TT + + def read_backend(): + return raw_backend.bindings[TT.RIGID_BODY_POSE]._data.copy() + before = torch.from_numpy(read_backend()).clone() + root_pose = torch.tensor([[11.0, 12.0, 13.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32) + + obj.write_root_link_pose_to_sim_index(root_pose=root_pose, env_ids=torch.tensor([0], dtype=torch.int32)) + after = torch.from_numpy(read_backend()) + + torch.testing.assert_close(after[0], root_pose[0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1], before[1], rtol=0.0, atol=0.0) + if backend == "physx": + method_name, forwarded, indices = raw_backend._contract_write_calls[-1] + assert method_name == "set_transforms" + np.testing.assert_array_equal(indices, np.array([0], dtype=np.int64)) + np.testing.assert_array_equal(forwarded[0], root_pose.numpy()[0]) + np.testing.assert_array_equal(forwarded[1], before.numpy()[1]) + + @_backends + def test_root_velocity_index_forwards_selected_literal(self, backend): + obj, raw_backend = get_rigid_object(backend, num_instances=2, device="cpu") + if backend == "physx": + def read_backend(): + return raw_backend.get_velocities().numpy() + elif backend == "newton": + def read_backend(): + return raw_backend.get_root_velocities(None).numpy().reshape(2, -1, 6)[:, 0] + else: + from isaaclab_ov import tensor_types as TT + + def read_backend(): + return raw_backend.bindings[TT.RIGID_BODY_VELOCITY]._data.copy() + before = torch.from_numpy(read_backend()).clone() + root_velocity = torch.tensor([[21.0, 22.0, 23.0, 24.0, 25.0, 26.0]], dtype=torch.float32) + + obj.write_root_com_velocity_to_sim_index( + root_velocity=root_velocity, env_ids=torch.tensor([0], dtype=torch.int32) + ) + after = torch.from_numpy(read_backend()) + + torch.testing.assert_close(after[0], root_velocity[0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1], before[1], rtol=0.0, atol=0.0) + if backend == "physx": + method_name, forwarded, indices = raw_backend._contract_write_calls[-1] + assert method_name == "set_velocities" + np.testing.assert_array_equal(indices, np.array([0], dtype=np.int64)) + np.testing.assert_array_equal(forwarded[0], root_velocity.numpy()[0]) + np.testing.assert_array_equal(forwarded[1], before.numpy()[1]) + + @_backends + def test_root_pose_mask_preserves_unselected_backend_row(self, backend): + obj, raw_backend = get_rigid_object(backend, num_instances=2, device="cpu") + before = torch.from_numpy(_read_rigid_root_pose(backend, raw_backend)).clone() + root_pose = torch.tensor( + [[31.0, 32.0, 33.0, 0.0, 0.0, 0.0, 1.0], [41.0, 42.0, 43.0, 0.0, 0.0, 0.0, 1.0]], + dtype=torch.float32, + ) + + obj.write_root_link_pose_to_sim_mask(root_pose=root_pose, env_mask=_make_env_mask(2, "cpu", True)) + after = torch.from_numpy(_read_rigid_root_pose(backend, raw_backend)) + + torch.testing.assert_close(after[0], root_pose[0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1], before[1], rtol=0.0, atol=0.0) + + @_backends + def test_mass_mask_preserves_unselected_backend_row(self, backend): + obj, raw_backend = get_rigid_object(backend, num_instances=2, device="cpu") + before = torch.from_numpy(_read_rigid_masses(backend, raw_backend)).clone() + masses = torch.tensor([[51.0], [61.0]], dtype=torch.float32) + + obj.set_masses_mask( + masses=masses, + env_mask=_make_env_mask(2, "cpu", True), + body_mask=_make_item_mask(1, [0], "cpu"), + ) + after = torch.from_numpy(_read_rigid_masses(backend, raw_backend)) + + torch.testing.assert_close(after[0], masses[0], rtol=0.0, atol=0.0) + torch.testing.assert_close(after[1], before[1], rtol=0.0, atol=0.0) + # -- index variants -- @_backends @@ -965,8 +1080,8 @@ class TestRigidObjectWritersBody: def test_body_writer_index( self, backend, num_instances, device, rigid_object_iface, method_base, kwarg, wp_dtype, trailing ): - if backend == "newton" and method_base == "set_coms": - pytest.xfail("Newton set_coms expects vec3f (position only), not transformf (pose)") + if method_base == "set_coms": + require_backend_capability(backend, "com_orientation_write") obj, _ = rigid_object_iface obj.data.update(dt=0.01) num_bodies = 1 @@ -1033,8 +1148,8 @@ def _make_warp(n_envs, n_bods): def test_body_writer_mask( self, backend, num_instances, device, rigid_object_iface, method_base, kwarg, wp_dtype, trailing ): - if backend == "newton" and method_base == "set_coms": - pytest.xfail("Newton set_coms expects vec3f (position only), not transformf (pose)") + if method_base == "set_coms": + require_backend_capability(backend, "com_orientation_write") obj, _ = rigid_object_iface obj.data.update(dt=0.01) num_bodies = 1 @@ -1095,6 +1210,9 @@ def _make_warp(n_envs, n_bods): # --------------------------------------------------------------------------- +_backends = contract_backend("data") + + class TestRigidObjectDataAliases: """Test that alias properties return the same shape/dtype as their canonical counterparts.""" diff --git a/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py similarity index 80% rename from source/isaaclab/test/assets/_rigid_object_iface_test_utils.py rename to source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py index 94b198da64b7..546102d1fbd8 100644 --- a/source/isaaclab/test/assets/_rigid_object_iface_test_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py @@ -6,11 +6,12 @@ # ignore private usage of variables warning # pyright: reportPrivateUsage=none -"""Shared mocked rigid-object backend factories for interface tests.""" +"""Shared mocked rigid-object backend factories for contract tests.""" from unittest.mock import MagicMock -from _iface_test_boot import simulation_app +from ._contract_boot import simulation_app +from .capabilities import available_backends import numpy as np import warp as wp @@ -18,42 +19,60 @@ from isaaclab.assets.rigid_object.rigid_object_cfg import RigidObjectCfg from isaaclab.utils.wrench_composer import WrenchComposer -BACKENDS: list[str] = [] +from ._manager_patch_scope import patch_contract_manager -try: +BACKENDS = available_backends("api") + +if "physx" in BACKENDS: from isaaclab_physx.assets.rigid_object.rigid_object import RigidObject as PhysXRigidObject from isaaclab_physx.assets.rigid_object.rigid_object_data import RigidObjectData as PhysXRigidObjectData from isaaclab_physx.physics import PhysxManager as SimulationManager from isaaclab_physx.test.fixtures.views import MockRigidBodyViewWarp as PhysXMockRigidBodyViewWarp -except ImportError: - pass -else: - # PhysX data classes need gravity even though interface tests do not create a physics scene. - _mock_physics_sim_view = MagicMock() - _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) - SimulationManager.get_physics_sim_view = MagicMock(return_value=_mock_physics_sim_view) - - BACKENDS.append("physx") -try: +if "newton" in BACKENDS: from isaaclab_newton.assets.rigid_object.rigid_object import RigidObject as NewtonRigidObject from isaaclab_newton.assets.rigid_object.rigid_object_data import RigidObjectData as NewtonRigidObjectData from isaaclab_newton.test.fixtures.views import MockNewtonArticulationView as NewtonMockArticulationView -except ImportError: - pass -else: - BACKENDS.append("newton") -try: +if "ovphysx" in BACKENDS: import ovphysx # noqa: F401 from isaaclab_ov.assets.rigid_object.rigid_object import RigidObject as OvPhysxRigidObject from isaaclab_ov.assets.rigid_object.rigid_object_data import RigidObjectData as OvPhysxRigidObjectData from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet -except ImportError: - pass -else: - BACKENDS.append("ovphysx") + + +def _install_physx_recording_setters(mock_view) -> None: + """Install contract-local PhysX setters that record and apply staged rows.""" + storage_by_method = { + "set_transforms": "_transforms", + "set_velocities": "_velocities", + "set_masses": "_masses", + "set_coms": "_coms", + "set_inertias": "_inertias", + } + mock_view._contract_write_calls = [] + + def make_setter(method_name: str, storage_name: str): + def setter(values: wp.array, indices: wp.array | None = None) -> None: + values_np = values.numpy().copy() + indices_np = None if indices is None else indices.numpy().astype(np.int64, copy=True) + mock_view._contract_write_calls.append((method_name, values_np.copy(), indices_np)) + stored_np = getattr(mock_view, storage_name).numpy() + if indices_np is None: + stored_np[...] = values_np.reshape(stored_np.shape) + else: + if values_np.size == stored_np.size: + normalized = values_np.reshape(stored_np.shape) + else: + normalized = values_np.reshape(len(indices_np), *stored_np.shape[1:]) + staged_rows = normalized[indices_np] if normalized.shape[0] == stored_np.shape[0] else normalized + stored_np[indices_np] = staged_rows + + return setter + + for method_name, storage_name in storage_by_method.items(): + setattr(mock_view, method_name, make_setter(method_name, storage_name)) def create_physx_rigid_object( @@ -61,6 +80,11 @@ def create_physx_rigid_object( device: str = "cuda:0", ): """Create a test RigidObject instance with mocked dependencies.""" + mock_physics_sim_view = MagicMock() + mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) + patch_contract_manager( + SimulationManager, "get_physics_sim_view", MagicMock(return_value=mock_physics_sim_view) + ) body_names = ["body_0"] rigid_object = object.__new__(PhysXRigidObject) @@ -73,7 +97,8 @@ def create_physx_rigid_object( device=device, ) mock_view.set_random_mock_data() - mock_view._noop_setters = True + mock_view._noop_setters = False + _install_physx_recording_setters(mock_view) object.__setattr__(rigid_object, "_root_view", mock_view) object.__setattr__(rigid_object, "_device", device) @@ -115,7 +140,9 @@ def create_physx_rigid_object( object.__setattr__(rigid_object, "_sim_env_ids_views", {}) cpu_env_ids = wp.array(np.arange(N, dtype=np.int32), device="cpu") object.__setattr__(rigid_object, "_cpu_env_ids_all", cpu_env_ids) - object.__setattr__(rigid_object, "_cpu_env_ids", wp.empty(N, dtype=wp.int32, device="cpu", pinned=True)) + object.__setattr__( + rigid_object, "_cpu_env_ids", wp.empty(N, dtype=wp.int32, device="cpu", pinned=wp.is_cuda_available()) + ) object.__setattr__(rigid_object, "_cpu_env_ids_views", {}) object.__setattr__(rigid_object, "_cpu_body_mass", wp.zeros((N, B), dtype=wp.float32, device="cpu")) object.__setattr__(rigid_object, "_cpu_body_coms", wp.zeros((N, B, 7), dtype=wp.float32, device="cpu")) @@ -144,7 +171,7 @@ def create_newton_rigid_object( body_names=body_names, ) mock_view.set_random_mock_data() - mock_view._noop_setters = True + mock_view._noop_setters = False # Mock NewtonManager (aliased as SimulationManager in Newton modules) mock_model = MagicMock() @@ -164,13 +191,8 @@ def create_newton_rigid_object( mock_manager.get_control.return_value = mock_control # Patch SimulationManager in the Newton data module - original_sim_manager = newton_data_module.SimulationManager - newton_data_module.SimulationManager = mock_manager - - try: - data = NewtonRigidObjectData(mock_view, device) - finally: - newton_data_module.SimulationManager = original_sim_manager + patch_contract_manager(newton_data_module, "SimulationManager", mock_manager) + data = NewtonRigidObjectData(mock_view, device) # Create RigidObject shell (bypass __init__) rigid_object = object.__new__(NewtonRigidObject) diff --git a/source/isaaclab/test/assets/contract/capabilities.py b/source/isaaclab/test/assets/contract/capabilities.py new file mode 100644 index 000000000000..60cad30b9320 --- /dev/null +++ b/source/isaaclab/test/assets/contract/capabilities.py @@ -0,0 +1,174 @@ +# 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 + +"""Explicit backend capability declarations for shared asset contracts.""" + +from collections.abc import Callable +from dataclasses import dataclass, field +from importlib.util import find_spec + +import pytest +import warp as wp +from _pytest.mark.structures import ParameterSet + + +@dataclass(frozen=True) +class BackendDeclaration: + """Declare dependencies and supported shared-contract families for a backend.""" + + name: str + required_modules: tuple[str, ...] + capabilities: frozenset[str] + unsupported: dict[str, str] = field(default_factory=dict) + requires_cuda_runtime: bool = False + + +@dataclass(frozen=True) +class BackendStatus: + """Describe whether one declared backend can run shared contracts.""" + + declaration: BackendDeclaration + available: bool + reason: str | None + + +_SHARED_CAPABILITIES = frozenset({"api", "data", "writes", "ordering", "fixed_tendons", "cuda"}) +_EXTENDED_ARTICULATION_CAPABILITIES = frozenset( + { + "com_orientation_write", + "fixed_tendon_extended_data", + "fixed_tendon_extended_write_index", + "fixed_tendon_write_mask", + "fixed_tendon_write_to_sim_index", + "fixed_tendon_write_to_sim_mask", + } +) + +BACKEND_DECLARATIONS = ( + BackendDeclaration( + name="physx", + required_modules=("carb", "isaaclab_physx"), + capabilities=_SHARED_CAPABILITIES + | _EXTENDED_ARTICULATION_CAPABILITIES + | {"index_resolution", "spatial_tendons"}, + ), + BackendDeclaration( + name="newton", + required_modules=("isaaclab_newton",), + capabilities=_SHARED_CAPABILITIES | {"index_resolution"}, + unsupported={ + "com_orientation_write": "Newton stores center-of-mass position but not orientation", + "fixed_tendon_extended_data": "Newton does not expose extended fixed-tendon data", + "fixed_tendon_extended_write_index": "Newton does not implement extended fixed-tendon index writers", + "fixed_tendon_write_mask": "Newton does not implement fixed-tendon mask writers", + "fixed_tendon_write_to_sim_index": ( + "Newton fixed-tendon write-to-sim does not resolve a default environment selector" + ), + "fixed_tendon_write_to_sim_mask": "Newton does not implement fixed-tendon mask write-to-sim", + "spatial_tendons": "Newton does not support spatial tendons", + }, + ), + BackendDeclaration( + name="ovphysx", + required_modules=("ovphysx", "isaaclab_ov"), + capabilities=_SHARED_CAPABILITIES | _EXTENDED_ARTICULATION_CAPABILITIES | {"spatial_tendons"}, + unsupported={"index_resolution": "OVPhysX does not expose the shared index-resolution helpers"}, + # The mock contract allocates pinned host staging buffers even for CPU tensors. + requires_cuda_runtime=True, + ), +) + + +def evaluate_backend( + declaration: BackendDeclaration, + module_available: Callable[[str], bool], + cuda_available: bool, +) -> BackendStatus: + """Evaluate one backend declaration against the current process.""" + for module_name in declaration.required_modules: + if not module_available(module_name): + return BackendStatus(declaration, available=False, reason=f"missing required module: {module_name}") + if declaration.requires_cuda_runtime and not cuda_available: + return BackendStatus(declaration, available=False, reason="CUDA runtime unavailable") + return BackendStatus(declaration, available=True, reason=None) + + +def backend_parameters( + capability: str, + *, + statuses: tuple[BackendStatus, ...] | None = None, + names: tuple[str, ...] | None = None, +) -> list[ParameterSet]: + """Return explicit pytest parameters for one backend capability.""" + use_runtime_statuses = statuses is None + if statuses is None: + statuses = BACKEND_STATUSES + if names is not None: + statuses = tuple(status for status in statuses if status.declaration.name in names) + + parameters = [] + for status in statuses: + declaration = status.declaration + reason = status.reason + if reason is None and capability == "cuda" and use_runtime_statuses and not _CUDA_AVAILABLE: + reason = "CUDA runtime unavailable" + if reason is None and capability not in declaration.capabilities: + reason = declaration.unsupported.get(capability) + if reason is None: + raise ValueError(f"{declaration.name} has no declaration for capability {capability!r}") + marks = () if reason is None else pytest.mark.skip(reason=reason) + parameters.append(pytest.param(declaration.name, marks=marks, id=declaration.name)) + return parameters + + +def contract_backend(capability: str, *, names: tuple[str, ...] | None = None) -> Callable: + """Parametrize a contract test with its explicit backend capability.""" + parametrize = pytest.mark.parametrize("backend", backend_parameters(capability, names=names), indirect=False) + + def decorator(test_function: Callable) -> Callable: + decorated = parametrize(test_function) + decorated.contract_backend_capability = capability + return decorated + + return decorator + + +def available_backends(capability: str) -> list[str]: + """Return available backends that explicitly support a capability.""" + return [ + status.declaration.name + for status in BACKEND_STATUSES + if status.available and capability in status.declaration.capabilities + ] + + +def require_backend_capability(backend: str, capability: str) -> None: + """Skip the current case with the declared reason when a backend lacks a capability.""" + declaration = next(declaration for declaration in BACKEND_DECLARATIONS if declaration.name == backend) + if capability in declaration.capabilities: + return + reason = declaration.unsupported.get(capability) + if reason is None: + raise ValueError(f"{backend} has no declaration for capability {capability!r}") + pytest.skip(reason) + + +def backend_unavailable_reasons() -> dict[str, str]: + """Return explicit reasons for backends unavailable to this contract process.""" + return {status.declaration.name: status.reason for status in BACKEND_STATUSES if status.reason is not None} + + +def _module_available(module_name: str) -> bool: + """Return whether an explicitly declared optional module can be resolved.""" + try: + return find_spec(module_name) is not None + except ModuleNotFoundError: + return False + + +_CUDA_AVAILABLE = wp.is_cuda_available() +BACKEND_STATUSES = tuple( + evaluate_backend(declaration, _module_available, _CUDA_AVAILABLE) for declaration in BACKEND_DECLARATIONS +) diff --git a/source/isaaclab/test/assets/contract/conftest.py b/source/isaaclab/test/assets/contract/conftest.py new file mode 100644 index 000000000000..b8dc619426b1 --- /dev/null +++ b/source/isaaclab/test/assets/contract/conftest.py @@ -0,0 +1,19 @@ +# 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 + +"""Fixtures shared by the focused asset contract gate.""" + +from collections.abc import Iterator + +import pytest + +from ._manager_patch_scope import contract_manager_patch_scope + + +@pytest.fixture(autouse=True) +def _restore_contract_manager_patches() -> Iterator[None]: + """Limit factory manager substitutions to one test invocation.""" + with contract_manager_patch_scope(): + yield diff --git a/source/isaaclab/test/assets/contract/public_surface.py b/source/isaaclab/test/assets/contract/public_surface.py new file mode 100644 index 000000000000..7e92a03ce6e1 --- /dev/null +++ b/source/isaaclab/test/assets/contract/public_surface.py @@ -0,0 +1,394 @@ +# 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 + +"""Public base-surface classification for shared asset contracts.""" + +from dataclasses import dataclass +from enum import StrEnum + +from isaaclab.assets.articulation.base_articulation import BaseArticulation +from isaaclab.assets.articulation.base_articulation_data import BaseArticulationData +from isaaclab.assets.asset_base import AssetBase +from isaaclab.assets.rigid_object.base_rigid_object import BaseRigidObject +from isaaclab.assets.rigid_object.base_rigid_object_data import BaseRigidObjectData +from isaaclab.assets.rigid_object_collection.base_rigid_object_collection import BaseRigidObjectCollection +from isaaclab.assets.rigid_object_collection.base_rigid_object_collection_data import BaseRigidObjectCollectionData + + +class ContractKind(StrEnum): + """Supported classifications for a declared public asset member.""" + + API = "api" + DATA = "data" + WRITE = "write" + UNSUPPORTED = "unsupported" + OUT_OF_SCOPE = "out_of_scope" + + +BASE_SURFACE_CLASSES = ( + AssetBase, + BaseRigidObject, + BaseRigidObjectData, + BaseRigidObjectCollection, + BaseRigidObjectCollectionData, + BaseArticulation, + BaseArticulationData, +) + + +_PUBLIC_MEMBER_SNAPSHOT = { + "AssetBase": """ + assert_shape_and_dtype assert_shape_and_dtype_mask data device has_debug_vis_implementation is_initialized + num_instances reset set_debug_vis set_visibility update write_data_to_sim + """, + "BaseRigidObject": """ + body_names data find_bodies instantaneous_wrench_composer num_bodies num_instances permanent_wrench_composer + reset root_view set_coms set_coms_index set_coms_mask set_external_force_and_torque set_inertias + set_inertias_index set_inertias_mask set_masses set_masses_index set_masses_mask update write_data_to_sim + write_root_com_pose_to_sim write_root_com_pose_to_sim_index write_root_com_pose_to_sim_mask + write_root_com_state_to_sim write_root_com_velocity_to_sim write_root_com_velocity_to_sim_index + write_root_com_velocity_to_sim_mask write_root_link_pose_to_sim write_root_link_pose_to_sim_index + write_root_link_pose_to_sim_mask write_root_link_state_to_sim write_root_link_velocity_to_sim + write_root_link_velocity_to_sim_index write_root_link_velocity_to_sim_mask write_root_pose_to_sim + write_root_pose_to_sim_index write_root_pose_to_sim_mask write_root_state_to_sim write_root_velocity_to_sim + write_root_velocity_to_sim_index write_root_velocity_to_sim_mask + """, + "BaseRigidObjectData": """ + body_acc_w body_ang_acc_w body_ang_vel_w body_com_acc_w body_com_ang_acc_w body_com_ang_vel_w + body_com_lin_acc_w body_com_lin_vel_w body_com_pos_b body_com_pos_w body_com_pose_b body_com_pose_w + body_com_quat_b body_com_quat_w body_com_state_w body_com_vel_w body_inertia body_lin_acc_w body_lin_vel_w + body_link_ang_vel_w body_link_lin_vel_w body_link_pos_w body_link_pose_w body_link_quat_w body_link_state_w + body_link_vel_w body_mass body_names body_pos_w body_pose_w body_quat_w body_state_w body_vel_w com_pos_b + com_quat_b default_inertia default_mass default_root_pose default_root_state default_root_vel heading_w + projected_gravity_b root_ang_vel_b root_ang_vel_w root_com_ang_vel_b root_com_ang_vel_w root_com_lin_vel_b + root_com_lin_vel_w root_com_pos_w root_com_pose_w root_com_quat_w root_com_state_w root_com_vel_w + root_lin_vel_b root_lin_vel_w root_link_ang_vel_b root_link_ang_vel_w root_link_lin_vel_b root_link_lin_vel_w + root_link_pos_w root_link_pose_w root_link_quat_w root_link_state_w root_link_vel_w root_pos_w root_pose_w + root_quat_w root_state_w root_vel_w update + """, + "BaseRigidObjectCollection": """ + body_names data find_bodies find_objects instantaneous_wrench_composer num_bodies num_instances num_objects + object_names permanent_wrench_composer reset root_view set_coms set_coms_index set_coms_mask + set_external_force_and_torque set_inertias set_inertias_index set_inertias_mask set_masses set_masses_index + set_masses_mask update write_body_com_pose_to_sim write_body_com_pose_to_sim_index + write_body_com_pose_to_sim_mask write_body_com_state_to_sim write_body_com_velocity_to_sim + write_body_com_velocity_to_sim_index write_body_com_velocity_to_sim_mask write_body_link_pose_to_sim + write_body_link_pose_to_sim_index write_body_link_pose_to_sim_mask write_body_link_state_to_sim + write_body_link_velocity_to_sim write_body_link_velocity_to_sim_index write_body_link_velocity_to_sim_mask + write_body_pose_to_sim write_body_pose_to_sim_index write_body_pose_to_sim_mask write_body_state_to_sim + write_body_velocity_to_sim write_body_velocity_to_sim_index write_body_velocity_to_sim_mask write_data_to_sim + write_object_com_pose_to_sim write_object_com_state_to_sim write_object_com_velocity_to_sim + write_object_link_pose_to_sim write_object_link_state_to_sim write_object_link_velocity_to_sim + write_object_pose_to_sim write_object_state_to_sim write_object_velocity_to_sim + """, + "BaseRigidObjectCollectionData": """ + body_acc_w body_ang_acc_w body_ang_vel_w body_com_acc_w body_com_ang_acc_w body_com_ang_vel_b + body_com_ang_vel_w body_com_lin_acc_w body_com_lin_vel_b body_com_lin_vel_w body_com_pos_b body_com_pos_w + body_com_pose_b body_com_pose_w body_com_quat_b body_com_quat_w body_com_state_w body_com_vel_w body_inertia + body_lin_acc_w body_lin_vel_w body_link_ang_vel_b body_link_ang_vel_w body_link_lin_vel_b body_link_lin_vel_w + body_link_pos_w body_link_pose_w body_link_quat_w body_link_state_w body_link_vel_w body_mass body_names + body_pos_w body_pose_w body_quat_w body_state_w body_vel_w com_pos_b com_quat_b default_body_pose + default_body_state default_body_vel default_inertia default_mass default_object_pose default_object_state + default_object_vel heading_w object_acc_w object_ang_acc_w object_ang_vel_b object_ang_vel_w object_com_acc_w + object_com_ang_acc_w object_com_ang_vel_b object_com_ang_vel_w object_com_lin_acc_w object_com_lin_vel_b + object_com_lin_vel_w object_com_pos_b object_com_pos_w object_com_pose_b object_com_pose_w object_com_quat_b + object_com_quat_w object_com_state_w object_com_vel_w object_lin_acc_w object_lin_vel_b object_lin_vel_w + object_link_ang_vel_b object_link_ang_vel_w object_link_lin_vel_b object_link_lin_vel_w object_link_pos_w + object_link_pose_w object_link_quat_w object_link_state_w object_link_vel_w object_pos_w object_pose_w + object_quat_w object_state_w object_vel_w projected_gravity_b update + """, + "BaseArticulation": """ + backend_body_names backend_joint_names body_names body_ordering data find_bodies find_fixed_tendons find_joints + find_spatial_tendons fixed_tendon_names instantaneous_wrench_composer is_fixed_base joint_names joint_ordering + map_body_ids_to_backend map_joint_ids_to_backend num_base_dofs num_bodies num_fixed_tendons num_instances + num_joints num_spatial_tendons permanent_wrench_composer reset root_view set_coms set_coms_index set_coms_mask + set_external_force_and_torque set_fixed_tendon_damping set_fixed_tendon_damping_index + set_fixed_tendon_damping_mask set_fixed_tendon_limit set_fixed_tendon_limit_stiffness + set_fixed_tendon_limit_stiffness_index set_fixed_tendon_limit_stiffness_mask set_fixed_tendon_offset + set_fixed_tendon_offset_index set_fixed_tendon_offset_mask set_fixed_tendon_position_limit + set_fixed_tendon_position_limit_index set_fixed_tendon_position_limit_mask set_fixed_tendon_rest_length + set_fixed_tendon_rest_length_index set_fixed_tendon_rest_length_mask set_fixed_tendon_stiffness + set_fixed_tendon_stiffness_index set_fixed_tendon_stiffness_mask set_inertias set_inertias_index + set_inertias_mask set_joint_effort_target set_joint_effort_target_index set_joint_effort_target_mask + set_joint_position_target set_joint_position_target_index set_joint_position_target_mask + set_joint_velocity_target set_joint_velocity_target_index set_joint_velocity_target_mask set_masses + set_masses_index set_masses_mask set_spatial_tendon_damping set_spatial_tendon_damping_index + set_spatial_tendon_damping_mask set_spatial_tendon_limit_stiffness + set_spatial_tendon_limit_stiffness_index set_spatial_tendon_limit_stiffness_mask set_spatial_tendon_offset + set_spatial_tendon_offset_index set_spatial_tendon_offset_mask set_spatial_tendon_stiffness + set_spatial_tendon_stiffness_index set_spatial_tendon_stiffness_mask spatial_tendon_names update + write_data_to_sim write_fixed_tendon_properties_to_sim write_fixed_tendon_properties_to_sim_index + write_fixed_tendon_properties_to_sim_mask write_joint_armature_to_sim write_joint_armature_to_sim_index + write_joint_armature_to_sim_mask write_joint_damping_to_sim write_joint_damping_to_sim_index + write_joint_damping_to_sim_mask write_joint_effort_limit_to_sim write_joint_effort_limit_to_sim_index + write_joint_effort_limit_to_sim_mask write_joint_friction_coefficient_to_sim + write_joint_friction_coefficient_to_sim_index write_joint_friction_coefficient_to_sim_mask + write_joint_friction_to_sim write_joint_limits_to_sim write_joint_position_limit_to_sim + write_joint_position_limit_to_sim_index write_joint_position_limit_to_sim_mask write_joint_position_to_sim + write_joint_position_to_sim_index write_joint_position_to_sim_mask write_joint_state_to_sim + write_joint_stiffness_to_sim write_joint_stiffness_to_sim_index write_joint_stiffness_to_sim_mask + write_joint_velocity_limit_to_sim write_joint_velocity_limit_to_sim_index write_joint_velocity_limit_to_sim_mask + write_joint_velocity_to_sim write_joint_velocity_to_sim_index write_joint_velocity_to_sim_mask + write_root_com_pose_to_sim write_root_com_pose_to_sim_index write_root_com_pose_to_sim_mask + write_root_com_state_to_sim write_root_com_velocity_to_sim write_root_com_velocity_to_sim_index + write_root_com_velocity_to_sim_mask write_root_link_pose_to_sim write_root_link_pose_to_sim_index + write_root_link_pose_to_sim_mask write_root_link_state_to_sim write_root_link_velocity_to_sim + write_root_link_velocity_to_sim_index write_root_link_velocity_to_sim_mask write_root_pose_to_sim + write_root_pose_to_sim_index write_root_pose_to_sim_mask write_root_state_to_sim write_root_velocity_to_sim + write_root_velocity_to_sim_index write_root_velocity_to_sim_mask write_spatial_tendon_properties_to_sim + write_spatial_tendon_properties_to_sim_index write_spatial_tendon_properties_to_sim_mask + """, + "BaseArticulationData": """ + applied_torque bind_actuator_collection body_acc_w body_ang_acc_w body_ang_vel_w body_com_acc_w + body_com_ang_acc_w body_com_ang_vel_w body_com_jacobian_w body_com_lin_acc_w body_com_lin_vel_w body_com_pos_b + body_com_pos_w body_com_pose_b body_com_pose_w body_com_quat_b body_com_quat_w body_com_state_w body_com_vel_w + body_inertia body_lin_acc_w body_lin_vel_w body_link_ang_vel_w body_link_jacobian_w body_link_lin_vel_w + body_link_pos_w body_link_pose_w body_link_quat_w body_link_state_w body_link_vel_w body_mass body_names + body_ordering body_pos_w body_pose_w body_quat_w body_state_w body_vel_w com_pos_b com_quat_b computed_torque + default_fixed_tendon_damping default_fixed_tendon_limit default_fixed_tendon_limit_stiffness + default_fixed_tendon_offset default_fixed_tendon_pos_limits default_fixed_tendon_rest_length + default_fixed_tendon_stiffness default_inertia default_joint_armature default_joint_damping + default_joint_friction default_joint_friction_coeff default_joint_limits default_joint_pos + default_joint_pos_limits default_joint_stiffness default_joint_vel default_joint_viscous_friction_coeff + default_mass default_root_pose default_root_state default_root_vel default_spatial_tendon_damping + default_spatial_tendon_limit_stiffness default_spatial_tendon_offset default_spatial_tendon_stiffness + fixed_tendon_damping fixed_tendon_limit fixed_tendon_limit_stiffness fixed_tendon_names fixed_tendon_offset + fixed_tendon_pos_limits fixed_tendon_rest_length fixed_tendon_stiffness gravity_compensation_forces + has_body_ordering has_joint_ordering heading_w joint_acc joint_armature joint_damping joint_effort_limits + joint_effort_target joint_friction joint_friction_coeff joint_limits joint_names joint_ordering joint_pos + joint_pos_limits joint_pos_target joint_stiffness joint_vel joint_vel_limits joint_vel_target + joint_velocity_limits mass_matrix projected_gravity_b root_ang_vel_b root_ang_vel_w root_com_ang_vel_b + root_com_ang_vel_w root_com_lin_vel_b root_com_lin_vel_w root_com_pos_w root_com_pose_w root_com_quat_w + root_com_state_w root_com_vel_w root_lin_vel_b root_lin_vel_w root_link_ang_vel_b root_link_ang_vel_w + root_link_lin_vel_b root_link_lin_vel_w root_link_pos_w root_link_pose_w root_link_quat_w root_link_state_w + root_link_vel_w root_pos_w root_pose_w root_quat_w root_state_w root_vel_w soft_joint_pos_limits + soft_joint_vel_limits spatial_tendon_damping spatial_tendon_limit_stiffness spatial_tendon_names + spatial_tendon_offset spatial_tendon_stiffness update + """, +} + + +@dataclass(frozen=True) +class PublicMemberContract: + """Classify one public member or record why it is excluded.""" + + kind: ContractKind + reason: str | None = None + + +@dataclass(frozen=True) +class PublicSurfaceAudit: + """Describe mismatches between declared members and their explicit mappings.""" + + missing: frozenset[str] + stale: frozenset[str] + unreasoned: frozenset[str] + + @property + def is_valid(self) -> bool: + """Return whether the public inventory and mappings agree exactly.""" + return not (self.missing or self.stale or self.unreasoned) + + def format_errors(self) -> str: + """Format all mapping failures for one actionable pytest diagnostic.""" + return "; ".join( + f"{label}: {sorted(values)}" + for label, values in ( + ("missing", self.missing), + ("stale", self.stale), + ("unreasoned exclusions", self.unreasoned), + ) + if values + ) + + +def _mapping( + class_name: str, + members: str, + kind: ContractKind, + *, + reason: str | None = None, +) -> dict[str, PublicMemberContract]: + """Build explicit qualified mappings for one reviewed member group.""" + return { + f"{class_name}.{member_name}": PublicMemberContract(kind=kind, reason=reason) for member_name in members.split() + } + + +PUBLIC_SURFACE_CONTRACTS = { + **_mapping( + "AssetBase", + "data device is_initialized num_instances", + ContractKind.API, + ), + **_mapping( + "AssetBase", + "assert_shape_and_dtype assert_shape_and_dtype_mask reset update write_data_to_sim", + ContractKind.WRITE, + ), + **_mapping( + "AssetBase", + "has_debug_vis_implementation set_debug_vis set_visibility", + ContractKind.OUT_OF_SCOPE, + reason="debug visualization and stage visibility require a live USD/Kit application", + ), + **_mapping( + "BaseRigidObject", + """ + body_names data find_bodies instantaneous_wrench_composer num_bodies num_instances + permanent_wrench_composer root_view + """, + ContractKind.API, + ), + **_mapping( + "BaseRigidObject", + """ + reset set_coms set_coms_index set_coms_mask set_external_force_and_torque set_inertias + set_inertias_index set_inertias_mask set_masses set_masses_index set_masses_mask update + write_data_to_sim write_root_com_pose_to_sim write_root_com_pose_to_sim_index + write_root_com_pose_to_sim_mask write_root_com_state_to_sim write_root_com_velocity_to_sim + write_root_com_velocity_to_sim_index write_root_com_velocity_to_sim_mask write_root_link_pose_to_sim + write_root_link_pose_to_sim_index write_root_link_pose_to_sim_mask write_root_link_state_to_sim + write_root_link_velocity_to_sim write_root_link_velocity_to_sim_index + write_root_link_velocity_to_sim_mask write_root_pose_to_sim write_root_pose_to_sim_index + write_root_pose_to_sim_mask write_root_state_to_sim write_root_velocity_to_sim + write_root_velocity_to_sim_index write_root_velocity_to_sim_mask + """, + ContractKind.WRITE, + ), + **_mapping( + "BaseRigidObjectData", + _PUBLIC_MEMBER_SNAPSHOT["BaseRigidObjectData"], + ContractKind.DATA, + ), + **_mapping( + "BaseRigidObjectCollection", + """ + body_names data find_bodies find_objects instantaneous_wrench_composer num_bodies num_instances + num_objects object_names permanent_wrench_composer root_view + """, + ContractKind.API, + ), + **_mapping( + "BaseRigidObjectCollection", + """ + reset set_coms set_coms_index set_coms_mask set_external_force_and_torque set_inertias + set_inertias_index set_inertias_mask set_masses set_masses_index set_masses_mask update + write_body_com_pose_to_sim write_body_com_pose_to_sim_index write_body_com_pose_to_sim_mask + write_body_com_state_to_sim write_body_com_velocity_to_sim write_body_com_velocity_to_sim_index + write_body_com_velocity_to_sim_mask write_body_link_pose_to_sim write_body_link_pose_to_sim_index + write_body_link_pose_to_sim_mask write_body_link_state_to_sim write_body_link_velocity_to_sim + write_body_link_velocity_to_sim_index write_body_link_velocity_to_sim_mask write_body_pose_to_sim + write_body_pose_to_sim_index write_body_pose_to_sim_mask write_body_state_to_sim + write_body_velocity_to_sim write_body_velocity_to_sim_index write_body_velocity_to_sim_mask + write_data_to_sim write_object_com_pose_to_sim write_object_com_state_to_sim + write_object_com_velocity_to_sim write_object_link_pose_to_sim write_object_link_state_to_sim + write_object_link_velocity_to_sim write_object_pose_to_sim write_object_state_to_sim + write_object_velocity_to_sim + """, + ContractKind.WRITE, + ), + **_mapping( + "BaseRigidObjectCollectionData", + _PUBLIC_MEMBER_SNAPSHOT["BaseRigidObjectCollectionData"], + ContractKind.DATA, + ), + **_mapping( + "BaseArticulation", + """ + backend_body_names backend_joint_names body_names body_ordering data find_bodies find_fixed_tendons + find_joints find_spatial_tendons fixed_tendon_names instantaneous_wrench_composer is_fixed_base + joint_names joint_ordering map_body_ids_to_backend map_joint_ids_to_backend num_base_dofs num_bodies + num_fixed_tendons num_instances num_joints num_spatial_tendons permanent_wrench_composer root_view + spatial_tendon_names + """, + ContractKind.API, + ), + **_mapping( + "BaseArticulation", + """ + reset set_coms set_coms_index set_coms_mask set_external_force_and_torque set_fixed_tendon_damping + set_fixed_tendon_damping_index set_fixed_tendon_damping_mask set_fixed_tendon_limit + set_fixed_tendon_limit_stiffness set_fixed_tendon_limit_stiffness_index + set_fixed_tendon_limit_stiffness_mask set_fixed_tendon_offset set_fixed_tendon_offset_index + set_fixed_tendon_offset_mask set_fixed_tendon_position_limit set_fixed_tendon_position_limit_index + set_fixed_tendon_position_limit_mask set_fixed_tendon_rest_length set_fixed_tendon_rest_length_index + set_fixed_tendon_rest_length_mask set_fixed_tendon_stiffness set_fixed_tendon_stiffness_index + set_fixed_tendon_stiffness_mask set_inertias set_inertias_index set_inertias_mask + set_joint_effort_target set_joint_effort_target_index set_joint_effort_target_mask + set_joint_position_target set_joint_position_target_index set_joint_position_target_mask + set_joint_velocity_target set_joint_velocity_target_index set_joint_velocity_target_mask set_masses + set_masses_index set_masses_mask set_spatial_tendon_damping set_spatial_tendon_damping_index + set_spatial_tendon_damping_mask set_spatial_tendon_limit_stiffness + set_spatial_tendon_limit_stiffness_index set_spatial_tendon_limit_stiffness_mask + set_spatial_tendon_offset set_spatial_tendon_offset_index set_spatial_tendon_offset_mask + set_spatial_tendon_stiffness set_spatial_tendon_stiffness_index set_spatial_tendon_stiffness_mask update + write_data_to_sim write_fixed_tendon_properties_to_sim write_fixed_tendon_properties_to_sim_index + write_fixed_tendon_properties_to_sim_mask write_joint_armature_to_sim write_joint_armature_to_sim_index + write_joint_armature_to_sim_mask write_joint_damping_to_sim write_joint_damping_to_sim_index + write_joint_damping_to_sim_mask write_joint_effort_limit_to_sim write_joint_effort_limit_to_sim_index + write_joint_effort_limit_to_sim_mask write_joint_friction_coefficient_to_sim + write_joint_friction_coefficient_to_sim_index write_joint_friction_coefficient_to_sim_mask + write_joint_friction_to_sim write_joint_limits_to_sim write_joint_position_limit_to_sim + write_joint_position_limit_to_sim_index write_joint_position_limit_to_sim_mask + write_joint_position_to_sim write_joint_position_to_sim_index write_joint_position_to_sim_mask + write_joint_state_to_sim write_joint_stiffness_to_sim write_joint_stiffness_to_sim_index + write_joint_stiffness_to_sim_mask write_joint_velocity_limit_to_sim + write_joint_velocity_limit_to_sim_index write_joint_velocity_limit_to_sim_mask + write_joint_velocity_to_sim write_joint_velocity_to_sim_index write_joint_velocity_to_sim_mask + write_root_com_pose_to_sim write_root_com_pose_to_sim_index write_root_com_pose_to_sim_mask + write_root_com_state_to_sim write_root_com_velocity_to_sim write_root_com_velocity_to_sim_index + write_root_com_velocity_to_sim_mask write_root_link_pose_to_sim write_root_link_pose_to_sim_index + write_root_link_pose_to_sim_mask write_root_link_state_to_sim write_root_link_velocity_to_sim + write_root_link_velocity_to_sim_index write_root_link_velocity_to_sim_mask write_root_pose_to_sim + write_root_pose_to_sim_index write_root_pose_to_sim_mask write_root_state_to_sim + write_root_velocity_to_sim write_root_velocity_to_sim_index write_root_velocity_to_sim_mask + write_spatial_tendon_properties_to_sim write_spatial_tendon_properties_to_sim_index + write_spatial_tendon_properties_to_sim_mask + """, + ContractKind.WRITE, + ), + **_mapping( + "BaseArticulationData", + _PUBLIC_MEMBER_SNAPSHOT["BaseArticulationData"], + ContractKind.DATA, + ), +} + +PUBLIC_SURFACE_CLASSIFICATIONS = { + member_name: contract.kind for member_name, contract in PUBLIC_SURFACE_CONTRACTS.items() +} + + +def audit_public_surface(classes: tuple[type, ...], mappings: dict[str, PublicMemberContract]) -> PublicSurfaceAudit: + """Compare the current declared inventory with exact, reasoned mappings.""" + declared_members = { + f"{cls.__name__}.{member_name}" + for cls in classes + for member_name in cls.__dict__ + if not member_name.startswith("_") + } + mapped_members = set(mappings) + exclusion_kinds = {ContractKind.UNSUPPORTED, ContractKind.OUT_OF_SCOPE} + return PublicSurfaceAudit( + missing=frozenset(declared_members - mapped_members), + stale=frozenset(mapped_members - declared_members), + unreasoned=frozenset( + member_name + for member_name, contract in mappings.items() + if contract.kind in exclusion_kinds and not contract.reason + ), + ) + + +def unclassified_public_members(classes: tuple[type, ...], classifications: dict[str, ContractKind]) -> set[str]: + """Return declared public members that have no contract classification.""" + declared_members = { + f"{cls.__name__}.{member_name}" + for cls in classes + for member_name in cls.__dict__ + if not member_name.startswith("_") + } + return declared_members - classifications.keys() diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_api.py b/source/isaaclab/test/assets/contract/test_asset_contract_api.py new file mode 100644 index 000000000000..23aeee449a44 --- /dev/null +++ b/source/isaaclab/test/assets/contract/test_asset_contract_api.py @@ -0,0 +1,345 @@ +# 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 + +# ruff: noqa: F401 + +"""Shared asset API contract tests.""" + +from importlib.util import find_spec + +import pytest + +pytestmark = pytest.mark.unit + +from . import public_surface +from ._articulation_contract_cases import TestArticulationDataRootState as _ArticulationDataContract +from ._articulation_contract_cases import TestArticulationDataTendonState as _ArticulationTendonDataContract +from ._articulation_contract_cases import ( # noqa: F401 + TestArticulationFinderReturnModes, + TestArticulationFinders, + TestArticulationIndexResolution, + TestArticulationProperties, + TestArticulationTendonFinders, + TestArticulationTendonProperties, + TestResolveMatchingNamesCache, + articulation_iface, +) +from ._articulation_contract_cases import TestArticulationWritersRoot as _ArticulationWriteContract +from ._articulation_ordering_contract_cases import TestArticulationOrderingAllocation as _ArticulationOrderingContract +from ._rigid_object_collection_contract_cases import ( + TestCollectionCacheInvalidation as _CollectionCacheInvalidationContract, +) +from ._rigid_object_collection_contract_cases import ( # noqa: F401 + TestCollectionFinderReturnModes, + TestCollectionFinders, + TestCollectionIndexResolution, + TestCollectionProperties, + collection_iface, +) +from ._rigid_object_contract_cases import TestRigidObjectCacheInvalidation as _RigidObjectCacheInvalidationContract +from ._rigid_object_contract_cases import ( # noqa: F401 + TestRigidObjectFinderReturnModes, + TestRigidObjectFinders, + TestRigidObjectIndexResolution, + TestRigidObjectProperties, + rigid_object_iface, +) +from .capabilities import ( + BACKEND_STATUSES, + BackendDeclaration, + BackendStatus, + backend_parameters, + evaluate_backend, +) +from .public_surface import ( + BASE_SURFACE_CLASSES, + PUBLIC_SURFACE_CLASSIFICATIONS, + ContractKind, + unclassified_public_members, +) + + +def _available_contract_manager_globals() -> dict[str, object]: + """Return installed backend bindings touched by the mock factories.""" + import inspect + + bindings: dict[str, object] = {} + if "physx" in BACKEND_STATUSES_BY_NAME and BACKEND_STATUSES_BY_NAME["physx"].available: + from isaaclab_physx.physics import PhysxManager + + bindings["physx.get_physics_sim_view"] = inspect.getattr_static(PhysxManager, "get_physics_sim_view") + if "newton" in BACKEND_STATUSES_BY_NAME and BACKEND_STATUSES_BY_NAME["newton"].available: + import isaaclab_newton.assets.articulation.articulation_data as articulation_data + import isaaclab_newton.assets.rigid_object.rigid_object_data as rigid_object_data + import isaaclab_newton.assets.rigid_object_collection.rigid_object_collection as rigid_object_collection + from isaaclab_newton.assets.rigid_object_collection import ( + rigid_object_collection_data, + ) + + bindings.update( + { + "newton.articulation_data": articulation_data.SimulationManager, + "newton.rigid_object_data": rigid_object_data.SimulationManager, + "newton.collection": rigid_object_collection.SimulationManager, + "newton.collection_data": rigid_object_collection_data.SimulationManager, + } + ) + return bindings + + +def _assert_identical_bindings(actual: dict[str, object], expected: dict[str, object]) -> None: + """Assert exact binding identity without invoking mock equality operators.""" + assert actual.keys() == expected.keys() + assert all(actual[name] is original for name, original in expected.items()) + + +BACKEND_STATUSES_BY_NAME = {status.declaration.name: status for status in BACKEND_STATUSES} + + +@pytest.mark.parametrize("reverse", [False, True]) +def test_contract_factory_manager_patch_supports_cross_suite_asset_access(reverse: bool) -> None: + """Keep each manager available while rigid, collection, and articulation methods run.""" + from ._articulation_contract_utils import get_articulation + from ._manager_patch_scope import contract_manager_patch_scope + from ._rigid_object_collection_contract_utils import get_rigid_object_collection + from ._rigid_object_contract_utils import get_rigid_object + + available_backends = [name for name in ("physx", "newton") if BACKEND_STATUSES_BY_NAME[name].available] + if reverse: + available_backends.reverse() + original_bindings = _available_contract_manager_globals() + + with contract_manager_patch_scope(): + for backend in available_backends: + rigid_object, _ = get_rigid_object(backend, device="cpu") + collection, _ = get_rigid_object_collection(backend, device="cpu") + articulation, _ = get_articulation(backend, device="cpu") + + assert rigid_object.data.root_link_pose_w.shape == (2,) + assert collection.data.body_link_pose_w.shape == (2, 3) + assert articulation.data.root_link_pose_w.shape == (2,) + + _assert_identical_bindings(_available_contract_manager_globals(), original_bindings) + + +def test_contract_factory_modules_keep_production_manager_bindings_after_collection() -> None: + """Keep optional backend imports free of import-time manager substitutions.""" + import inspect + + if BACKEND_STATUSES_BY_NAME["physx"].available: + from isaaclab_physx.physics import PhysxManager + + assert isinstance(inspect.getattr_static(PhysxManager, "get_physics_sim_view"), classmethod) + if BACKEND_STATUSES_BY_NAME["newton"].available: + from isaaclab_newton.physics import NewtonManager + + bindings = _available_contract_manager_globals() + assert all(manager is NewtonManager for name, manager in bindings.items() if name.startswith("newton.")) + + +def test_backend_declaration_reports_missing_required_module() -> None: + """Classify a declared backend as unavailable when an explicit dependency is missing.""" + declaration = BackendDeclaration( + name="example", + required_modules=("present", "missing"), + capabilities=frozenset({"api"}), + ) + + status = evaluate_backend(declaration, lambda name: name == "present", cuda_available=True) + + assert not status.available + assert status.reason == "missing required module: missing" + + +def test_backend_declaration_reports_required_cuda_runtime() -> None: + """Classify a declared backend as unavailable when its mock contract requires CUDA.""" + declaration = BackendDeclaration( + name="example", + required_modules=("present",), + capabilities=frozenset({"api"}), + requires_cuda_runtime=True, + ) + + status = evaluate_backend(declaration, lambda name: True, cuda_available=False) + + assert not status.available + assert status.reason == "CUDA runtime unavailable" + + +def test_backend_parameters_keep_unavailable_backends_as_explicit_skips() -> None: + """Represent an unavailable declared backend explicitly instead of returning an empty matrix.""" + declaration = BackendDeclaration( + name="example", + required_modules=("missing",), + capabilities=frozenset({"api"}), + ) + status = BackendStatus(declaration, available=False, reason="missing required module: missing") + + parameters = backend_parameters("api", statuses=(status,)) + + assert len(parameters) == 1 + assert parameters[0].values == ("example",) + assert parameters[0].marks[0].kwargs["reason"] == "missing required module: missing" + + +def test_backend_parameters_keep_unsupported_behavior_as_explicit_skip() -> None: + """Represent explicitly unsupported behavior with the declared backend-specific reason.""" + declaration = BackendDeclaration( + name="example", + required_modules=("present",), + capabilities=frozenset({"api"}), + unsupported={"writes": "write path is intentionally unsupported"}, + ) + status = BackendStatus(declaration, available=True, reason=None) + + parameters = backend_parameters("writes", statuses=(status,)) + + assert len(parameters) == 1 + assert parameters[0].values == ("example",) + assert parameters[0].marks[0].kwargs["reason"] == "write path is intentionally unsupported" + + +def test_installed_backend_packages_cannot_remain_silently_unavailable() -> None: + """Require every installed declared backend package to have a runnable contract status.""" + unavailable = { + status.declaration.name: status.reason + for status in BACKEND_STATUSES + if find_spec(status.declaration.required_modules[-1]) is not None + and not status.declaration.requires_cuda_runtime + and not status.available + } + + assert unavailable == {} + + +@pytest.mark.parametrize( + ("test_method", "expected_capability"), + [ + (_ArticulationDataContract.test_root_link_pose_w, "data"), + (_ArticulationWriteContract.test_write_root_pose_to_sim_index, "writes"), + (_RigidObjectCacheInvalidationContract.test_pose_write_invalidates_pose_dependent_caches, "writes"), + (_CollectionCacheInvalidationContract.test_pose_write_invalidates_pose_dependent_caches, "writes"), + (_ArticulationOrderingContract.test_backends_allocate_shadows_only_for_nonidentity_ordering, "ordering"), + (_ArticulationTendonDataContract.test_fixed_tendon_stiffness, "fixed_tendons"), + (_ArticulationTendonDataContract.test_spatial_tendon_stiffness, "spatial_tendons"), + ], +) +def test_contract_cases_declare_their_distinct_backend_capability(test_method, expected_capability: str) -> None: + """Classify data, write, ordering, and tendon cases with their actual backend capability.""" + assert getattr(test_method, "contract_backend_capability", None) == expected_capability + + +def test_newton_fixed_tendon_contract_is_not_skipped_as_spatially_unsupported() -> None: + """Keep Newton fixed-tendon coverage runnable while its spatial-tendon behavior remains unsupported.""" + backend_mark = next( + mark + for mark in _ArticulationTendonDataContract.test_fixed_tendon_stiffness.pytestmark + if mark.name == "parametrize" and mark.args[0] == "backend" + ) + newton_parameter = next(parameter for parameter in backend_mark.args[1] if parameter.values == ("newton",)) + + assert not any(mark.name == "skip" for mark in newton_parameter.marks) + + +@pytest.mark.parametrize( + "capability", + [ + "com_orientation_write", + "fixed_tendon_extended_data", + "fixed_tendon_extended_write_index", + "fixed_tendon_write_mask", + "fixed_tendon_write_to_sim_index", + "fixed_tendon_write_to_sim_mask", + ], +) +def test_newton_partial_articulation_support_has_reasoned_capability_skips(capability: str) -> None: + """Keep Newton's partial CoM and fixed-tendon support explicit in the contract matrix.""" + parameter = backend_parameters(capability, names=("newton",))[0] + + assert parameter.values == ("newton",) + assert parameter.marks[0].name == "skip" + assert parameter.marks[0].kwargs["reason"] + + +def test_public_surface_reports_an_omitted_declared_member() -> None: + """Report a newly declared public member until a contract explicitly classifies it.""" + + class SyntheticAsset: + @property + def added_member(self) -> int: + return 1 + + unclassified = unclassified_public_members((SyntheticAsset,), {}) + + assert unclassified == {"SyntheticAsset.added_member"} + + +def test_public_surface_accepts_a_classified_declared_member() -> None: + """Accept a declared public member after assigning its contract family.""" + + class SyntheticAsset: + @property + def added_member(self) -> int: + return 1 + + classifications = {"SyntheticAsset.added_member": ContractKind.API} + + assert unclassified_public_members((SyntheticAsset,), classifications) == set() + + +def test_public_surface_audit_reports_a_missing_mapping() -> None: + """Report a declared member that has no explicit contract mapping.""" + + class SyntheticAsset: + added_member = 1 + + audit = public_surface.audit_public_surface((SyntheticAsset,), {}) + + assert audit.missing == frozenset({"SyntheticAsset.added_member"}) + + +def test_public_surface_audit_reports_a_stale_mapping() -> None: + """Report a mapped member that is no longer declared by its base class.""" + + class SyntheticAsset: + added_member = 1 + + mapping = { + "SyntheticAsset.added_member": public_surface.PublicMemberContract( + kind=ContractKind.API, + ), + "SyntheticAsset.removed_member": public_surface.PublicMemberContract( + kind=ContractKind.API, + ), + } + + audit = public_surface.audit_public_surface((SyntheticAsset,), mapping) + + assert audit.stale == frozenset({"SyntheticAsset.removed_member"}) + + +def test_public_surface_audit_reports_an_unreasoned_exclusion() -> None: + """Reject unsupported or out-of-scope mappings that omit their rationale.""" + + class SyntheticAsset: + added_member = 1 + + mapping = { + "SyntheticAsset.added_member": public_surface.PublicMemberContract( + kind=ContractKind.OUT_OF_SCOPE, + reason=None, + ) + } + + audit = public_surface.audit_public_surface((SyntheticAsset,), mapping) + + assert audit.unreasoned == frozenset({"SyntheticAsset.added_member"}) + + +def test_base_public_surface_has_an_explicit_contract_classification() -> None: + """Require the classification inventory and current public surface to match exactly.""" + audit = public_surface.audit_public_surface(BASE_SURFACE_CLASSES, public_surface.PUBLIC_SURFACE_CONTRACTS) + + assert audit.is_valid, audit.format_errors() diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_data.py b/source/isaaclab/test/assets/contract/test_asset_contract_data.py new file mode 100644 index 000000000000..2bbc469d880a --- /dev/null +++ b/source/isaaclab/test/assets/contract/test_asset_contract_data.py @@ -0,0 +1,80 @@ +# 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 + +# ruff: noqa: F401 + +"""Shared asset data contract tests.""" + +import pytest + +pytestmark = pytest.mark.unit + +from ._articulation_contract_cases import ( # noqa: F401 + TestArticulationDataAliases, + TestArticulationDataBodyState, + TestArticulationDataDefaults, + TestArticulationDataDerivedProperties, + TestArticulationDataJointState, + TestArticulationDataRootState, + TestArticulationDataTendonState, + articulation_iface, +) +from ._articulation_contract_utils import get_articulation +from ._articulation_ordering_contract_cases import ( # noqa: F401 + TestArticulationDataBodyState as TestArticulationOrderingDataBodyState, +) +from ._articulation_ordering_contract_cases import ( + TestArticulationDataJointState as TestArticulationOrderingDataJointState, +) +from ._articulation_ordering_contract_cases import ( + TestArticulationOrderingAllocation, +) +from ._rigid_object_collection_contract_cases import ( # noqa: F401 + TestCollectionCacheInvalidation, + TestCollectionDataAliases, + TestCollectionDataBodyState, + TestCollectionDataDefaults, + TestCollectionDataDerived, + TestCollectionDataMass, + TestCollectionDataSliced, + TestCollectionViewReshape, + collection_iface, +) +from ._rigid_object_collection_contract_utils import get_rigid_object_collection +from ._rigid_object_contract_cases import ( # noqa: F401 + TestRigidObjectCacheInvalidation, + TestRigidObjectDataAliases, + TestRigidObjectDataBodyState, + TestRigidObjectDataDefaults, + TestRigidObjectDataDerivedProperties, + TestRigidObjectDataRootState, + rigid_object_iface, +) +from ._rigid_object_contract_utils import get_rigid_object +from .capabilities import backend_parameters + + +@pytest.mark.parametrize("backend", backend_parameters("cuda")) +def test_articulation_data_has_one_cuda_smoke_per_backend(backend: str) -> None: + """Exercise the distinct CUDA allocation path once for each articulation backend.""" + articulation, _ = get_articulation(backend, num_instances=2, num_joints=4, num_bodies=3, device="cuda:0") + + assert str(articulation.data.joint_pos.device) == "cuda:0" + + +@pytest.mark.parametrize("backend", backend_parameters("cuda")) +def test_rigid_object_data_has_one_cuda_smoke_per_backend(backend: str) -> None: + """Exercise the distinct CUDA allocation path once for each rigid-object backend.""" + rigid_object, _ = get_rigid_object(backend, num_instances=2, device="cuda:0") + + assert str(rigid_object.data.root_link_pose_w.device) == "cuda:0" + + +@pytest.mark.parametrize("backend", backend_parameters("cuda")) +def test_collection_data_has_one_cuda_smoke_per_backend(backend: str) -> None: + """Exercise the distinct CUDA allocation path once for each collection backend.""" + collection, _ = get_rigid_object_collection(backend, num_instances=2, num_bodies=3, device="cuda:0") + + assert str(collection.data.body_link_pose_w.device) == "cuda:0" diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_writes.py b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py new file mode 100644 index 000000000000..1bfe9a3844c5 --- /dev/null +++ b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py @@ -0,0 +1,44 @@ +# 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 + +# ruff: noqa: F401 + +"""Shared asset write contract tests.""" + +import pytest + +pytestmark = pytest.mark.unit + +from ._articulation_contract_cases import ( # noqa: F401 + TestArticulationWritersBody, + TestArticulationWritersFixedTendon, + TestArticulationWritersJoint, + TestArticulationWritersRoot, + TestArticulationWritersSpatialTendon, + TestArticulationWritersTendonToSim, + articulation_iface, +) +from ._articulation_ordering_contract_cases import ( # noqa: F401 + TestArticulationOperations, + TestArticulationOrderingComWrites, + TestArticulationOrderingRootWriteParity, + TestArticulationOrderingWriteParity, +) +from ._articulation_ordering_contract_cases import ( + TestArticulationWritersBody as TestArticulationOrderingWritersBody, +) +from ._articulation_ordering_contract_cases import ( + TestArticulationWritersJoint as TestArticulationOrderingWritersJoint, +) +from ._rigid_object_collection_contract_cases import ( # noqa: F401 + TestCollectionWritersBody, + TestCollectionWritersPose, + collection_iface, +) +from ._rigid_object_contract_cases import ( # noqa: F401 + TestRigidObjectWritersBody, + TestRigidObjectWritersRoot, + rigid_object_iface, +) diff --git a/source/isaaclab/test/assets/test_iface_test_utils.py b/source/isaaclab/test/assets/test_iface_test_utils.py deleted file mode 100644 index 883105ee4d02..000000000000 --- a/source/isaaclab/test/assets/test_iface_test_utils.py +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -import os -import subprocess -import sys -from pathlib import Path - -import pytest - -_ASSET_TEST_DIR = Path(__file__).parent -_IFACE_UTIL_MODULES = ( - "_articulation_iface_test_utils", - "_rigid_object_iface_test_utils", - "_rigid_object_collection_iface_test_utils", -) - - -def _run_probe(script: str) -> subprocess.CompletedProcess[str]: - env = os.environ.copy() - env.pop("EXP_PATH", None) - env.pop("LD_PRELOAD", None) - return subprocess.run( - [sys.executable, "-c", script], - cwd=_ASSET_TEST_DIR, - env=env, - capture_output=True, - text=True, - timeout=60, - ) - - -def test_iface_utilities_share_one_bootstrap_module() -> None: - script = f""" -import importlib -import sys - -sys.path.insert(0, {_ASSET_TEST_DIR.as_posix()!r}) -modules = [importlib.import_module(name) for name in {_IFACE_UTIL_MODULES!r}] -boot = importlib.import_module("_iface_test_boot") -assert all(module.simulation_app is boot.simulation_app for module in modules) -""" - - result = _run_probe(script) - - assert result.returncode == 0, result.stdout + result.stderr - - -def test_iface_utilities_import_without_physx_package() -> None: - script = f""" -import builtins -import importlib -import sys - -sys.path.insert(0, {_ASSET_TEST_DIR.as_posix()!r}) -real_import = builtins.__import__ - -def blocked_import(name, *args, **kwargs): - if name == "isaaclab_physx" or name.startswith("isaaclab_physx."): - raise ModuleNotFoundError(name) - return real_import(name, *args, **kwargs) - -builtins.__import__ = blocked_import -modules = [importlib.import_module(name) for name in {_IFACE_UTIL_MODULES!r}] -assert all(not any(backend.lower() == "physx" for backend in module.BACKENDS) for module in modules) -""" - - result = _run_probe(script) - - assert result.returncode == 0, result.stdout + result.stderr - - -@pytest.mark.parametrize("module_name", _IFACE_UTIL_MODULES) -def test_iface_utility_handles_no_available_backends(module_name: str) -> None: - script = f""" -import builtins -import importlib -import sys - -sys.path.insert(0, {_ASSET_TEST_DIR.as_posix()!r}) -real_import = builtins.__import__ - -def guarded_import(name, globals=None, locals=None, fromlist=(), level=0): - backend_prefixes = ("isaaclab_physx.", "isaaclab_newton.", "isaaclab_ov.") - if name == "ovphysx" or name.startswith(backend_prefixes): - raise ModuleNotFoundError("backend dependency unavailable", name="backend_dependency") - return real_import(name, globals, locals, fromlist, level) - -builtins.__import__ = guarded_import -module = importlib.import_module({module_name!r}) -assert module.BACKENDS == [] -""" - - result = _run_probe(script) - - assert result.returncode == 0, result.stdout + result.stderr diff --git a/source/isaaclab/test/envs/mdp/test_terminations.py b/source/isaaclab/test/envs/mdp/test_terminations.py new file mode 100644 index 000000000000..5dd9926abab1 --- /dev/null +++ b/source/isaaclab/test/envs/mdp/test_terminations.py @@ -0,0 +1,33 @@ +# 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 + +"""Focused unit tests for MDP termination terms.""" + +from types import SimpleNamespace + +import torch + +from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit +from isaaclab.managers import SceneEntityCfg + + +def test_joint_effort_limit_terminates_only_environments_with_clipped_selected_joints() -> None: + """Compare computed and applied torque over the selected joints for each environment.""" + robot = SimpleNamespace( + actuators=SimpleNamespace( + computed_effort=SimpleNamespace( + torch=torch.tensor([[10.0, 20.0, 30.0], [10.0, 20.0, 30.0], [10.0, 20.0, 30.0]]) + ), + applied_effort=SimpleNamespace( + torch=torch.tensor([[10.0, 19.0, 30.0], [10.0, 20.0, 30.0], [10.0, 17.0, 30.0]]) + ), + ) + ) + env = SimpleNamespace(scene={"robot": robot}) + asset_cfg = SceneEntityCfg("robot", joint_ids=[1]) + + result = joint_effort_out_of_limit(env, asset_cfg) + + torch.testing.assert_close(result, torch.tensor([True, False, True])) diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py index b0a346eaa6c0..cb0b7dcbfcfe 100644 --- a/source/isaaclab/test/utils/test_wrench_composer.py +++ b/source/isaaclab/test/utils/test_wrench_composer.py @@ -3,1775 +3,435 @@ # # SPDX-License-Identifier: BSD-3-Clause +"""Focused unit tests for :class:`isaaclab.utils.wrench_composer.WrenchComposer`.""" + from types import SimpleNamespace -import numpy as np import pytest import torch import warp as wp -from isaaclab.test.utils import test_devices from isaaclab.utils.warp import ProxyArray from isaaclab.utils.wrench_composer import WrenchComposer pytestmark = pytest.mark.unit -class _WrenchAssetDataFixture: - """Minimal asset data required by :class:`WrenchComposer`.""" - - def __init__(self, com_pos_w: torch.Tensor, link_quat_w: torch.Tensor, device: str) -> None: - self._device = device - self.body_com_pos_w = ProxyArray(wp.from_torch(com_pos_w.to(device), dtype=wp.vec3f)) - self.body_link_quat_w = ProxyArray(wp.from_torch(link_quat_w.to(device), dtype=wp.quatf)) - - def set_body_com_pose_w(self, pose_w: torch.Tensor) -> None: - self.body_com_pos_w = ProxyArray(wp.from_torch(pose_w[..., :3].to(self._device), dtype=wp.vec3f)) - - -def create_mock_asset( - num_envs: int, - num_bodies: int, - device: str, - link_pos: torch.Tensor | None = None, - link_quat: torch.Tensor | None = None, -) -> SimpleNamespace: - """Create a minimal asset fixture with optional custom link poses. - - Args: - num_envs: Number of environments. - num_bodies: Number of bodies. - device: Device to use. - link_pos: Optional link positions (num_envs, num_bodies, 3). Defaults to zeros. - link_quat: Optional link quaternions in (x, y, z, w) format (num_envs, num_bodies, 4). - Defaults to identity quaternion. - - Returns: - Asset fixture with the state required by WrenchComposer. - """ - - # Build combined pose (N, B, 7) = pos(3) + quat_xyzw(4) matching wp.transformf layout - if link_pos is None: - pos = torch.zeros(num_envs, num_bodies, 3, dtype=torch.float32) - else: - pos = link_pos.float() - - if link_quat is None: - # Identity quaternion in (x, y, z, w) format = (0, 0, 0, 1) - quat = torch.zeros(num_envs, num_bodies, 4, dtype=torch.float32) - quat[..., 3] = 1.0 - else: - quat = link_quat.float() - - return SimpleNamespace( - num_instances=num_envs, num_bodies=num_bodies, device=device, data=_WrenchAssetDataFixture(pos, quat, device) - ) - - -# --- Helper functions for quaternion math --- - - -def quat_rotate_inv_np(quat_xyzw: np.ndarray, vec: np.ndarray) -> np.ndarray: - """Rotate a vector by the inverse of a quaternion (numpy). - - Args: - quat_xyzw: Quaternion in (x, y, z, w) format. Shape: (..., 4) - vec: Vector to rotate. Shape: (..., 3) - - Returns: - Rotated vector. Shape: (..., 3) - """ - # Extract components - xyz = quat_xyzw[..., 0:3] - w = quat_xyzw[..., 3:4] - - # For inverse rotation, we conjugate the quaternion (negate xyz) - # q^-1 * v * q = q_conj * v * q_conj^-1 for unit quaternion - # Using the formula: v' = v + 2*w*(xyz x v) + 2*(xyz x (xyz x v)) - # But for inverse: use -xyz - - # Cross product: xyz x vec - t = 2.0 * np.cross(-xyz, vec, axis=-1) - # Result: vec + w*t + xyz x t - return vec + w * t + np.cross(-xyz, t, axis=-1) - - -def random_unit_quaternion_np(rng: np.random.Generator, shape: tuple) -> np.ndarray: - """Generate random unit quaternions in (x, y, z, w) format. - - Args: - rng: Random number generator. - shape: Output shape, e.g. (num_envs, num_bodies). - - Returns: - Random unit quaternions. Shape: (*shape, 4) - """ - # Generate random quaternion components - q = rng.standard_normal(shape + (4,)).astype(np.float32) - # Normalize to unit quaternion - q = q / np.linalg.norm(q, axis=-1, keepdims=True) - return q - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) -def test_wrench_composer_add_force(device: str, num_envs: int, num_bodies: int): - # Initialize random number generator - rng = np.random.default_rng(seed=0) - - for _ in range(10): - mock_asset = create_mock_asset(num_envs, num_bodies, device) - wrench_composer = WrenchComposer(mock_asset) - # Initialize hand-calculated composed force - hand_calculated_composed_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for _ in range(10): - # Get random number of envs and bodies and their indices - num_envs_np = rng.integers(1, num_envs, endpoint=True) - num_bodies_np = rng.integers(1, num_bodies, endpoint=True) - env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) - body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) - # Convert to warp arrays - env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) - body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) - # Get random forces - forces_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) - # Add forces to wrench composer - wrench_composer.add_forces_and_torques_index(forces=forces, body_ids=body_ids, env_ids=env_ids) - # Add forces to hand-calculated composed force - hand_calculated_composed_force_np[env_ids_np[:, None], body_ids_np[None, :], :] += forces_np - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - # Get composed force from wrench composer - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, hand_calculated_composed_force_np, atol=1, rtol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) -def test_wrench_composer_add_torque(device: str, num_envs: int, num_bodies: int): - # Initialize random number generator - rng = np.random.default_rng(seed=1) - - for _ in range(10): - mock_asset = create_mock_asset(num_envs, num_bodies, device) - wrench_composer = WrenchComposer(mock_asset) - # Initialize hand-calculated composed torque - hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for _ in range(10): - # Get random number of envs and bodies and their indices - num_envs_np = rng.integers(1, num_envs, endpoint=True) - num_bodies_np = rng.integers(1, num_bodies, endpoint=True) - env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) - body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) - # Convert to warp arrays - env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) - body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) - # Get random torques - torques_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) - # Add torques to wrench composer - wrench_composer.add_forces_and_torques_index(torques=torques, body_ids=body_ids, env_ids=env_ids) - # Add torques to hand-calculated composed torque - hand_calculated_composed_torque_np[env_ids_np[:, None], body_ids_np[None, :], :] += torques_np - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - # Get composed torque from wrench composer - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) -def test_add_forces_at_positions(device: str, num_envs: int, num_bodies: int): - """Test adding forces at local positions (offset from link frame).""" - rng = np.random.default_rng(seed=2) - - for _ in range(10): - # Initialize wrench composer - mock_asset = create_mock_asset(num_envs, num_bodies, device) - wrench_composer = WrenchComposer(mock_asset) - # Initialize hand-calculated composed force - hand_calculated_composed_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - # Initialize hand-calculated composed torque - hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for _ in range(10): - # Get random number of envs and bodies and their indices - num_envs_np = rng.integers(1, num_envs, endpoint=True) - num_bodies_np = rng.integers(1, num_bodies, endpoint=True) - env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) - body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) - # Convert to warp arrays - env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) - body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) - # Get random forces - forces_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - positions_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) - positions = wp.from_numpy(positions_np, dtype=wp.vec3f, device=device) - # Add forces at positions to wrench composer - wrench_composer.add_forces_and_torques_index( - forces=forces, positions=positions, body_ids=body_ids, env_ids=env_ids - ) - # Add forces to hand-calculated composed force - hand_calculated_composed_force_np[env_ids_np[:, None], body_ids_np[None, :], :] += forces_np - # Add torques to hand-calculated composed torque: torque = cross(position, force) - torques_from_forces = np.cross(positions_np, forces_np) - for i in range(num_envs_np): - for j in range(num_bodies_np): - hand_calculated_composed_torque_np[env_ids_np[i], body_ids_np[j], :] += torques_from_forces[i, j, :] - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - # Get composed force from wrench composer - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, hand_calculated_composed_force_np, atol=1, rtol=1e-7) - # Get composed torque from wrench composer - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) -def test_add_torques_at_position(device: str, num_envs: int, num_bodies: int): - rng = np.random.default_rng(seed=3) - - for _ in range(10): - mock_asset = create_mock_asset(num_envs, num_bodies, device) - wrench_composer = WrenchComposer(mock_asset) - # Initialize hand-calculated composed torque - hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for _ in range(10): - # Get random number of envs and bodies and their indices - num_envs_np = rng.integers(1, num_envs, endpoint=True) - num_bodies_np = rng.integers(1, num_bodies, endpoint=True) - env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) - body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) - # Convert to warp arrays - env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) - body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) - # Get random torques - torques_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - positions_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) - positions = wp.from_numpy(positions_np, dtype=wp.vec3f, device=device) - # Add torques at positions to wrench composer - wrench_composer.add_forces_and_torques_index( - torques=torques, positions=positions, body_ids=body_ids, env_ids=env_ids - ) - # Add torques to hand-calculated composed torque - hand_calculated_composed_torque_np[env_ids_np[:, None], body_ids_np[None, :], :] += torques_np - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - # Get composed torque from wrench composer - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) -def test_add_forces_and_torques_at_position(device: str, num_envs: int, num_bodies: int): - """Test adding forces and torques at local positions.""" - rng = np.random.default_rng(seed=4) - - for _ in range(10): - mock_asset = create_mock_asset(num_envs, num_bodies, device) - wrench_composer = WrenchComposer(mock_asset) - # Initialize hand-calculated composed force and torque - hand_calculated_composed_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - hand_calculated_composed_torque_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for _ in range(10): - # Get random number of envs and bodies and their indices - num_envs_np = rng.integers(1, num_envs, endpoint=True) - num_bodies_np = rng.integers(1, num_bodies, endpoint=True) - env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) - body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) - # Convert to warp arrays - env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) - body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) - # Get random forces and torques - forces_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - torques_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - positions_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) - torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) - positions = wp.from_numpy(positions_np, dtype=wp.vec3f, device=device) - # Add forces and torques at positions to wrench composer - wrench_composer.add_forces_and_torques_index( - forces=forces, torques=torques, positions=positions, body_ids=body_ids, env_ids=env_ids - ) - # Add forces to hand-calculated composed force - hand_calculated_composed_force_np[env_ids_np[:, None], body_ids_np[None, :], :] += forces_np - # Add torques to hand-calculated composed torque: torque = cross(position, force) + torque - torques_from_forces = np.cross(positions_np, forces_np) - for i in range(num_envs_np): - for j in range(num_bodies_np): - hand_calculated_composed_torque_np[env_ids_np[i], body_ids_np[j], :] += torques_from_forces[i, j, :] - hand_calculated_composed_torque_np[env_ids_np[:, None], body_ids_np[None, :], :] += torques_np - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - # Get composed force from wrench composer - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, hand_calculated_composed_force_np, atol=1, rtol=1e-7) - # Get composed torque from wrench composer - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - assert np.allclose(composed_torque_np, hand_calculated_composed_torque_np, atol=1, rtol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100, 1000]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5, 10]) -def test_wrench_composer_reset(device: str, num_envs: int, num_bodies: int): - rng = np.random.default_rng(seed=5) - for _ in range(10): - mock_asset = create_mock_asset(num_envs, num_bodies, device) - wrench_composer = WrenchComposer(mock_asset) - # Get random number of envs and bodies and their indices - num_envs_np = rng.integers(1, num_envs, endpoint=True) - num_bodies_np = rng.integers(1, num_bodies, endpoint=True) - env_ids_np = rng.choice(num_envs, size=num_envs_np, replace=False) - body_ids_np = rng.choice(num_bodies, size=num_bodies_np, replace=False) - # Convert to warp arrays - env_ids = wp.from_numpy(env_ids_np, dtype=wp.int32, device=device) - body_ids = wp.from_numpy(body_ids_np, dtype=wp.int32, device=device) - # Get random forces and torques - forces_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - torques_np = ( - np.random.uniform(low=-100.0, high=100.0, size=(num_envs_np * num_bodies_np * 3)) - .reshape(num_envs_np, num_bodies_np, 3) - .astype(np.float32) - ) - forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) - torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) - # Add forces and torques to wrench composer - wrench_composer.add_forces_and_torques_index(forces=forces, torques=torques, body_ids=body_ids, env_ids=env_ids) - # Reset wrench composer - wrench_composer.reset() - # Check all 7 buffers are zero (5 input + 2 output) - zeros = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - assert np.allclose(wrench_composer.global_force_w.numpy(), zeros, atol=1, rtol=1e-7) - assert np.allclose(wrench_composer.global_torque_w.numpy(), zeros, atol=1, rtol=1e-7) - assert np.allclose(wrench_composer.global_force_at_com_w.numpy(), zeros, atol=1, rtol=1e-7) - assert np.allclose(wrench_composer.local_force_b.numpy(), zeros, atol=1, rtol=1e-7) - assert np.allclose(wrench_composer.local_torque_b.numpy(), zeros, atol=1, rtol=1e-7) - assert np.allclose(wrench_composer.out_force_b.warp.numpy(), zeros, atol=1, rtol=1e-7) - assert np.allclose(wrench_composer.out_torque_b.warp.numpy(), zeros, atol=1, rtol=1e-7) - - -# ============================================================================ -# Global Frame Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_global_forces_with_rotation(device: str, num_envs: int, num_bodies: int): - """Test that global forces are correctly rotated to the local frame.""" - rng = np.random.default_rng(seed=10) - - for _ in range(5): - # Create random link quaternions - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_quat_torch = torch.from_numpy(link_quat_np) - - # Create mock asset with custom quaternions - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Generate random global forces for all envs and bodies - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) - - # Apply global forces - wrench_composer.add_forces_and_torques_index(forces=forces_global, is_global=True) - - # Compute expected local forces by rotating global forces by inverse quaternion - expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np) - - # Check raw global buffer has the global forces - global_force_np = wrench_composer.global_force_at_com_w.numpy() - assert np.allclose(global_force_np, forces_global_np, atol=1e-4, rtol=1e-5), ( - f"Global force buffer mismatch.\nExpected:\n{forces_global_np}\nGot:\n{global_force_np}" - ) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - # Verify - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, expected_forces_local, atol=1e-4, rtol=1e-5), ( - f"Global force rotation failed.\nExpected:\n{expected_forces_local}\nGot:\n{composed_force_np}" - ) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_global_torques_with_rotation(device: str, num_envs: int, num_bodies: int): - """Test that global torques are correctly rotated to the local frame.""" - rng = np.random.default_rng(seed=11) - - for _ in range(5): - # Create random link quaternions - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_quat_torch = torch.from_numpy(link_quat_np) - - # Create mock asset with custom quaternions - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Generate random global torques - torques_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - torques_global = wp.from_numpy(torques_global_np, dtype=wp.vec3f, device=device) - - # Apply global torques - wrench_composer.add_forces_and_torques_index(torques=torques_global, is_global=True) - - # Compute expected local torques - expected_torques_local = quat_rotate_inv_np(link_quat_np, torques_global_np) - - # Check raw global buffer has the global torques - global_torque_np = wrench_composer.global_torque_w.numpy() - assert np.allclose(global_torque_np, torques_global_np, atol=1e-4, rtol=1e-5), ( - f"Global torque buffer mismatch.\nExpected:\n{torques_global_np}\nGot:\n{global_torque_np}" - ) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - # Verify - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-4, rtol=1e-5), ( - f"Global torque rotation failed.\nExpected:\n{expected_torques_local}\nGot:\n{composed_torque_np}" - ) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 50]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies: int): - """Test global forces at global positions with full coordinate transformation.""" - rng = np.random.default_rng(seed=12) - - for _ in range(5): - # Create random link poses - link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_torch = torch.from_numpy(link_pos_np) - link_quat_torch = torch.from_numpy(link_quat_np) - - # Create mock asset - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Generate random global forces and positions - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_global_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) - positions_global = wp.from_numpy(positions_global_np, dtype=wp.vec3f, device=device) - - # Apply global forces at global positions - wrench_composer.add_forces_and_torques_index(forces=forces_global, positions=positions_global, is_global=True) - - # Compute expected results: - # 1. Force in local frame = quat_rotate_inv(link_quat, global_force) - expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np) - - # 2. Torque about CoM in world frame = cross(P_global - link_pos, F_global) - # Then rotate to body frame - position_offset_global = positions_global_np - link_pos_np - expected_torques_local = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for i in range(num_envs): - for j in range(num_bodies): - torque_w = np.cross(position_offset_global[i, j], forces_global_np[i, j]) - expected_torques_local[i, j] = quat_rotate_inv_np( - link_quat_np[i : i + 1, j : j + 1], torque_w.reshape(1, 1, 3) - )[0, 0] - - # Check raw global force buffer has the global forces - global_force_np = wrench_composer.global_force_w.numpy() - assert np.allclose(global_force_np, forces_global_np, atol=1e-4, rtol=1e-5), ( - f"Global force buffer mismatch.\nExpected:\n{forces_global_np}\nGot:\n{global_force_np}" - ) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - # Verify forces - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, expected_forces_local, atol=1e-3, rtol=1e-4), ( - f"Global force at position failed.\nExpected forces:\n{expected_forces_local}\nGot:\n{composed_force_np}" - ) - - # Verify torques - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-3, rtol=1e-4), ( - f"Global force at position failed.\nExpected torques:\n{expected_torques_local}\nGot:\n{composed_torque_np}" - ) - - -@pytest.mark.parametrize("device", test_devices()) -def test_local_vs_global_identity_quaternion(device: str): - """Test that local and global give same result with identity quaternion and zero position.""" - rng = np.random.default_rng(seed=13) - num_envs, num_bodies = 10, 5 - - # Create mock with identity pose (default) - mock_asset_local = create_mock_asset(num_envs, num_bodies, device) - mock_asset_global = create_mock_asset(num_envs, num_bodies, device) - - wrench_composer_local = WrenchComposer(mock_asset_local) - wrench_composer_global = WrenchComposer(mock_asset_global) - - # Generate random forces and torques - forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - torques_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - forces = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device) - torques = wp.from_numpy(torques_np, dtype=wp.vec3f, device=device) - - # Apply as local - wrench_composer_local.add_forces_and_torques_index(forces=forces, torques=torques, is_global=False) - - # Apply as global (should be same with identity quaternion) - wrench_composer_global.add_forces_and_torques_index(forces=forces, torques=torques, is_global=True) - - # Compose to body frame before checking output - wrench_composer_local.compose_to_body_frame() - wrench_composer_global.compose_to_body_frame() - - # Results should be identical - assert np.allclose( - wrench_composer_local.out_force_b.warp.numpy(), - wrench_composer_global.out_force_b.warp.numpy(), - atol=1e-6, - ) - assert np.allclose( - wrench_composer_local.out_torque_b.warp.numpy(), - wrench_composer_global.out_torque_b.warp.numpy(), - atol=1e-6, - ) - - -@pytest.mark.parametrize("device", test_devices()) -def test_90_degree_rotation_global_force(device: str): - """Test global force with a known 90-degree rotation for easy verification.""" - num_envs, num_bodies = 1, 1 - - # 90-degree rotation around Z-axis: (x, y, z, w) = (0, 0, sin(45°), cos(45°)) - # This rotates X -> Y, Y -> -X - angle = np.pi / 2 - link_quat_np = np.array([[[[0, 0, np.sin(angle / 2), np.cos(angle / 2)]]]], dtype=np.float32).reshape(1, 1, 4) - link_quat_torch = torch.from_numpy(link_quat_np) - - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Apply force in global +X direction - force_global = np.array([[[1.0, 0.0, 0.0]]], dtype=np.float32) - force_wp = wp.from_numpy(force_global, dtype=wp.vec3f, device=device) - - wrench_composer.add_forces_and_torques_index(forces=force_wp, is_global=True) - - # Expected: After inverse rotation (rotate by -90° around Z), X becomes -Y - # Actually, inverse rotation of +90° around Z applied to (1,0,0) gives (0,-1,0) - expected_force_local = np.array([[[0.0, -1.0, 0.0]]], dtype=np.float32) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, expected_force_local, atol=1e-5), ( - f"90-degree rotation test failed.\nExpected:\n{expected_force_local}\nGot:\n{composed_force_np}" - ) - - -@pytest.mark.parametrize("device", test_devices()) -def test_composition_mixed_local_and_global(device: str): - """Test that local and global forces can be composed together correctly.""" - rng = np.random.default_rng(seed=14) - num_envs, num_bodies = 5, 3 - - # Create random link quaternions - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_quat_torch = torch.from_numpy(link_quat_np) - - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Generate random local and global forces - forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - - forces_local = wp.from_numpy(forces_local_np, dtype=wp.vec3f, device=device) - forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) - - # Add local forces first - wrench_composer.add_forces_and_torques_index(forces=forces_local, is_global=False) - - # Add global forces - wrench_composer.add_forces_and_torques_index(forces=forces_global, is_global=True) - - # Expected: local forces stay as-is, global forces get rotated, then sum - global_forces_in_local = quat_rotate_inv_np(link_quat_np, forces_global_np) - expected_total = forces_local_np + global_forces_in_local - - # Check raw buffer properties - local_force_np = wrench_composer.local_force_b.numpy() - assert np.allclose(local_force_np, forces_local_np, atol=1e-4, rtol=1e-5) - global_force_at_com_np = wrench_composer.global_force_at_com_w.numpy() - assert np.allclose(global_force_at_com_np, forces_global_np, atol=1e-4, rtol=1e-5) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - composed_force_np = wrench_composer.out_force_b.warp.numpy() - assert np.allclose(composed_force_np, expected_total, atol=1e-4, rtol=1e-5), ( - f"Mixed local/global composition failed.\nExpected:\n{expected_total}\nGot:\n{composed_force_np}" - ) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 50]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: int): - """Test local forces at local positions (offset from link frame).""" - rng = np.random.default_rng(seed=15) - - for _ in range(5): - # Create random link poses (shouldn't affect local frame calculations) - link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_torch = torch.from_numpy(link_pos_np) - link_quat_torch = torch.from_numpy(link_quat_np) - - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Generate random local forces and local positions (offsets) - forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_local_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - forces_local = wp.from_numpy(forces_local_np, dtype=wp.vec3f, device=device) - positions_local = wp.from_numpy(positions_local_np, dtype=wp.vec3f, device=device) - - # Apply local forces at local positions - wrench_composer.add_forces_and_torques_index(forces=forces_local, positions=positions_local, is_global=False) - - # Expected: forces stay as-is, torque = cross(position, force) - expected_forces = forces_local_np - expected_torques = np.cross(positions_local_np, forces_local_np) - - # Check raw local buffer - local_force_np = wrench_composer.local_force_b.numpy() - assert np.allclose(local_force_np, expected_forces, atol=1e-4, rtol=1e-5) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - # Verify - composed_force_np = wrench_composer.out_force_b.warp.numpy() - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - - assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5) - assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5) - - -@pytest.mark.parametrize("device", test_devices()) -def test_global_force_at_link_origin_no_torque(device: str): - """Test that a global force applied at the link origin produces no torque.""" - rng = np.random.default_rng(seed=16) - num_envs, num_bodies = 5, 3 - - # Create random link poses - link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_torch = torch.from_numpy(link_pos_np) - link_quat_torch = torch.from_numpy(link_quat_np) - - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - wrench_composer = WrenchComposer(mock_asset) - - # Generate random global forces - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device) - - # Position = link position (so offset is zero) - positions_at_link = wp.from_numpy(link_pos_np, dtype=wp.vec3f, device=device) +class _AssetData: + """Minimal asset data consumed by :class:`WrenchComposer`.""" - # Apply global forces at link origin - wrench_composer.add_forces_and_torques_index(forces=forces_global, positions=positions_at_link, is_global=True) + def __init__(self, com_pos_w: torch.Tensor, link_quat_w: torch.Tensor) -> None: + self.body_com_pos_w = ProxyArray(wp.from_torch(com_pos_w, dtype=wp.vec3f)) + self.body_link_quat_w = ProxyArray(wp.from_torch(link_quat_w, dtype=wp.quatf)) - # Expected: force rotated to local, torque = 0 (since position offset is zero) - expected_forces = quat_rotate_inv_np(link_quat_np, forces_global_np) - expected_torques = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - # Check raw global force buffer - global_force_np = wrench_composer.global_force_w.numpy() - assert np.allclose(global_force_np, forces_global_np, atol=1e-4, rtol=1e-5) - - # Compose to body frame before checking output - wrench_composer.compose_to_body_frame() - - composed_force_np = wrench_composer.out_force_b.warp.numpy() - composed_torque_np = wrench_composer.out_torque_b.warp.numpy() - - assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5) - assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5) - - -# ============================================================================ -# add_raw_buffers_from Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_add_raw_buffers_from(device: str, num_envs: int, num_bodies: int): - """Test that add_raw_buffers_from merges all five input buffers correctly.""" - rng = np.random.default_rng(seed=20) - - # Create two composers with random link poses - link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_torch = torch.from_numpy(link_pos_np) - link_quat_torch = torch.from_numpy(link_quat_np) - - mock_a = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - mock_b = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - - composer_a = WrenchComposer(mock_a) - composer_b = WrenchComposer(mock_b) - - # Populate composer_a with local forces at positions - forces_local_a_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_local_a_np = rng.uniform(-5.0, 5.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer_a.add_forces_and_torques_index( - forces=wp.from_numpy(forces_local_a_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_local_a_np, dtype=wp.vec3f, device=device), - is_global=False, - ) - - # Populate composer_b with global forces at global positions - forces_global_b_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_global_b_np = rng.uniform(-5.0, 5.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer_b.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_b_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_global_b_np, dtype=wp.vec3f, device=device), - is_global=True, - ) - - # Merge b into a - composer_a.add_raw_buffers_from(composer_b) - - # Build a reference composer that receives both calls directly - mock_ref = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - composer_ref = WrenchComposer(mock_ref) - composer_ref.add_forces_and_torques_index( - forces=wp.from_numpy(forces_local_a_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_local_a_np, dtype=wp.vec3f, device=device), - is_global=False, - ) - composer_ref.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_b_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_global_b_np, dtype=wp.vec3f, device=device), - is_global=True, - ) - - # Compose both and compare - composer_a.compose_to_body_frame() - composer_ref.compose_to_body_frame() - - assert np.allclose( - composer_a.out_force_b.warp.numpy(), composer_ref.out_force_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "add_raw_buffers_from force mismatch vs direct accumulation" - assert np.allclose( - composer_a.out_torque_b.warp.numpy(), composer_ref.out_torque_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "add_raw_buffers_from torque mismatch vs direct accumulation" - - -@pytest.mark.parametrize("device", test_devices()) -def test_add_raw_buffers_from_inactive_is_noop(device: str): - """Test that add_raw_buffers_from is a no-op when the source composer is inactive.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=21) - - mock_a = create_mock_asset(num_envs, num_bodies, device) - mock_b = create_mock_asset(num_envs, num_bodies, device) - composer_a = WrenchComposer(mock_a) - composer_b = WrenchComposer(mock_b) - - # Populate composer_a with some forces - forces_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer_a.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - ) - - # composer_b is inactive (never written to) - assert not composer_b.active - - # Snapshot composer_a's local buffer before merge - local_force_before = composer_a.local_force_b.numpy().copy() - - # Merge inactive composer_b into composer_a -- should be a no-op - composer_a.add_raw_buffers_from(composer_b) - - assert np.allclose(composer_a.local_force_b.numpy(), local_force_before, atol=1e-7) - - -# ============================================================================ -# Mask-based API Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_add_forces_mask(device: str, num_envs: int, num_bodies: int): - """Test that add_forces_and_torques_mask produces the same result as the index variant.""" - rng = np.random.default_rng(seed=30) - - for _ in range(5): - # Random subset selection - env_select = rng.choice([True, False], size=num_envs, replace=True) - body_select = rng.choice([True, False], size=num_bodies, replace=True) - # Ensure at least one env and body are selected - env_select[0] = True - body_select[0] = True - - env_ids_np = np.where(env_select)[0].astype(np.int32) - body_ids_np = np.where(body_select)[0].astype(np.int32) - env_mask_np = env_select.astype(np.bool_) - body_mask_np = body_select.astype(np.bool_) - - # Random forces for the full grid (mask variant takes full-sized arrays) - forces_full_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - - # Index-based composer - mock_idx = create_mock_asset(num_envs, num_bodies, device) - composer_idx = WrenchComposer(mock_idx) - # Extract the subset for index API - forces_subset_np = forces_full_np[env_ids_np[:, None], body_ids_np[None, :], :] - composer_idx.add_forces_and_torques_index( - forces=wp.from_numpy(forces_subset_np, dtype=wp.vec3f, device=device), - env_ids=wp.from_numpy(env_ids_np, dtype=wp.int32, device=device), - body_ids=wp.from_numpy(body_ids_np, dtype=wp.int32, device=device), - ) - - # Mask-based composer - mock_mask = create_mock_asset(num_envs, num_bodies, device) - composer_mask = WrenchComposer(mock_mask) - composer_mask.add_forces_and_torques_mask( - forces=wp.from_numpy(forces_full_np, dtype=wp.vec3f, device=device), - env_mask=wp.from_numpy(env_mask_np, dtype=wp.bool, device=device), - body_mask=wp.from_numpy(body_mask_np, dtype=wp.bool, device=device), +def _make_composer(*, com_pos_w: torch.Tensor | None = None, link_quat_w: torch.Tensor | None = None) -> WrenchComposer: + """Create a two-environment, two-body composer with literal fixture data.""" + if com_pos_w is None: + com_pos_w = torch.zeros((2, 2, 3), dtype=torch.float32) + if link_quat_w is None: + link_quat_w = torch.tensor( + [ + [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], + [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], + ], + dtype=torch.float32, ) - - # Compose both - composer_idx.compose_to_body_frame() - composer_mask.compose_to_body_frame() - - assert np.allclose( - composer_idx.out_force_b.warp.numpy(), composer_mask.out_force_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), f"Mask vs index force mismatch (envs={num_envs}, bodies={num_bodies})" - assert np.allclose( - composer_idx.out_torque_b.warp.numpy(), composer_mask.out_torque_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), f"Mask vs index torque mismatch (envs={num_envs}, bodies={num_bodies})" - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("num_envs", [1, 10, 100]) -@pytest.mark.parametrize("num_bodies", [1, 3, 5]) -def test_add_forces_mask_global(device: str, num_envs: int, num_bodies: int): - """Test mask-based API with global forces and positions.""" - rng = np.random.default_rng(seed=31) - - # Random link poses - link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_torch = torch.from_numpy(link_pos_np) - link_quat_torch = torch.from_numpy(link_quat_np) - - # Select all envs and bodies to keep comparison simple - forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - - # Index-based - mock_idx = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - composer_idx = WrenchComposer(mock_idx) - composer_idx.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - is_global=True, + asset = SimpleNamespace( + num_instances=2, + num_bodies=2, + device="cpu", + data=_AssetData(com_pos_w, link_quat_w), ) + return WrenchComposer(asset) - # Mask-based (all-True masks) - mock_mask = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - composer_mask = WrenchComposer(mock_mask) - env_mask = wp.from_numpy(np.ones(num_envs, dtype=np.bool_), dtype=wp.bool, device=device) - body_mask = wp.from_numpy(np.ones(num_bodies, dtype=np.bool_), dtype=wp.bool, device=device) - composer_mask.add_forces_and_torques_mask( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - env_mask=env_mask, - body_mask=body_mask, - is_global=True, - ) - composer_idx.compose_to_body_frame() - composer_mask.compose_to_body_frame() - - assert np.allclose( - composer_idx.out_force_b.warp.numpy(), composer_mask.out_force_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "Mask vs index global force mismatch" - assert np.allclose( - composer_idx.out_torque_b.warp.numpy(), composer_mask.out_torque_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "Mask vs index global torque mismatch" - - -# ============================================================================ -# set_forces_and_torques_index Tests -# ============================================================================ - - -@pytest.mark.parametrize("env_dtype", [torch.int32, torch.int64]) -@pytest.mark.parametrize("body_dtype", [torch.int32, torch.int64]) -def test_index_dtype_combinations_preserve_selected_wrench_cells( - env_dtype: torch.dtype, body_dtype: torch.dtype -) -> None: - """Set, add, and reset selected cells with either index width.""" - composer = WrenchComposer(create_mock_asset(num_envs=3, num_bodies=3, device="cpu")) - env_ids = torch.tensor([2, 0], dtype=env_dtype) - body_ids = torch.tensor([1, 2], dtype=body_dtype) - reset_env_ids = env_ids[:1] - set_forces_np = np.arange(1, 13, dtype=np.float32).reshape(2, 2, 3) - set_torques_np = set_forces_np + 20.0 - add_forces_np = np.full((2, 2, 3), 100.0, dtype=np.float32) - add_torques_np = np.full((2, 2, 3), 200.0, dtype=np.float32) +def _vectors(values: list[list[list[float]]]) -> wp.array: + """Convert a literal tensor-shaped list to a Warp vector array.""" + return wp.from_torch(torch.tensor(values, dtype=torch.float32).contiguous(), dtype=wp.vec3f) - composer.set_forces_and_torques_index( - forces=wp.from_numpy(set_forces_np, dtype=wp.vec3f, device="cpu"), - torques=wp.from_numpy(set_torques_np, dtype=wp.vec3f, device="cpu"), - env_ids=env_ids, - body_ids=body_ids, - ) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(add_forces_np, dtype=wp.vec3f, device="cpu"), - torques=wp.from_numpy(add_torques_np, dtype=wp.vec3f, device="cpu"), - env_ids=env_ids, - body_ids=body_ids, - ) - - expected_forces = np.zeros((3, 3, 3), dtype=np.float32) - expected_torques = np.zeros_like(expected_forces) - expected_forces[np.ix_([2, 0], [1, 2])] = set_forces_np + add_forces_np - expected_torques[np.ix_([2, 0], [1, 2])] = set_torques_np + add_torques_np - np.testing.assert_array_equal(composer.local_force_b.numpy(), expected_forces) - np.testing.assert_array_equal(composer.local_torque_b.numpy(), expected_torques) - - composer.reset(env_ids=reset_env_ids) - expected_forces[2] = 0.0 - expected_torques[2] = 0.0 - np.testing.assert_array_equal(composer.local_force_b.numpy(), expected_forces) - np.testing.assert_array_equal(composer.local_torque_b.numpy(), expected_torques) +def _mask(values: list[bool]) -> wp.array: + """Convert literal booleans to a Warp mask array with owned tensor storage.""" + return wp.from_torch(torch.tensor(values, dtype=torch.bool).contiguous(), dtype=wp.bool) -@pytest.mark.parametrize("device", test_devices()) -def test_set_forces_overwrites_previous_add(device: str): - """Test that set_forces_and_torques_index clears previously accumulated values.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=40) - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) +def test_local_force_and_torque_at_position_compose_to_literal_wrench() -> None: + composer = _make_composer() - # First accumulate some forces via add - forces_a_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_a_np, dtype=wp.vec3f, device=device), + forces=_vectors([[[2.0, 0.0, 0.0]]]), + torques=_vectors([[[0.0, 0.0, 5.0]]]), + positions=_vectors([[[0.0, 3.0, 0.0]]]), + env_ids=torch.tensor([1]), + body_ids=[0], ) - # Now set new forces -- should replace, not accumulate - forces_b_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.set_forces_and_torques_index( - forces=wp.from_numpy(forces_b_np, dtype=wp.vec3f, device=device), - ) - - composer.compose_to_body_frame() - - # Output should match forces_b only (forces_a should be gone) - assert np.allclose(composer.out_force_b.warp.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), ( - "set_forces did not clear previous add" - ) + expected_force = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[2.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, -1.0], [0.0, 0.0, 0.0]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) -@pytest.mark.parametrize("device", test_devices()) -def test_set_forces_clears_targeted_envs_only(device: str): - """Test that set_forces_and_torques_index clears only the targeted environments.""" - num_envs, num_bodies = 4, 3 - rng = np.random.default_rng(seed=41) - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) +def test_add_accumulates_local_wrenches() -> None: + composer = _make_composer() - # Add global forces at positions (populates global_force_w and global_torque_w) - forces_global_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_np = rng.uniform(-5.0, 5.0, (num_envs, num_bodies, 3)).astype(np.float32) composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - is_global=True, + forces=_vectors([[[1.0, 2.0, 3.0]]]), torques=_vectors([[[0.0, 1.0, 0.0]]]), body_ids=[1], env_ids=[0] ) - - # Also add local torques (populates local_torque_b) - torques_local_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) composer.add_forces_and_torques_index( - torques=wp.from_numpy(torques_local_np, dtype=wp.vec3f, device=device), - is_global=False, - ) - - # Now set local forces for envs [0, 2] -- should clear only envs 0, 2 - env_ids_np = np.array([0, 2], dtype=np.int32) - kept_env_ids = np.array([1, 3], dtype=np.int32) - forces_new_np = rng.uniform(-50.0, 50.0, (2, num_bodies, 3)).astype(np.float32) - composer.set_forces_and_torques_index( - forces=wp.from_numpy(forces_new_np, dtype=wp.vec3f, device=device), - env_ids=wp.from_numpy(env_ids_np, dtype=wp.int32, device=device), - is_global=False, - ) - - zeros = np.zeros((num_bodies, 3), dtype=np.float32) - - # Targeted envs [0, 2]: all buffers cleared, then local_force_b written - for eid in env_ids_np: - assert np.allclose(composer.global_force_w.numpy()[eid], zeros, atol=1e-7), ( - f"global_force_w not cleared for targeted env {eid}" - ) - assert np.allclose(composer.global_torque_w.numpy()[eid], zeros, atol=1e-7), ( - f"global_torque_w not cleared for targeted env {eid}" - ) - assert np.allclose(composer.local_torque_b.numpy()[eid], zeros, atol=1e-7), ( - f"local_torque_b not cleared for targeted env {eid}" - ) - - # Non-targeted envs [1, 3]: should retain original values - for eid in kept_env_ids: - assert np.allclose(composer.global_force_w.numpy()[eid], forces_global_np[eid], atol=1e-4, rtol=1e-5), ( - f"global_force_w changed for non-targeted env {eid}" - ) - assert np.allclose(composer.local_torque_b.numpy()[eid], torques_local_np[eid], atol=1e-4, rtol=1e-5), ( - f"local_torque_b changed for non-targeted env {eid}" - ) - - # local_force_b should have new values at env_ids [0, 2], zeros at [1, 3] - expected_local_force = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - expected_local_force[env_ids_np] = forces_new_np - assert np.allclose(composer.local_force_b.numpy(), expected_local_force, atol=1e-4, rtol=1e-5), ( - "local_force_b has wrong values after set" + forces=_vectors([[[4.0, -1.0, 0.0]]]), torques=_vectors([[[2.0, 0.0, -3.0]]]), body_ids=[1], env_ids=[0] ) + expected_force = torch.tensor([[[0.0, 0.0, 0.0], [5.0, 1.0, 3.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, 0.0, 0.0], [2.0, 1.0, -3.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) -# ============================================================================ -# Partial Reset Tests -# ============================================================================ + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) -@pytest.mark.parametrize("device", test_devices()) -def test_partial_reset_zeros_only_specified_envs(device: str): - """Test that partial reset zeros only the specified environments and leaves others intact.""" - num_envs, num_bodies = 8, 3 - rng = np.random.default_rng(seed=50) - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - # Populate all envs with local forces - forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), +def test_global_force_at_position_rotates_force_and_induced_torque() -> None: + quarter_turn_z = 2.0**-0.5 + composer = _make_composer( + com_pos_w=torch.tensor([[[1.0, 2.0, 3.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]), + link_quat_w=torch.tensor( + [ + [[0.0, 0.0, quarter_turn_z, quarter_turn_z], [0.0, 0.0, 0.0, 1.0]], + [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], + ] + ), ) - # Also add global forces to populate more buffers - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device), + forces=_vectors([[[0.0, 0.0, 2.0]]]), + positions=_vectors([[[1.0, 4.0, 3.0]]]), + body_ids=[0], + env_ids=[0], is_global=True, ) - # Partial reset: only envs [1, 3, 5] - reset_env_ids = np.array([1, 3, 5], dtype=np.int32) - kept_env_ids = np.array([0, 2, 4, 6, 7], dtype=np.int32) - composer.reset(env_ids=wp.from_numpy(reset_env_ids, dtype=wp.int32, device=device)) - - # Reset envs should be zeroed across all input buffers - zeros = np.zeros((num_bodies, 3), dtype=np.float32) - local_force = composer.local_force_b.numpy() - global_force_at_com = composer.global_force_at_com_w.numpy() - for eid in reset_env_ids: - assert np.allclose(local_force[eid], zeros, atol=1e-7), f"local_force_b not zeroed for env {eid}" - assert np.allclose(global_force_at_com[eid], zeros, atol=1e-7), ( - f"global_force_at_com_w not zeroed for env {eid}" - ) - - # Kept envs should retain their values - for eid in kept_env_ids: - assert np.allclose(local_force[eid], forces_np[eid], atol=1e-4, rtol=1e-5), ( - f"local_force_b changed for non-reset env {eid}" - ) - assert np.allclose(global_force_at_com[eid], forces_global_np[eid], atol=1e-4, rtol=1e-5), ( - f"global_force_at_com_w changed for non-reset env {eid}" - ) - - # Flags: _active should still be True, _dirty should be True - assert composer.active - assert composer._dirty - + expected_force = torch.tensor([[[0.0, 0.0, 2.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, -4.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("env_ids", [None, slice(None)], ids=["none", "full_slice"]) -def test_full_reset_clears_active_flag(device: str, env_ids: slice | None): - """Test that either full-reset selector clears the _active flag.""" - num_envs, num_bodies = 4, 2 + torch.testing.assert_close(composer.out_force_b.torch, expected_force, atol=1.0e-6, rtol=1.0e-6) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque, atol=1.0e-6, rtol=1.0e-6) - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - forces_np = np.ones((num_envs, num_bodies, 3), dtype=np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - ) - assert composer.active - - composer.reset(env_ids=env_ids) - assert not composer.active - assert not composer._dirty - - -# ============================================================================ -# Deprecated API Backward-Compatibility Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -def test_composed_force_emits_deprecation_warning(device: str): - """Test that accessing composed_force emits a DeprecationWarning.""" - num_envs, num_bodies = 2, 1 - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - forces_np = np.array([[[1.0, 2.0, 3.0]], [[4.0, 5.0, 6.0]]], dtype=np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - ) - - with pytest.warns(DeprecationWarning, match="composed_force.*is deprecated"): - result = composer.composed_force - - # Should return the same data as out_force_b - assert np.allclose(result.warp.numpy(), composer.out_force_b.warp.numpy(), atol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -def test_composed_torque_emits_deprecation_warning(device: str): - """Test that accessing composed_torque emits a DeprecationWarning.""" - num_envs, num_bodies = 2, 1 - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - torques_np = np.array([[[1.0, 2.0, 3.0]], [[4.0, 5.0, 6.0]]], dtype=np.float32) - composer.add_forces_and_torques_index( - torques=wp.from_numpy(torques_np, dtype=wp.vec3f, device=device), - ) - - with pytest.warns(DeprecationWarning, match="composed_torque.*is deprecated"): - result = composer.composed_torque - - assert np.allclose(result.warp.numpy(), composer.out_torque_b.warp.numpy(), atol=1e-7) - - -@pytest.mark.parametrize("device", test_devices()) -def test_deprecated_add_forces_and_torques_emits_warning(device: str): - """Test that the deprecated add_forces_and_torques wrapper emits a warning and works.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=52) - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - forces_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - - with pytest.warns(DeprecationWarning, match="add_forces_and_torques.*is deprecated"): - composer.add_forces_and_torques( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), +def test_global_force_at_com_uses_dedicated_buffer_and_rotates() -> None: + quarter_turn_z = 2.0**-0.5 + composer = _make_composer( + link_quat_w=torch.tensor( + [ + [[0.0, 0.0, quarter_turn_z, quarter_turn_z], [0.0, 0.0, 0.0, 1.0]], + [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], + ] ) - - composer.compose_to_body_frame() - assert np.allclose(composer.out_force_b.warp.numpy(), forces_np, atol=1e-4, rtol=1e-5) - - -# ============================================================================ -# set_forces_and_torques_mask Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -def test_set_forces_mask_overwrites_previous_add(device: str): - """Test that set_forces_and_torques_mask clears previously accumulated values.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=60) - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - # Accumulate some forces via add - forces_a_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_a_np, dtype=wp.vec3f, device=device), ) - - # Now set new forces via mask -- should replace, not accumulate - forces_b_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.set_forces_and_torques_mask( - forces=wp.from_numpy(forces_b_np, dtype=wp.vec3f, device=device), + composer.add_forces_and_torques_index( + forces=_vectors([[[2.0, 0.0, 0.0]]]), body_ids=[0], env_ids=[0], is_global=True ) - composer.compose_to_body_frame() - - # Output should match forces_b only (forces_a should be gone) - assert np.allclose(composer.out_force_b.warp.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), ( - "set_forces_and_torques_mask did not clear previous add" + expected_global_force_at_com = torch.tensor( + [[[2.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]] ) + expected_force = torch.tensor([[[0.0, -2.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + torch.testing.assert_close(wp.to_torch(composer.global_force_at_com_w), expected_global_force_at_com) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, torch.zeros((2, 2, 3))) -@pytest.mark.parametrize("device", test_devices()) -def test_set_forces_mask_clears_targeted_envs_only(device: str): - """Test that set_forces_and_torques_mask clears only the masked environments.""" - num_envs, num_bodies = 4, 3 - rng = np.random.default_rng(seed=61) - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - # Populate global buffers for all envs - forces_global_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_np = rng.uniform(-5.0, 5.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), +def test_global_mask_add_and_set_route_forces_and_torques() -> None: + composer = _make_composer() + composer.add_forces_and_torques_mask( + forces=_vectors( + [ + [[0.0, 0.0, 2.0], [0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + ] + ), + torques=_vectors( + [ + [[1.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + ] + ), + positions=_vectors( + [ + [[0.0, 3.0, 0.0], [0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + ] + ), + env_mask=_mask([True, False]), + body_mask=_mask([True, False]), is_global=True, ) - - # Also add local torques for all envs - torques_local_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - torques=wp.from_numpy(torques_local_np, dtype=wp.vec3f, device=device), - is_global=False, - ) - - # Set local forces via mask for envs [0, 2] -- should clear only masked envs - env_mask_np = np.array([True, False, True, False], dtype=np.bool_) - body_mask_np = np.array([True, True, False], dtype=np.bool_) - forces_new_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) composer.set_forces_and_torques_mask( - forces=wp.from_numpy(forces_new_np, dtype=wp.vec3f, device=device), - env_mask=wp.from_numpy(env_mask_np, dtype=wp.bool, device=device), - body_mask=wp.from_numpy(body_mask_np, dtype=wp.bool, device=device), - is_global=False, - ) - - zeros = np.zeros((num_bodies, 3), dtype=np.float32) - - # Masked envs [0, 2]: all buffers cleared by reset, then local_force_b written where body_mask is True - for eid in [0, 2]: - assert np.allclose(composer.global_force_w.numpy()[eid], zeros, atol=1e-7), ( - f"global_force_w not cleared for masked env {eid}" - ) - assert np.allclose(composer.global_torque_w.numpy()[eid], zeros, atol=1e-7), ( - f"global_torque_w not cleared for masked env {eid}" - ) - assert np.allclose(composer.local_torque_b.numpy()[eid], zeros, atol=1e-7), ( - f"local_torque_b not cleared for masked env {eid}" - ) - - # Non-masked envs [1, 3]: should retain original values - for eid in [1, 3]: - assert np.allclose(composer.global_force_w.numpy()[eid], forces_global_np[eid], atol=1e-4, rtol=1e-5), ( - f"global_force_w changed for non-masked env {eid}" - ) - assert np.allclose(composer.local_torque_b.numpy()[eid], torques_local_np[eid], atol=1e-4, rtol=1e-5), ( - f"local_torque_b changed for non-masked env {eid}" - ) - - # local_force_b should have new values where both masks are True, zeros for masked envs otherwise - expected_local_force = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - for e in range(num_envs): - for b in range(num_bodies): - if env_mask_np[e] and body_mask_np[b]: - expected_local_force[e, b] = forces_new_np[e, b] - assert np.allclose(composer.local_force_b.numpy(), expected_local_force, atol=1e-4, rtol=1e-5), ( - "local_force_b has wrong values after mask set" - ) - - -@pytest.mark.parametrize("device", test_devices()) -def test_set_forces_mask_matches_set_forces_index(device: str): - """Test that set_forces_and_torques_mask produces the same result as the index variant.""" - num_envs, num_bodies = 6, 3 - rng = np.random.default_rng(seed=62) - - # Random link poses - link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_torch = torch.from_numpy(link_pos_np) - link_quat_torch = torch.from_numpy(link_quat_np) - - # Use all envs/bodies to compare - forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - - # Index-based - mock_idx = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - composer_idx = WrenchComposer(mock_idx) - composer_idx.set_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), + forces=_vectors( + [ + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0], [4.0, 0.0, 0.0]], + ] + ), + torques=_vectors( + [ + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0], [0.0, 3.0, 0.0]], + ] + ), + env_mask=_mask([False, True]), + body_mask=_mask([False, True]), is_global=True, ) - # Mask-based (all-True) - mock_mask = create_mock_asset(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch) - composer_mask = WrenchComposer(mock_mask) - composer_mask.set_forces_and_torques_mask( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - is_global=True, + expected_force = torch.tensor([[[0.0, 0.0, 2.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [4.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[7.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 3.0, 0.0]]]) + expected_global_force = torch.tensor([[[0.0, 0.0, 2.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + expected_global_force_at_com = torch.tensor( + [[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [4.0, 0.0, 0.0]]] ) + expected_global_torque = torch.tensor([[[7.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 3.0, 0.0]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) + torch.testing.assert_close(wp.to_torch(composer.global_force_w), expected_global_force) + torch.testing.assert_close(wp.to_torch(composer.global_force_at_com_w), expected_global_force_at_com) + torch.testing.assert_close(wp.to_torch(composer.global_torque_w), expected_global_torque) - composer_idx.compose_to_body_frame() - composer_mask.compose_to_body_frame() - - assert np.allclose( - composer_idx.out_force_b.warp.numpy(), composer_mask.out_force_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "set mask vs index force mismatch" - assert np.allclose( - composer_idx.out_torque_b.warp.numpy(), composer_mask.out_torque_b.warp.numpy(), atol=1e-4, rtol=1e-5 - ), "set mask vs index torque mismatch" - - -# ============================================================================ -# Lazy Composition (_ensure_composed) Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -def test_out_force_b_triggers_lazy_composition(device: str): - """Test that accessing out_force_b without explicit compose_to_body_frame still returns correct results.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=70) - - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_quat_torch = torch.from_numpy(link_quat_np) - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - composer = WrenchComposer(mock_asset) +def test_index_and_mask_selection_change_only_selected_cells() -> None: + composer = _make_composer() - forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device), - is_global=True, - ) - - # Do NOT call compose_to_body_frame -- rely on lazy composition - expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np) - composed_force_np = composer.out_force_b.warp.numpy() - - assert np.allclose(composed_force_np, expected_forces_local, atol=1e-4, rtol=1e-5), ( - "Lazy composition via out_force_b failed" + forces=_vectors([[[1.0, 0.0, 0.0]]]), body_ids=torch.tensor([1], dtype=torch.int64), env_ids=torch.tensor([0]) + ) + composer.add_forces_and_torques_mask( + forces=_vectors( + [ + [[10.0, 0.0, 0.0], [20.0, 0.0, 0.0]], + [[30.0, 0.0, 0.0], [40.0, 0.0, 0.0]], + ] + ), + env_mask=_mask([False, True]), + body_mask=_mask([True, False]), ) + expected_force = torch.tensor([[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], [[30.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) -@pytest.mark.parametrize("device", test_devices()) -def test_out_torque_b_triggers_lazy_composition(device: str): - """Test that accessing out_torque_b without explicit compose_to_body_frame still returns correct results.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=71) - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_quat_torch = torch.from_numpy(link_quat_np) - - mock_asset = create_mock_asset(num_envs, num_bodies, device, link_quat=link_quat_torch) - composer = WrenchComposer(mock_asset) - - torques_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) +def test_set_clears_only_targeted_environment_before_writing() -> None: + composer = _make_composer() composer.add_forces_and_torques_index( - torques=wp.from_numpy(torques_global_np, dtype=wp.vec3f, device=device), - is_global=True, + forces=_vectors( + [ + [[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]], + [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]], + ] + ), + torques=_vectors( + [ + [[0.0, 0.0, 1.0], [0.0, 0.0, 2.0]], + [[0.0, 0.0, 3.0], [0.0, 0.0, 4.0]], + ] + ), ) - # Do NOT call compose_to_body_frame -- rely on lazy composition - expected_torques_local = quat_rotate_inv_np(link_quat_np, torques_global_np) - composed_torque_np = composer.out_torque_b.warp.numpy() - - assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-4, rtol=1e-5), ( - "Lazy composition via out_torque_b failed" + composer.set_forces_and_torques_index( + forces=_vectors([[[9.0, 0.0, 0.0]]]), torques=_vectors([[[0.0, 0.0, 6.0]]]), body_ids=[1], env_ids=[0] ) + expected_force = torch.tensor([[[0.0, 0.0, 0.0], [9.0, 0.0, 0.0]], [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 6.0]], [[0.0, 0.0, 3.0], [0.0, 0.0, 4.0]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) -@pytest.mark.parametrize("device", test_devices()) -def test_lazy_composition_tracks_dirty_flag(device: str): - """Test that the dirty flag is correctly managed through add/compose/add cycles.""" - num_envs, num_bodies = 2, 1 - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - # Initially clean - assert not composer._dirty - # After add, dirty - forces_np = np.ones((num_envs, num_bodies, 3), dtype=np.float32) +def test_mask_set_clears_only_masked_environments_before_writing() -> None: + composer = _make_composer() composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), + forces=_vectors( + [ + [[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]], + [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]], + ] + ), + torques=_vectors( + [ + [[0.0, 0.0, 1.0], [0.0, 0.0, 2.0]], + [[0.0, 0.0, 3.0], [0.0, 0.0, 4.0]], + ] + ), ) - assert composer._dirty - - # After accessing out_force_b, clean (lazy compose happened) - _ = composer.out_force_b - assert not composer._dirty - # After another add, dirty again - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), + composer.set_forces_and_torques_mask( + forces=_vectors( + [ + [[5.0, 0.0, 0.0], [6.0, 0.0, 0.0]], + [[7.0, 0.0, 0.0], [9.0, 0.0, 0.0]], + ] + ), + torques=_vectors( + [ + [[0.0, 0.0, 5.0], [0.0, 0.0, 6.0]], + [[0.0, 0.0, 7.0], [0.0, 0.0, 9.0]], + ] + ), + env_mask=_mask([False, True]), + body_mask=_mask([False, True]), ) - assert composer._dirty - - # Accessing out_torque_b also triggers composition - _ = composer.out_torque_b - assert not composer._dirty - - # Verify accumulated result (2x forces) - expected = 2.0 * forces_np - assert np.allclose(composer.out_force_b.warp.numpy(), expected, atol=1e-4, rtol=1e-5) - - -@pytest.mark.parametrize("device", test_devices()) -def test_compose_is_idempotent(device: str): - """Calling compose_to_body_frame twice without intervening writes produces the same result.""" - rng = np.random.default_rng(seed=456) - num_envs, num_bodies = 4, 3 - # Non-trivial link pose so the rotation path is exercised - link_pos_np = rng.uniform(-2, 2, (num_envs, num_bodies, 3)).astype(np.float32) - link_quat_np = rng.standard_normal((num_envs, num_bodies, 4)).astype(np.float32) - link_quat_np /= np.linalg.norm(link_quat_np, axis=-1, keepdims=True) + expected_force = torch.tensor([[[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [9.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, 0.0, 1.0], [0.0, 0.0, 2.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 9.0]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) - mock_asset = create_mock_asset( - num_envs, - num_bodies, - device, - link_pos=torch.from_numpy(link_pos_np), - link_quat=torch.from_numpy(link_quat_np), - ) - composer = WrenchComposer(mock_asset) - - # Add global forces with positions (exercises cross-product torque path) - forces_np = rng.uniform(-5, 5, (num_envs, num_bodies, 3)).astype(np.float32) - positions_np = rng.uniform(-1, 1, (num_envs, num_bodies, 3)).astype(np.float32) - torques_np = rng.uniform(-3, 3, (num_envs, num_bodies, 3)).astype(np.float32) +def test_partial_and_full_reset_clear_their_documented_scope() -> None: + composer = _make_composer() composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - torques=wp.from_numpy(torques_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - is_global=True, - ) - - # First compose - composer.compose_to_body_frame() - force_first = composer.out_force_b.warp.numpy().copy() - torque_first = composer.out_torque_b.warp.numpy().copy() - - # Second compose (no writes in between) - composer.compose_to_body_frame() - force_second = composer.out_force_b.warp.numpy() - torque_second = composer.out_torque_b.warp.numpy() - - np.testing.assert_array_equal(force_first, force_second) - np.testing.assert_array_equal(torque_first, torque_second) - - -# ============================================================================ -# CoM Offset from Link Origin Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -def test_global_force_with_com_offset(device: str): - """Test that torque correction uses CoM position, not link position, when they differ.""" - num_envs, num_bodies = 2, 1 - - # Link at origin, CoM offset by [1, 0, 0] - link_pos_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - link_quat_np = np.zeros((num_envs, num_bodies, 4), dtype=np.float32) - link_quat_np[..., 3] = 1.0 # identity quaternion (xyzw) - - com_pos_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - com_pos_np[..., 0] = 1.0 # CoM at [1, 0, 0] - - mock_asset = create_mock_asset( - num_envs, - num_bodies, - device, - link_pos=torch.from_numpy(link_pos_np), - link_quat=torch.from_numpy(link_quat_np), - ) - # Set CoM pose separately (pos=[1,0,0], quat=identity) - com_pose = torch.cat( - ( - torch.from_numpy(com_pos_np), - torch.tensor([0.0, 0.0, 0.0, 1.0]).view(1, 1, 4).expand(num_envs, num_bodies, 4), + forces=_vectors( + [ + [[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]], + [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]], + ] + ), + torques=_vectors( + [ + [[0.0, 0.0, 1.0], [0.0, 0.0, 2.0]], + [[0.0, 0.0, 3.0], [0.0, 0.0, 4.0]], + ] ), - dim=-1, ) - mock_asset.data.set_body_com_pose_w(com_pose) - - composer = WrenchComposer(mock_asset) - - # Apply global force [0, 0, 10] at position [0, 0, 0] (world origin) - forces_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - forces_np[..., 2] = 10.0 - positions_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - is_global=True, + composer.reset(env_ids=[0]) + expected_after_partial = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]]]) + expected_torque_after_partial = torch.tensor( + [[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 3.0], [0.0, 0.0, 4.0]]] ) + torch.testing.assert_close(composer.out_force_b.torch, expected_after_partial) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque_after_partial) + assert composer.active - composer.compose_to_body_frame() - - # With identity quaternion: - # torque_w = cross(P, F) - cross(com, F) = cross([0,0,0], [0,0,10]) - cross([1,0,0], [0,0,10]) - # = [0,0,0] - [0*10-0*0, 0*0-1*10, 1*0-0*0] = [0,0,0] - [0, -10, 0] = [0, 10, 0] - # In body frame (identity rotation): [0, 10, 0] - expected_torque = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - expected_torque[..., 1] = 10.0 - - actual_torque = composer.out_torque_b.warp.numpy() - assert np.allclose(actual_torque, expected_torque, atol=1e-4, rtol=1e-5), ( - f"CoM offset torque correction failed.\nExpected:\n{expected_torque}\nGot:\n{actual_torque}" - ) + composer.reset() + torch.testing.assert_close(composer.out_force_b.torch, torch.zeros((2, 2, 3))) + torch.testing.assert_close(composer.out_torque_b.torch, torch.zeros((2, 2, 3))) + torch.testing.assert_close(wp.to_torch(composer.local_torque_b), torch.zeros((2, 2, 3))) + assert not composer.active - # Force should be unchanged (identity rotation) - assert np.allclose(composer.out_force_b.warp.numpy(), forces_np, atol=1e-4, rtol=1e-5) +def test_permanent_and_instantaneous_composers_remain_independent() -> None: + permanent = _make_composer() + instantaneous = _make_composer() + permanent.add_forces_and_torques_index(forces=_vectors([[[5.0, 0.0, 0.0]]]), body_ids=[0], env_ids=[0]) + instantaneous.add_forces_and_torques_index(forces=_vectors([[[0.0, 7.0, 0.0]]]), body_ids=[0], env_ids=[0]) -@pytest.mark.parametrize("device", test_devices()) -def test_global_force_at_com_no_torque_with_com_offset(device: str): - """Test that a global force at CoM position produces zero torque even with CoM offset.""" - num_envs, num_bodies = 2, 1 + instantaneous.reset() - # Link at origin, CoM offset by [2, 3, 0] - link_pos_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - link_quat_np = np.zeros((num_envs, num_bodies, 4), dtype=np.float32) - link_quat_np[..., 3] = 1.0 + expected_permanent = torch.tensor([[[5.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + torch.testing.assert_close(permanent.out_force_b.torch, expected_permanent) + torch.testing.assert_close(instantaneous.out_force_b.torch, torch.zeros((2, 2, 3))) - com_pos_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - com_pos_np[..., 0] = 2.0 - com_pos_np[..., 1] = 3.0 - mock_asset = create_mock_asset( - num_envs, - num_bodies, - device, - link_pos=torch.from_numpy(link_pos_np), - link_quat=torch.from_numpy(link_quat_np), - ) - com_pose = torch.cat( - ( - torch.from_numpy(com_pos_np), - torch.tensor([0.0, 0.0, 0.0, 1.0]).view(1, 1, 4).expand(num_envs, num_bodies, 4), - ), - dim=-1, - ) - mock_asset.data.set_body_com_pose_w(com_pose) +def test_out_torque_b_lazily_composes_a_fresh_torque_only_wrench() -> None: + composer = _make_composer() + composer.add_forces_and_torques_index(torques=_vectors([[[0.0, 0.0, 5.0]]]), body_ids=[0], env_ids=[1]) - composer = WrenchComposer(mock_asset) + expected_torque = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 5.0], [0.0, 0.0, 0.0]]]) + assert composer._dirty + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) + assert not composer._dirty - # Apply global force at the CoM position - forces_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - forces_np[..., 2] = 50.0 - positions_np = com_pos_np.copy() - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), +def test_raw_buffer_merge_accumulates_all_five_buffers_and_ignores_inactive_source() -> None: + destination = _make_composer() + source = _make_composer() + inactive_source = _make_composer() + destination.add_forces_and_torques_index(forces=_vectors([[[2.0, 0.0, 0.0]]]), body_ids=[0], env_ids=[0]) + source.add_forces_and_torques_index( + forces=_vectors([[[1.0, 0.0, 0.0]]]), torques=_vectors([[[0.0, 2.0, 0.0]]]), body_ids=[0], env_ids=[0] + ) + source.add_forces_and_torques_index(forces=_vectors([[[0.0, 0.0, 3.0]]]), body_ids=[0], env_ids=[0], is_global=True) + source.add_forces_and_torques_index( + forces=_vectors([[[0.0, 4.0, 0.0]]]), + torques=_vectors([[[0.0, 0.0, 5.0]]]), + positions=_vectors([[[0.0, 0.0, 0.0]]]), + body_ids=[0], + env_ids=[0], is_global=True, ) - - composer.compose_to_body_frame() - - # Torque = cross(com, F) - cross(com, F) = 0 - expected_torque = np.zeros((num_envs, num_bodies, 3), dtype=np.float32) - assert np.allclose(composer.out_torque_b.warp.numpy(), expected_torque, atol=1e-4, rtol=1e-5), ( - "Force at CoM should produce zero torque regardless of CoM offset" + wp.copy( + inactive_source.local_force_b, + _vectors( + [ + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0], [8.0, 0.0, 0.0]], + ] + ), ) + assert not inactive_source.active + destination.add_raw_buffers_from(source) + destination.add_raw_buffers_from(inactive_source) -@pytest.mark.parametrize("device", test_devices()) -def test_com_offset_with_rotation(device: str): - """Test torque correction with both CoM offset and non-identity rotation.""" - num_envs, num_bodies = 1, 1 - rng = np.random.default_rng(seed=73) - - # Random rotation - link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies)) - link_pos_np = rng.uniform(-5.0, 5.0, (num_envs, num_bodies, 3)).astype(np.float32) - - # CoM offset from link - com_offset_np = rng.uniform(0.5, 2.0, (num_envs, num_bodies, 3)).astype(np.float32) - com_pos_np = link_pos_np + com_offset_np # simple world-frame offset for test clarity - - mock_asset = create_mock_asset( - num_envs, - num_bodies, - device, - link_pos=torch.from_numpy(link_pos_np), - link_quat=torch.from_numpy(link_quat_np), + expected_force = torch.tensor([[[3.0, 4.0, 3.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, 2.0, 5.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + torch.testing.assert_close( + wp.to_torch(destination.local_force_b), + torch.tensor([[[3.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]), ) - com_pose = torch.cat( - ( - torch.from_numpy(com_pos_np), - torch.tensor([0.0, 0.0, 0.0, 1.0]).view(1, 1, 4).expand(num_envs, num_bodies, 4), - ), - dim=-1, + torch.testing.assert_close( + wp.to_torch(destination.global_force_w), + torch.tensor([[[0.0, 4.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]), ) - mock_asset.data.set_body_com_pose_w(com_pose) - - composer = WrenchComposer(mock_asset) - - # Apply global force at a random world position - forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32) - positions_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32) - - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - positions=wp.from_numpy(positions_np, dtype=wp.vec3f, device=device), - is_global=True, + torch.testing.assert_close( + wp.to_torch(destination.global_force_at_com_w), + torch.tensor([[[0.0, 0.0, 3.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]), ) - - composer.compose_to_body_frame() - - # Expected: torque_w = cross(P, F) - cross(com, F) = cross(P - com, F) - lever_arm = positions_np - com_pos_np - torque_w = np.cross(lever_arm, forces_np) - expected_torque_b = quat_rotate_inv_np(link_quat_np, torque_w) - expected_force_b = quat_rotate_inv_np(link_quat_np, forces_np) - - assert np.allclose(composer.out_force_b.warp.numpy(), expected_force_b, atol=1e-3, rtol=1e-4), ( - "Force mismatch with CoM offset + rotation" + torch.testing.assert_close( + wp.to_torch(destination.local_torque_b), + torch.tensor([[[0.0, 2.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]), ) - assert np.allclose(composer.out_torque_b.warp.numpy(), expected_torque_b, atol=1e-3, rtol=1e-4), ( - f"Torque mismatch with CoM offset + rotation.\n" - f"Expected:\n{expected_torque_b}\nGot:\n{composer.out_torque_b.warp.numpy()}" + torch.testing.assert_close( + wp.to_torch(destination.global_torque_w), + torch.tensor([[[0.0, 0.0, 5.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]), ) + torch.testing.assert_close(destination.out_force_b.torch, expected_force) + torch.testing.assert_close(destination.out_torque_b.torch, expected_torque) -# ============================================================================ -# Deprecated set_forces_and_torques Tests -# ============================================================================ - - -@pytest.mark.parametrize("device", test_devices()) -def test_deprecated_set_forces_and_torques_emits_warning(device: str): - """Test that the deprecated set_forces_and_torques wrapper emits a warning and works.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=80) - - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - - forces_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) +def test_invalid_selection_and_empty_wrench_input_are_reported() -> None: + composer = _make_composer() - with pytest.warns(DeprecationWarning, match="set_forces_and_torques.*is deprecated"): - composer.set_forces_and_torques( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - ) - - composer.compose_to_body_frame() - assert np.allclose(composer.out_force_b.warp.numpy(), forces_np, atol=1e-4, rtol=1e-5) - - -@pytest.mark.parametrize("device", test_devices()) -def test_deprecated_set_forces_and_torques_clears_previous(device: str): - """Test that deprecated set_forces_and_torques actually replaces previous values.""" - num_envs, num_bodies = 4, 2 - rng = np.random.default_rng(seed=81) + with pytest.raises(TypeError, match="env_ids must be"): + composer.add_forces_and_torques_index(forces=_vectors([[[1.0, 0.0, 0.0]]]), env_ids=(0,)) + with pytest.warns(UserWarning, match="No forces or torques"): + composer.add_forces_and_torques_index() + assert not composer.active - mock_asset = create_mock_asset(num_envs, num_bodies, device) - composer = WrenchComposer(mock_asset) - # First add some forces - forces_a_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_a_np, dtype=wp.vec3f, device=device), - ) +def test_deprecated_wrappers_warn_and_preserve_wrench_behavior() -> None: + composer = _make_composer() - # Then set via deprecated method -- should replace - forces_b_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) - with pytest.warns(DeprecationWarning): - composer.set_forces_and_torques( - forces=wp.from_numpy(forces_b_np, dtype=wp.vec3f, device=device), + with pytest.warns(DeprecationWarning, match="add_forces_and_torques.*deprecated"): + composer.add_forces_and_torques( + forces=_vectors([[[3.0, 0.0, 0.0]]]), + torques=_vectors([[[0.0, 0.0, 2.0]]]), + body_ids=[1], + env_ids=[1], ) - - composer.compose_to_body_frame() - assert np.allclose(composer.out_force_b.warp.numpy(), forces_b_np, atol=1e-4, rtol=1e-5), ( - "Deprecated set_forces_and_torques did not replace previous values" - ) + with pytest.warns(DeprecationWarning, match="composed_force.*deprecated"): + force = composer.composed_force.torch + with pytest.warns(DeprecationWarning, match="composed_torque.*deprecated"): + torque = composer.composed_torque.torch + + expected_force = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [3.0, 0.0, 0.0]]]) + expected_torque = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 2.0]]]) + torch.testing.assert_close(force, expected_force) + torch.testing.assert_close(torque, expected_torque) + + with pytest.warns(DeprecationWarning, match="set_forces_and_torques.*deprecated"): + composer.set_forces_and_torques(forces=_vectors([[[4.0, 0.0, 0.0]]]), body_ids=[1], env_ids=[1]) + + expected_force[1, 1] = torch.tensor([4.0, 0.0, 0.0]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, torch.zeros((2, 2, 3))) diff --git a/source/isaaclab/test/utils/test_wrench_composer_integration.py b/source/isaaclab/test/utils/test_wrench_composer_integration.py index 5f5e0ccb1ee6..4b3f130be4b1 100644 --- a/source/isaaclab/test/utils/test_wrench_composer_integration.py +++ b/source/isaaclab/test/utils/test_wrench_composer_integration.py @@ -3,19 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Integration tests for wrench composer with rigid objects. - -These tests validate that global forces/torques remain invariant under body rotation -""" - -"""Launch Isaac Sim Simulator first.""" +"""Minimal real PhysX parity coverage for :class:`WrenchComposer`.""" from isaaclab.app import AppLauncher -# launch omniverse app simulation_app = AppLauncher(headless=True).app -"""Rest everything follows.""" +import math import pytest import torch @@ -24,822 +18,81 @@ import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sim import build_simulation_context -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR pytestmark = pytest.mark.integration +_ROTATION_45_Z = (0.0, 0.0, math.sin(math.pi / 8), math.cos(math.pi / 8)) -def generate_cubes_scene( - num_cubes: int = 1, - height: float = 1.0, - device: str = "cuda:0", -) -> tuple[RigidObject, torch.Tensor]: - """Generate a scene with the provided number of cubes.""" - origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - ) +def _make_dual_cube_scene(device: str) -> tuple[RigidObject, RigidObject]: + """Create matched composer and raw-PhysX cubes with a rotated initial pose.""" + for name, y_offset in (("Composer", 0.0), ("Raw", 3.0)): + sim_utils.create_prim(f"/World/{name}", "Xform", translation=(0.0, y_offset, 1.0)) - cube_object_cfg = RigidObjectCfg( - prim_path="/World/Table_[^/]*/Object", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height)), + spawn = sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), ) - cube_object = RigidObject(cfg=cube_object_cfg) - return cube_object, origins - - -N_STEPS = 100 -FORCE_MAGNITUDE = 10.0 -TORQUE_MAGNITUDE = 1.0 - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_force_invariant_under_rotation(device): - """Test that a permanent global force produces the same acceleration before and after body rotation. - - A global +X force is applied. After 100 steps the body is rotated 180deg about Z. - The acceleration (delta_v per phase) should be the same in both phases because the - force is in the global frame and should not rotate with the body. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - mass = float(wp.to_torch(cube_object.root_view.get_masses())[0]) - com = cube_object.data.body_com_pos_w.torch.clone() - - # Apply permanent global force along +X at CoM - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=com, - body_ids=body_ids, - is_global=True, - ) - - # Phase 1: run N_STEPS - for _ in range(N_STEPS): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - vel_after_phase1 = cube_object.data.root_lin_vel_w.torch[0].clone() - - # Rotate body 180deg about Z (quat wxyz = [0, 0, 0, 1]) while keeping velocity - root_pose = cube_object.data.root_pose_w.torch[0].clone().unsqueeze(0) - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 1.0, 0.0], device=device) # 180deg about Z (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - # Phase 2: run N_STEPS more - for _ in range(N_STEPS): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - vel_after_phase2 = cube_object.data.root_lin_vel_w.torch[0].clone() - - # Acceleration should be same in both phases: delta_v_phase2 ≈ delta_v_phase1 - delta_v_phase1 = vel_after_phase1[0].item() # vx after phase 1 - delta_v_phase2 = vel_after_phase2[0].item() - vel_after_phase1[0].item() # vx gained in phase 2 - - expected_dv = FORCE_MAGNITUDE / mass * sim.cfg.dt * N_STEPS - - torch.testing.assert_close( - torch.tensor(delta_v_phase1), - torch.tensor(expected_dv), - rtol=0.001, - atol=0.0001, - ) - torch.testing.assert_close( - torch.tensor(delta_v_phase2), - torch.tensor(expected_dv), - rtol=0.001, - atol=0.0001, - ) - - # Y and Z velocity should remain ~0 - assert abs(vel_after_phase2[1].item()) < 0.5, f"Unexpected Y velocity: {vel_after_phase2[1].item()}" - assert abs(vel_after_phase2[2].item()) < 0.5, f"Unexpected Z velocity: {vel_after_phase2[2].item()}" - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_local_force_follows_rotation(device): - """Test that a permanent local force rotates with the body. - - A local +X force is applied. After 100 steps the body is rotated 180deg about Z. - Since local +X is now world -X, the force should decelerate the body back towards zero velocity. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Apply permanent local force along body +X - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=False, - ) - - # Phase 1: run N_STEPS — object accelerates along world +X - for _ in range(N_STEPS): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - vel_after_phase1 = cube_object.data.root_lin_vel_w.torch[0].clone() - assert vel_after_phase1[0].item() > 1.0, "Object should be moving in +X" - - # Rotate body 180deg about Z while keeping velocity - root_pose = cube_object.data.root_pose_w.torch[0].clone().unsqueeze(0) - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 1.0, 0.0], device=device) # 180deg about Z (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - # Phase 2: run N_STEPS — local +X is now world -X, so force decelerates - for _ in range(N_STEPS): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - vel_after_phase2 = cube_object.data.root_lin_vel_w.torch[0].clone() - - # Velocity should be approximately zero: decelerated by the same amount as it accelerated - torch.testing.assert_close( - vel_after_phase2[0], - torch.tensor(0.0, device=device), - atol=0.0001, - rtol=0.001, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_force_at_offset_generates_torque(device): - """Test that a global force applied at an offset from CoM generates the expected torque. - - A global +X force applied at +1m Y offset from CoM should produce: - - Linear acceleration in +X - - Angular acceleration about -Z (from cross product: (0,1,0) × (10,0,0) = (0,0,-10)) - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Force at offset: +1m in Y from CoM (global frame) - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE # +X force - - torques = torch.zeros(1, len(body_ids), 3, device=device) - - # Position offset: CoM position + 1m in Y (global frame) - com_pos = cube_object.data.body_com_pos_w.torch[:, body_ids, :3].clone() - positions = com_pos.clone() - positions[..., 1] += 1.0 # +1m Y offset - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=positions, - body_ids=body_ids, - is_global=True, - ) - - # Run 50 steps - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - lin_vel = cube_object.data.root_lin_vel_w.torch[0] - ang_vel = cube_object.data.root_ang_vel_w.torch[0] - - # Linear velocity in +X should be positive - assert lin_vel[0].item() > 0.1, f"Expected positive X velocity, got {lin_vel[0].item()}" - - # Angular velocity about Z should be negative (cross product: r × F, r=(0,1,0), F=(10,0,0) -> (0,0,-10)) - assert ang_vel[2].item() < -0.1, f"Expected negative Z angular velocity, got {ang_vel[2].item()}" - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_torque_invariant_under_rotation(device): - """Test that a permanent global torque produces the same angular acceleration before and after rotation. - - A global +Z torque is applied. After 100 steps the body is rotated 90deg about X. - The angular acceleration (delta_omega per phase) about Z should be the same in both phases - because the torque is in the global frame. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Apply permanent global torque about +Z - forces = torch.zeros(1, len(body_ids), 3, device=device) - torques = torch.zeros(1, len(body_ids), 3, device=device) - torques[..., 2] = TORQUE_MAGNITUDE - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - # Phase 1: run N_STEPS - for _ in range(N_STEPS): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - omega_z_after_phase1 = cube_object.data.root_ang_vel_w.torch[0, 2].clone().item() - - # Rotate body 90deg about X and zero out velocities so phase 2 starts from rest - # (avoids gyroscopic cross-coupling at high omega) - root_pose = cube_object.data.root_pose_w.torch[0].clone().unsqueeze(0) - root_pose[0, 3:7] = torch.tensor([0.7071, 0.0, 0.0, 0.7071], device=device) # 90deg about X (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - root_vel = cube_object.data.root_vel_w.torch.clone() - root_vel[0, :] = 0.0 - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # Phase 2: run N_STEPS from rest with different body orientation - for _ in range(N_STEPS): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - omega_z_after_phase2 = cube_object.data.root_ang_vel_w.torch[0, 2].clone().item() - - # Both phases start from rest — angular acceleration about Z should be the same - torch.testing.assert_close( - torch.tensor(omega_z_after_phase1), - torch.tensor(omega_z_after_phase2), - rtol=0.001, - atol=0.0001, + composer = RigidObject( + cfg=RigidObjectCfg( + prim_path="/World/Composer/Object", + spawn=spawn, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0), rot=_ROTATION_45_Z), ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_force_torque_after_translation(device): - """Test that global force torque updates dynamically when the body translates. - - Phase 1: Cube at (1,0,0). Global force F=(0,10,0) applied at explicit position (1,0,0). - stored_torque = cross((1,0,0), (0,10,0)) = (0,0,10) - correction = -cross((1,0,0), (0,10,0)) = (0,0,-10) - net torque = 0 → no rotation, only linear acceleration in +Y. - - Phase 2: Teleport cube to origin (0,0,0), zero velocity, don't re-apply force. - stored_torque = (0,0,10) (unchanged in buffer) - correction = -cross((0,0,0), (0,10,0)) = (0,0,0) - net torque = (0,0,10) → rotation about +Z. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, height=1.0, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Phase 1 setup: Move cube to (1, 0, 1) and apply force at (1, 0, 1) - root_pose = cube_object.data.root_pose_w.torch.clone() - root_pose[0, 0] = 1.0 # x = 1 - root_pose[0, 1] = 0.0 # y = 0 - root_pose[0, 2] = 1.0 # z = 1 - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity quat (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - root_vel = cube_object.data.root_vel_w.torch.clone() - root_vel[0, :] = 0.0 # zero velocity - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # Step once to let the state settle - sim.step() - cube_object.update(sim.cfg.dt) - - # Get current CoM position for the force application point - com_pos = cube_object.data.body_com_pos_w.torch[:, body_ids, :3].clone() - - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 1] = FORCE_MAGNITUDE # +Y force - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=com_pos, - body_ids=body_ids, - is_global=True, - ) - - # Phase 1: run 50 steps — force at CoM, expect no rotation - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - ang_vel_phase1 = cube_object.data.root_ang_vel_w.torch[0].clone() - lin_vel_phase1 = cube_object.data.root_lin_vel_w.torch[0].clone() - - # Should have linear velocity in +Y - assert lin_vel_phase1[1].item() > 0.1, f"Expected positive Y velocity, got {lin_vel_phase1[1].item()}" - - # Angular velocity should be ~0 (force applied at CoM → no torque) - assert abs(ang_vel_phase1[2].item()) < 0.1, ( - f"Expected ~0 Z angular velocity in phase 1, got {ang_vel_phase1[2].item()}" - ) - - # Phase 2: Teleport cube to origin, zero velocity, don't re-apply force - root_pose2 = cube_object.data.root_pose_w.torch.clone() - root_pose2[0, 0] = 0.0 # x = 0 - root_pose2[0, 1] = 0.0 - root_pose2[0, 2] = 1.0 # z = 1 - root_pose2[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose2) - - root_vel2 = cube_object.data.root_vel_w.torch.clone() - root_vel2[0, :] = 0.0 # zero velocity - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel2) - - # Step once to let state settle - sim.step() - cube_object.update(sim.cfg.dt) - - # Phase 2: run 50 steps — body at origin but stored torque = cross((1,0,1), (0,10,0)) = (-10,0,10) - # correction = -cross((0,0,1), (0,10,0)) = -(0,0,0 - but wait, z=1) - # Actually: stored = cross((com_x,com_y,com_z), (0,10,0)) - # After teleport: correction = -cross(new_pos, F), net torque ≠ 0 since positions differ - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - ang_vel_phase2 = cube_object.data.root_ang_vel_w.torch[0].clone() - - # The X component of position changed from ~1 to ~0, so torque about Z changes. - # stored_torque_z = com_x * Fy = ~1 * 10 = ~10 - # After teleport, correction_z = -new_x * Fy = ~0 * 10 = ~0 - # net torque_z ≈ 10 → positive Z angular velocity - assert ang_vel_phase2[2].item() > 0.5, ( - f"Expected positive Z angular velocity in phase 2, got {ang_vel_phase2[2].item()}" - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_force_torque_reverses_on_opposite_side(device): - """Test that dynamic correction produces correct torque sign depending on body position. - - Phase 1: Cube at (-1, 0, 1). Global F=(0, 10, 0) at world point P=(0, 0, 1). - net torque_z = cross(P - link_pos, F)_z = cross((1,0,0), (0,10,0))_z = +10 - → positive Z angular velocity - - Phase 2: Teleport cube to (+1, 0, 1), zero velocity, don't re-apply force. - net torque_z = cross(P - link_pos, F)_z = cross((-1,0,0), (0,10,0))_z = -10 - → negative Z angular velocity - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, height=1.0, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Move cube to (-1, 0, 1) - root_pose = cube_object.data.root_pose_w.torch.clone() - root_pose[0, 0] = -1.0 - root_pose[0, 1] = 0.0 - root_pose[0, 2] = 1.0 - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - root_vel = cube_object.data.root_vel_w.torch.clone() - root_vel[0, :] = 0.0 - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - sim.step() - cube_object.update(sim.cfg.dt) - - # Apply permanent global F=(0, 10, 0) at world point P=(0, 0, 1) - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 1] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - positions = torch.zeros(1, len(body_ids), 3, device=device) - positions[..., 2] = 1.0 # P = (0, 0, 1) - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=positions, - body_ids=body_ids, - is_global=True, - ) - - # Phase 1: run 50 steps — expect positive Z angular velocity - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - omega_z_phase1 = cube_object.data.root_ang_vel_w.torch[0, 2].item() - assert omega_z_phase1 > 0.1, f"Phase 1: expected positive omega_z, got {omega_z_phase1}" - - # Phase 2: Teleport cube to (+1, 0, 1), zero velocity - root_pose2 = cube_object.data.root_pose_w.torch.clone() - root_pose2[0, 0] = 1.0 - root_pose2[0, 1] = 0.0 - root_pose2[0, 2] = 1.0 - root_pose2[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose2) - - root_vel2 = cube_object.data.root_vel_w.torch.clone() - root_vel2[0, :] = 0.0 - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel2) - sim.step() - cube_object.update(sim.cfg.dt) - - # Phase 2: run 50 steps — expect negative Z angular velocity - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - omega_z_phase2 = cube_object.data.root_ang_vel_w.torch[0, 2].item() - assert omega_z_phase2 < -0.1, f"Phase 2: expected negative omega_z, got {omega_z_phase2}" - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_force_no_position_no_torque(device): - """Test that global force without positions produces no torque (applied at CoM). - - A body at (2, 0, 1) with global F=(0, 10, 0) and no positions should experience - only linear acceleration, no rotation. The force is applied at the body's CoM. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, height=1.0, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Move cube to (2, 0, 1) - root_pose = cube_object.data.root_pose_w.torch.clone() - root_pose[0, 0] = 2.0 - root_pose[0, 1] = 0.0 - root_pose[0, 2] = 1.0 - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - root_vel = cube_object.data.root_vel_w.torch.clone() - root_vel[0, :] = 0.0 - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - sim.step() - cube_object.update(sim.cfg.dt) - - # Apply global F=(0, 10, 0) WITHOUT positions → force at CoM, no torque - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 1] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - # Run 50 steps - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - omega_z = cube_object.data.root_ang_vel_w.torch[0, 2].item() - # No positions → force at CoM → zero torque → zero angular velocity - assert abs(omega_z) < 0.01, f"Expected ~zero omega_z for force at CoM, got {omega_z}" - - # Should still have linear acceleration in +Y - lin_vel_y = cube_object.data.root_lin_vel_w.torch[0, 1].item() - assert lin_vel_y > 0.1, f"Expected positive Y velocity from applied force, got {lin_vel_y}" - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_multi_cube_different_torques_from_same_force(device): - """Test kernel indexing across multiple envs with different CoM positions. - - 2 cubes: Cube 0 at (-1, 0, 1), Cube 1 at (+1, 0, 1). - Same global F=(0, 10, 0) at same world point P=(0, 0, 1) to both cubes. - Cube 0: torque_z = cross((1,0,0), (0,10,0))_z = +10 → omega_z > 0 - Cube 1: torque_z = cross((-1,0,0), (0,10,0))_z = -10 → omega_z < 0 - Both have same linear acceleration in +Y. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=2, height=1.0, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Position cubes: Cube 0 at (-1, 0, 1), Cube 1 at (+1, 0, 1) - root_pose = cube_object.data.root_pose_w.torch.clone() - root_pose[0, 0] = -1.0 - root_pose[0, 1] = 0.0 - root_pose[0, 2] = 1.0 - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - - root_pose[1, 0] = 1.0 - root_pose[1, 1] = 0.0 - root_pose[1, 2] = 1.0 - root_pose[1, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - root_vel = cube_object.data.root_vel_w.torch.clone() - root_vel[:, :] = 0.0 - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - sim.step() - cube_object.update(sim.cfg.dt) - - # Apply same global F=(0, 10, 0) at P=(0, 0, 1) to both cubes - forces = torch.zeros(2, len(body_ids), 3, device=device) - forces[..., 1] = FORCE_MAGNITUDE - torques = torch.zeros(2, len(body_ids), 3, device=device) - positions = torch.zeros(2, len(body_ids), 3, device=device) - positions[..., 2] = 1.0 # P = (0, 0, 1) - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=positions, - body_ids=body_ids, - is_global=True, - ) - - # Run 50 steps - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - # Cube 0: omega_z > 0 (force point is to the right of CoM) - omega_z_0 = cube_object.data.root_ang_vel_w.torch[0, 2].item() - assert omega_z_0 > 0.1, f"Cube 0: expected positive omega_z, got {omega_z_0}" - - # Cube 1: omega_z < 0 (force point is to the left of CoM) - omega_z_1 = cube_object.data.root_ang_vel_w.torch[1, 2].item() - assert omega_z_1 < -0.1, f"Cube 1: expected negative omega_z, got {omega_z_1}" - - # Both cubes should have same linear velocity in +Y (same force magnitude) - lin_vel_y_0 = cube_object.data.root_lin_vel_w.torch[0, 1].item() - lin_vel_y_1 = cube_object.data.root_lin_vel_w.torch[1, 1].item() - assert abs(lin_vel_y_0 - lin_vel_y_1) < 0.5, ( - f"Both cubes should have similar Y velocity, got {lin_vel_y_0} and {lin_vel_y_1}" + ) + raw = RigidObject( + cfg=RigidObjectCfg( + prim_path="/World/Raw/Object", + spawn=spawn, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 3.0, 1.0), rot=_ROTATION_45_Z), ) + ) + return composer, raw -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_global_force_torque_far_from_origin(device): - """Test that global force torque correction produces correct physics at large world coordinates. - - Two cubes with identical relative geometry (force offset = (1, 0, 0) from CoM): - Cube 0 at (0, 0, 1) — near origin (reference) - Cube 1 at (2000, 0, 1) — far from origin - - Both get global F=(0, 10, 0) at offset (1, 0, 0) from their respective CoMs. - Expected torque: cross((1,0,0), (0,10,0)) = (0, 0, 10) for both. - - The compose kernel computes cross(P, F) - cross(link_pos, F): - Cube 0: cross((1,0,1), F) - cross((0,0,1), F) — small values, no cancellation - Cube 1: cross((2001,0,1), F) - cross((2000,0,1), F) — large values nearly cancel - - Both cubes should produce the same angular and linear velocities. - """ +@pytest.mark.skipif(not torch.cuda.is_available(), reason="PhysX wrench delivery requires CUDA-pinned staging") +def test_rotated_global_force_at_position_matches_physx_delivery() -> None: + """Deliver a rotated global force at an offset and match raw PhysX velocity changes.""" + device = "cuda:0" with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=2, height=1.0, device=device) - + composer, raw = _make_dual_cube_scene(device) sim.reset() - body_ids, _ = cube_object.find_bodies(".*") - - # Position cubes: Cube 0 near origin, Cube 1 far from origin - root_pose = cube_object.data.root_pose_w.torch.clone() - # Cube 0 at (0, 0, 1) - root_pose[0, 0] = 0.0 - root_pose[0, 1] = 0.0 - root_pose[0, 2] = 1.0 - root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - # Cube 1 at (2000, 0, 1) - root_pose[1, 0] = 2000.0 - root_pose[1, 1] = 0.0 - root_pose[1, 2] = 1.0 - root_pose[1, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - - root_vel = cube_object.data.root_vel_w.torch.clone() - root_vel[:, :] = 0.0 - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - sim.step() - cube_object.update(sim.cfg.dt) - - # Apply F=(0, 10, 0) at +1m X offset from each cube's CoM - forces = torch.zeros(2, len(body_ids), 3, device=device) - forces[..., 1] = FORCE_MAGNITUDE # +Y force - torques = torch.zeros(2, len(body_ids), 3, device=device) - - # Positions: each cube's CoM + (1, 0, 0) - com_pos = cube_object.data.body_com_pos_w.torch[:, body_ids, :3].clone() - positions = com_pos.clone() - positions[..., 0] += 1.0 # +1m X offset from CoM - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=positions, + body_ids, _ = composer.find_bodies(".*") + force = torch.tensor([[[0.0, 0.0, 10.0]]], device=device) + torque = torch.zeros_like(force) + world_offset = torch.tensor([[[0.0, 1.0, 0.0]]], device=device) + composer_position = composer.data.body_com_pos_w.torch[:, body_ids, :3] + world_offset + raw_position = raw.data.body_com_pos_w.torch[:, body_ids, :3] + world_offset + + composer.permanent_wrench_composer.set_forces_and_torques_index( + forces=force, + torques=torque, + positions=composer_position, body_ids=body_ids, is_global=True, ) - # Run 50 steps - for _ in range(50): - cube_object.write_data_to_sim() + for _ in range(4): + composer.write_data_to_sim() + raw.root_view.apply_forces_and_torques_at_position( + force_data=wp.from_torch(force.view(-1, 3).contiguous(), dtype=wp.float32), + torque_data=wp.from_torch(torque.view(-1, 3).contiguous(), dtype=wp.float32), + position_data=wp.from_torch(raw_position.view(-1, 3).contiguous(), dtype=wp.float32), + indices=raw._ALL_INDICES, + is_global=True, + ) sim.step() - cube_object.update(sim.cfg.dt) + composer.update(sim.cfg.dt) + raw.update(sim.cfg.dt) - # Both cubes should have positive omega_z (cross((1,0,0), (0,10,0)) = (0,0,10)) - omega_z_0 = cube_object.data.root_ang_vel_w.torch[0, 2].item() - omega_z_1 = cube_object.data.root_ang_vel_w.torch[1, 2].item() - assert omega_z_0 > 0.1, f"Cube 0: expected positive omega_z, got {omega_z_0}" - assert omega_z_1 > 0.1, f"Cube 1: expected positive omega_z, got {omega_z_1}" - - # omega_z values should match within 1% (same relative geometry) torch.testing.assert_close( - torch.tensor(omega_z_0), - torch.tensor(omega_z_1), - rtol=0.01, - atol=0.0, - msg=lambda msg: ( - f"Angular velocity mismatch between near-origin and far-from-origin cubes:\n" - f" Cube 0 (near): omega_z = {omega_z_0:.6f}\n" - f" Cube 1 (far): omega_z = {omega_z_1:.6f}\n{msg}" - ), + composer.data.root_lin_vel_w.torch, raw.data.root_lin_vel_w.torch, rtol=1.0e-4, atol=1.0e-4 ) - - # Linear velocity in +Y should also match - lin_vel_y_0 = cube_object.data.root_lin_vel_w.torch[0, 1].item() - lin_vel_y_1 = cube_object.data.root_lin_vel_w.torch[1, 1].item() torch.testing.assert_close( - torch.tensor(lin_vel_y_0), - torch.tensor(lin_vel_y_1), - rtol=0.01, - atol=0.0, - msg=lambda msg: ( - f"Linear velocity mismatch between near-origin and far-from-origin cubes:\n" - f" Cube 0 (near): lin_vel_y = {lin_vel_y_0:.6f}\n" - f" Cube 1 (far): lin_vel_y = {lin_vel_y_1:.6f}\n{msg}" - ), + composer.data.root_ang_vel_w.torch, raw.data.root_ang_vel_w.torch, rtol=1.0e-4, atol=1.0e-4 ) - - -@pytest.mark.parametrize("device", ["cuda:0"]) -def test_global_force_no_position_no_rotation_large_offset(device): - """Test that a global force without positions produces no rotation at large offsets. - - A cube is placed at (2000, 0, 1) and a global force F=(0, 10, 0) is applied - without positions. The cube should accelerate linearly but not rotate. - Before the fix, this would produce torque proportional to 2000 and cause rotation. - """ - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False - ) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, height=1.0, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Place cube at large X offset - root_pose = cube_object.data.default_root_pose.torch.clone() - root_pose[0, 0] = 2000.0 # large X position - root_pose[0, 1] = 0.0 - root_pose[0, 2] = 1.0 - 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) - cube_object.reset() - - # Apply global force without positions (should go to CoM, no torque) - forces = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=device) - forces[0, :, 1] = 10.0 # F_y = 10 N - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - body_ids=body_ids, - is_global=True, - ) - - # Step simulation - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - # Check: angular velocity should be near zero (no rotation) - ang_vel = cube_object.data.root_ang_vel_w.torch[0] - assert torch.allclose(ang_vel, torch.zeros(3, device=device), atol=0.01), ( - f"Expected near-zero angular velocity, got {ang_vel}. " - "Global force without positions should not produce torque." - ) - - # Check: linear velocity in Y should be positive (force is in +Y) - lin_vel = cube_object.data.root_lin_vel_w.torch[0] - assert lin_vel[1] > 0.1, f"Expected positive Y velocity from applied force, got {lin_vel[1]}" - - -@pytest.mark.parametrize("device", ["cuda:0"]) -def test_global_force_at_com_position_no_rotation_large_offset(device): - """Test that a global force with position at CoM produces no rotation at large offsets. - - A cube is placed at (2000, 0, 1) and a global force F=(0, 10, 0) is applied - at the cube's position (i.e., at its CoM). This should produce zero torque, - serving as a control test alongside test_global_force_no_position_no_rotation_large_offset. - """ - with build_simulation_context( - device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False - ) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=1, height=1.0, device=device) - - sim.reset() - - body_ids, _ = cube_object.find_bodies(".*") - - # Place cube at large X offset - root_pose = cube_object.data.default_root_pose.torch.clone() - root_pose[0, 0] = 2000.0 - root_pose[0, 1] = 0.0 - root_pose[0, 2] = 1.0 - 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) - cube_object.reset() - - # Apply global force AT the cube's position (torque should cancel) - forces = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=device) - forces[0, :, 1] = 10.0 - - positions = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=device) - positions[0, :, 0] = 2000.0 - positions[0, :, 2] = 1.0 - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - positions=positions, - body_ids=body_ids, - is_global=True, - ) - - for _ in range(50): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - - # Force at CoM → no rotation - ang_vel = cube_object.data.root_ang_vel_w.torch[0] - assert torch.allclose(ang_vel, torch.zeros(3, device=device), atol=0.01), ( - f"Expected near-zero angular velocity, got {ang_vel}. " - "Global force at CoM position should not produce torque." - ) - - lin_vel = cube_object.data.root_lin_vel_w.torch[0] - assert lin_vel[1] > 0.1, f"Expected positive Y velocity from applied force, got {lin_vel[1]}" + assert torch.abs(composer.data.root_ang_vel_w.torch[0, :2]).max().item() > 0.01 diff --git a/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py b/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py deleted file mode 100644 index f186993d3b8a..000000000000 --- a/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py +++ /dev/null @@ -1,849 +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 - -"""Integration tests comparing WrenchComposer output vs raw PhysX apply_forces_and_torques_at_position. - -Two identical rigid objects are placed in the same scene. One uses the WrenchComposer path -(set_forces_and_torques → write_data_to_sim → compose → PhysX apply with is_global=False), -the other uses the raw PhysX API directly (apply_forces_and_torques_at_position with matching -is_global flag). After N steps, both objects should have identical velocities. -""" - -"""Launch Isaac Sim Simulator first.""" - -from isaaclab.app import AppLauncher - -# launch omniverse app -simulation_app = AppLauncher(headless=True).app - -"""Rest everything follows.""" - -import math - -import pytest -import torch -import warp as wp - -import isaaclab.sim as sim_utils -from isaaclab.assets import RigidObject, RigidObjectCfg -from isaaclab.sim import build_simulation_context -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR - -pytestmark = pytest.mark.integration - - -def generate_dual_cube_scene( - num_cubes: int = 1, - height: float = 1.0, - device: str = "cuda:0", - initial_rot: tuple[float, ...] | None = None, - spacing: float = 2.0, -) -> tuple[RigidObject, RigidObject]: - """Generate a scene with two sets of cubes: one for the composer path, one for raw PhysX. - - Both sets share the same spawn config and initial state (except a Y offset to avoid overlap). - - Args: - num_cubes: Number of cubes per group (environments). - height: Spawn height. - device: Simulation device. - initial_rot: Initial quaternion (x, y, z, w). Defaults to identity. - spacing: Distance between env origins in X. Defaults to 2.0. - - Returns: - Tuple of (cube_composer, cube_raw) RigidObject instances. - """ - if initial_rot is None: - initial_rot = (0.0, 0.0, 0.0, 1.0) # identity in (x,y,z,w) - - y_offset = max(spacing, 3.0) - - # Create Xform prims for both groups - for i in range(num_cubes): - origin_composer = (i * spacing, 0.0, height) - origin_raw = (i * spacing, y_offset, height) # Y offset to avoid overlap - sim_utils.create_prim(f"/World/Composer_{i}", "Xform", translation=origin_composer) - sim_utils.create_prim(f"/World/Raw_{i}", "Xform", translation=origin_raw) - - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - ) - - cube_composer_cfg = RigidObjectCfg( - prim_path="/World/Composer_[^/]*/Object", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height), rot=initial_rot), - ) - cube_composer = RigidObject(cfg=cube_composer_cfg) - - cube_raw_cfg = RigidObjectCfg( - prim_path="/World/Raw_[^/]*/Object", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, y_offset, height), rot=initial_rot), - ) - cube_raw = RigidObject(cfg=cube_raw_cfg) - - return cube_composer, cube_raw - - -N_STEPS = 50 -FORCE_MAGNITUDE = 10.0 -TORQUE_MAGNITUDE = 1.0 -# 45 degrees about Z: (cos(22.5°), 0, 0, sin(22.5°)) -ROT_45_Z = (0.0, 0.0, math.sin(math.pi / 8), math.cos(math.pi / 8)) # 45deg about Z in (x,y,z,w) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_local_force(device): - """Baseline: local force at identity orientation. Composer and raw PhysX should match exactly.""" - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Composer path: local force +X - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=False, - ) - - # Raw PhysX data (flattened for PhysX view API) - raw_forces = torch.zeros(1, 3, device=device) - raw_forces[:, 0] = FORCE_MAGNITUDE - raw_torques = torch.zeros(1, 3, device=device) - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() # no-op (composer inactive) - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=False, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Compare velocities - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # Both should have ~zero angular velocity (force at CoM, no torque) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - torch.zeros(1, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - torch.testing.assert_close( - cube_raw.data.root_ang_vel_w.torch, - torch.zeros(1, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_global_force(device): - """Global force with non-identity rotation (45 deg Z). Rotation matters for frame conversion.""" - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device, initial_rot=ROT_45_Z) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Composer path: global force +X - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - # Raw PhysX data - raw_forces = torch.zeros(1, 3, device=device) - raw_forces[:, 0] = FORCE_MAGNITUDE - raw_torques = torch.zeros(1, 3, device=device) - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Linear velocities should match (same global force, same mass) - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # Angular velocities should match - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # Both should have ~zero angular velocity (force at CoM, no torque) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - torch.zeros(1, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - torch.testing.assert_close( - cube_raw.data.root_ang_vel_w.torch, - torch.zeros(1, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_local_force_at_position(device): - """Local force at a local offset. Both paths should produce identical cross-product torque.""" - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Local force +X at local offset +0.5m Y - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - positions = torch.zeros(1, len(body_ids), 3, device=device) - positions[..., 1] = 0.5 # +0.5m Y offset in local frame - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=positions, - body_ids=body_ids, - is_global=False, - ) - - # Raw PhysX data (local force at local position) - raw_forces = torch.zeros(1, 3, device=device) - raw_forces[:, 0] = FORCE_MAGNITUDE - raw_torques = torch.zeros(1, 3, device=device) - raw_positions = torch.zeros(1, 3, device=device) - raw_positions[:, 1] = 0.5 - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=wp.from_torch(raw_positions.contiguous(), dtype=wp.float32), - indices=raw_indices, - is_global=False, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Both linear and angular velocities should match - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - - # Sanity: angular velocity should be nonzero (cross-product torque) - assert torch.abs(cube_composer.data.root_ang_vel_w.torch[0, 2]).item() > 0.1, ( - "Expected nonzero Z angular velocity from cross-product torque" - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_global_force_at_position(device): - """Global force at world position with non-identity rotation. Both rotation AND position correction matter.""" - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device, initial_rot=ROT_45_Z) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Global force +X - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - # Position = each cube's link_pos + offset (same offset for both) - offset = torch.zeros(1, len(body_ids), 3, device=device) - offset[..., 1] = 1.0 # +1m Y offset in world frame - - pos_composer = cube_composer.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset - pos_raw = cube_raw.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=pos_composer, - body_ids=body_ids, - is_global=True, - ) - - # Raw PhysX data - raw_forces = torch.zeros(1, 3, device=device) - raw_forces[:, 0] = FORCE_MAGNITUDE - raw_torques = torch.zeros(1, 3, device=device) - raw_positions = pos_raw.view(-1, 3) - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=wp.from_torch(raw_positions.contiguous(), dtype=wp.float32), - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Both linear and angular velocities should match - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - - # Sanity: angular velocity should be nonzero (cross-product torque) - assert torch.abs(cube_composer.data.root_ang_vel_w.torch[0, 2]).item() > 0.1, ( - "Expected nonzero Z angular velocity from positional torque" - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_local_torque(device): - """Local torque at identity orientation. Should produce matching angular velocity.""" - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Composer path: local torque about +Z - forces = torch.zeros(1, len(body_ids), 3, device=device) - torques = torch.zeros(1, len(body_ids), 3, device=device) - torques[..., 2] = TORQUE_MAGNITUDE - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=False, - ) - - # Raw PhysX data - raw_forces = torch.zeros(1, 3, device=device) - raw_torques = torch.zeros(1, 3, device=device) - raw_torques[:, 2] = TORQUE_MAGNITUDE - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=False, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Angular velocities should match - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # Linear velocity should be ~zero for both (no force) - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - torch.zeros(1, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - torch.testing.assert_close( - cube_raw.data.root_lin_vel_w.torch, - torch.zeros(1, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_global_torque(device): - """Global torque with non-identity rotation (45 deg Z). Composer rotates to body frame internally.""" - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device, initial_rot=ROT_45_Z) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Composer path: global torque about +Z - forces = torch.zeros(1, len(body_ids), 3, device=device) - torques = torch.zeros(1, len(body_ids), 3, device=device) - torques[..., 2] = TORQUE_MAGNITUDE - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - # Raw PhysX data - raw_forces = torch.zeros(1, 3, device=device) - raw_torques = torch.zeros(1, 3, device=device) - raw_torques[:, 2] = TORQUE_MAGNITUDE - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Angular velocities should match - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - - -NUM_CUBES_MULTI = 4 - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_global_force_multi_env(device): - """Global force (no position) with multiple environments. - - Regression: checks that env-indexing and per-body quaternion handling work correctly - when there is more than one environment. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene( - num_cubes=NUM_CUBES_MULTI, device=device, initial_rot=ROT_45_Z - ) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Composer path: global force +X for all envs - forces = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - # Raw PhysX data (one row per env) - raw_forces = torch.zeros(NUM_CUBES_MULTI, 3, device=device) - raw_forces[:, 0] = FORCE_MAGNITUDE - raw_torques = torch.zeros(NUM_CUBES_MULTI, 3, device=device) - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Linear velocities should match across all envs - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # Angular velocities should match - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # All envs should have ~zero angular velocity - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - torch.zeros(NUM_CUBES_MULTI, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_global_force_with_reset(device): - """Global force (no position) with a mid-simulation reset of half the envs. - - Regression: after reset the permanent wrench is cleared. Re-setting it should - produce correct behavior even though the object state was just reset. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene( - num_cubes=NUM_CUBES_MULTI, device=device, initial_rot=ROT_45_Z, spacing=20.0 - ) - - sim.reset() - - # Capture initial world-frame state (includes env origin offsets) - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - initial_state_composer = torch.cat( - [ - cube_composer.data.root_link_pos_w.torch, - cube_composer.data.root_link_quat_w.torch, - cube_composer.data.root_com_vel_w.torch, - ], - dim=-1, - ).clone() - initial_state_raw = torch.cat( - [ - cube_raw.data.root_link_pos_w.torch, - cube_raw.data.root_link_quat_w.torch, - cube_raw.data.root_com_vel_w.torch, - ], - dim=-1, - ).clone() - - body_ids, _ = cube_composer.find_bodies(".*") - - def apply_global_force(): - """Set the same global +X force on the composer cube.""" - forces = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) - forces[..., 0] = FORCE_MAGNITUDE - torques = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - apply_global_force() - - # Raw PhysX data - raw_forces = torch.zeros(NUM_CUBES_MULTI, 3, device=device) - raw_forces[:, 0] = FORCE_MAGNITUDE - raw_torques = torch.zeros(NUM_CUBES_MULTI, 3, device=device) - raw_indices = cube_raw._ALL_INDICES - - # Phase 1: run N_STEPS / 2 - half = N_STEPS // 2 - for _ in range(half): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Reset first half of envs on both cubes - reset_ids = list(range(NUM_CUBES_MULTI // 2)) - reset_ids_torch = torch.tensor(reset_ids, dtype=torch.long, device=device) - - # Reset root state using captured world-frame initial state (includes env origins) - cube_composer.write_root_link_pose_to_sim_index( - root_pose=initial_state_composer[reset_ids_torch, :7], env_ids=reset_ids_torch - ) - cube_composer.write_root_com_velocity_to_sim_index( - root_velocity=initial_state_composer[reset_ids_torch, 7:], env_ids=reset_ids_torch - ) - cube_raw.write_root_link_pose_to_sim_index( - root_pose=initial_state_raw[reset_ids_torch, :7], env_ids=reset_ids_torch - ) - cube_raw.write_root_com_velocity_to_sim_index( - root_velocity=initial_state_raw[reset_ids_torch, 7:], env_ids=reset_ids_torch - ) - - cube_composer.reset(reset_ids) - cube_raw.reset(reset_ids) - - # Re-apply the force (reset cleared the permanent wrench) - apply_global_force() - - # Phase 2: run N_STEPS / 2 more - for _ in range(half): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # All envs: composer vs raw should match - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - # All envs should have ~zero angular velocity - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - torch.zeros(NUM_CUBES_MULTI, 3, device=device), - rtol=0.0, - atol=1e-4, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_payload_scenario(device): - """Mirrors the apply_payload MDP: permanent global downward force at CoM with gravity. - - A constant world-frame downward force (payload weight) is applied via the composer - path vs raw PhysX. The body falls under gravity + payload, contacts the ground, and - orientation changes. The composer does a world->body->world round-trip each step; - this test catches any precision drift from that. - """ - with build_simulation_context(device=device, gravity_enabled=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene( - num_cubes=1, height=0.5, device=device, initial_rot=ROT_45_Z, spacing=20.0 - ) - - sim.reset() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Record initial positions to compare displacements (cubes spawn at different Y) - init_pos_composer = cube_composer.data.root_pos_w.torch.clone() - init_pos_raw = cube_raw.data.root_pos_w.torch.clone() - - body_ids, _ = cube_composer.find_bodies(".*") - - payload_force = 2.0 * 9.81 - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 2] = -payload_force - torques = torch.zeros(1, len(body_ids), 3, device=device) - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - body_ids=body_ids, - is_global=True, - ) - - raw_forces = torch.zeros(1, 3, device=device) - raw_forces[:, 2] = -payload_force - raw_torques = torch.zeros(1, 3, device=device) - raw_indices = cube_raw._ALL_INDICES - - for _ in range(N_STEPS): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=None, - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - # Compare displacements (not absolute positions — cubes have different spawn Y) - disp_composer = cube_composer.data.root_pos_w.torch - init_pos_composer - disp_raw = cube_raw.data.root_pos_w.torch - init_pos_raw - - torch.testing.assert_close(disp_composer, disp_raw, rtol=1e-4, atol=1e-4) - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-4, - atol=1e-4, - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_composer_vs_physx_permanent_global_force_at_position_long_run(device): - """Permanent global force at a world-frame offset, run long enough for significant body motion. - - This test catches temporal drift bugs where the stored positional torque diverges from - what PhysX computes each step as the body moves. The force is large enough that the body - translates and rotates significantly over 100 steps, but not so large that it causes - numerical instability. - """ - with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_composer, cube_raw = generate_dual_cube_scene(num_cubes=1, device=device, initial_rot=ROT_45_Z) - - sim.reset() - - body_ids, _ = cube_composer.find_bodies(".*") - - # Global force +Z at +1m Y offset from CoM — produces torque around X - forces = torch.zeros(1, len(body_ids), 3, device=device) - forces[..., 2] = FORCE_MAGNITUDE - torques = torch.zeros(1, len(body_ids), 3, device=device) - - offset = torch.zeros(1, len(body_ids), 3, device=device) - offset[..., 1] = 1.0 - - pos_composer = cube_composer.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset - pos_raw = cube_raw.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset - - cube_composer.permanent_wrench_composer.set_forces_and_torques_index( - forces=forces, - torques=torques, - positions=pos_composer, - body_ids=body_ids, - is_global=True, - ) - - raw_forces = torch.zeros(1, 3, device=device) - raw_forces[:, 2] = FORCE_MAGNITUDE - raw_torques = torch.zeros(1, 3, device=device) - raw_positions = pos_raw.view(-1, 3) - raw_indices = cube_raw._ALL_INDICES - - for _ in range(100): - cube_composer.write_data_to_sim() - cube_raw.write_data_to_sim() - cube_raw.root_view.apply_forces_and_torques_at_position( - force_data=wp.from_torch(raw_forces.contiguous(), dtype=wp.float32), - torque_data=wp.from_torch(raw_torques.contiguous(), dtype=wp.float32), - position_data=wp.from_torch(raw_positions.contiguous(), dtype=wp.float32), - indices=raw_indices, - is_global=True, - ) - sim.step() - cube_composer.update(sim.cfg.dt) - cube_raw.update(sim.cfg.dt) - - torch.testing.assert_close( - cube_composer.data.root_lin_vel_w.torch, - cube_raw.data.root_lin_vel_w.torch, - rtol=1e-3, - atol=1e-3, - ) - torch.testing.assert_close( - cube_composer.data.root_ang_vel_w.torch, - cube_raw.data.root_ang_vel_w.torch, - rtol=1e-3, - atol=1e-3, - ) - - # Sanity: angular velocity should be nonzero - assert torch.abs(cube_composer.data.root_ang_vel_w.torch).max().item() > 0.1, ( - "Expected nonzero angular velocity from positional torque over 100 steps" - ) diff --git a/source/isaaclab_newton/changelog.d/asset-tests-redesign.skip b/source/isaaclab_newton/changelog.d/asset-tests-redesign.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_newton/test/articulation_test_utils.py b/source/isaaclab_newton/test/articulation_test_utils.py new file mode 100644 index 000000000000..a4dad0a52f1c --- /dev/null +++ b/source/isaaclab_newton/test/articulation_test_utils.py @@ -0,0 +1,90 @@ +# 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 + +"""Local articulation authoring helpers for kitless Newton integration tests.""" + +from collections.abc import Sequence + +from isaaclab_newton.assets import Articulation +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg + +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.sim import SimulationCfg, build_simulation_context + + +def build_newton_context(*, gravity: tuple[float, float, float] = (0.0, 0.0, 0.0)): + """Create a fresh CPU Newton simulation context.""" + return build_simulation_context( + sim_cfg=SimulationCfg( + device="cpu", + dt=1.0 / 120.0, + gravity=gravity, + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg(), use_cuda_graph=False), + ) + ) + + +def author_fixed_spatial_chain( + *, + actuators: dict | None = None, + prim_paths: Sequence[str] = ("/World/Robot",), + prim_path_expr: str | None = None, +) -> Articulation: + """Author one or more fixed chains with full-rank spatial Jacobians.""" + link_cfg = sim_utils.CuboidCfg( + size=(0.08, 0.08, 0.08), + rigid_props=sim_utils.RigidBodyBaseCfg(disable_gravity=False), + mass_props=sim_utils.MassPropertiesCfg(mass=0.25), + collision_props=sim_utils.CollisionBaseCfg(collision_enabled=False), + ) + stage = sim_utils.get_current_stage() + axes: Sequence[str] = ("X", "Y", "Z", "X", "Y", "Z") + for robot_path in prim_paths: + root_path = f"{robot_path}/Root" + sim_utils.create_prim(robot_path, "Xform") + link_cfg.func(root_path, link_cfg, translation=(0.0, 0.0, 1.0)) + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath(root_path)) + fixed_joint = UsdPhysics.FixedJoint.Define(stage, f"{robot_path}/RootJoint") + fixed_joint.CreateBody1Rel().SetTargets([root_path]) + + parent_path = root_path + for joint_index, axis in enumerate(axes): + child_path = f"{robot_path}/Link_{joint_index}" + link_cfg.func(child_path, link_cfg, translation=(0.0, 0.0, 1.0)) + joint_path = f"{robot_path}/Joint_{joint_index}" + if joint_index < 3: + joint = UsdPhysics.PrismaticJoint.Define(stage, joint_path) + joint.CreateLowerLimitAttr().Set(-0.2) + joint.CreateUpperLimitAttr().Set(0.2) + else: + joint = UsdPhysics.RevoluteJoint.Define(stage, joint_path) + joint.CreateLowerLimitAttr().Set(-45.0) + joint.CreateUpperLimitAttr().Set(45.0) + joint.CreateBody0Rel().SetTargets([parent_path]) + joint.CreateBody1Rel().SetTargets([child_path]) + joint.CreateAxisAttr().Set(axis) + parent_path = child_path + + return Articulation( + ArticulationCfg( + prim_path=prim_path_expr or prim_paths[0], + articulation_root_prim_path="/Root", + actuators=( + actuators + if actuators is not None + else { + "joints": ImplicitActuatorCfg( + joint_names_expr=["Joint_.*"], + stiffness=80.0, + damping=8.0, + ) + } + ), + ) + ) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index e45d1e96b1de..91010bd3b0f9 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3,4784 +3,257 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none +"""Kitless real-Newton articulation coverage on one module-scoped scene.""" -"""Launch Isaac Sim Simulator first.""" +from dataclasses import dataclass -from isaaclab.app import AppLauncher -from isaaclab.test.utils import DeviceScope, resolve_test_sim_device, test_devices -from isaaclab.test.utils.articulation_ordering import ( - ANYMAL_C_PHYSX_JOINT_NAMES, - BRANCHING_MJWARP_BODY_NAMES, - BRANCHING_MJWARP_JOINT_NAMES, - BRANCHING_PHYSX_BODY_NAMES, - BRANCHING_PHYSX_JOINT_NAMES, - PANDA_JOINT_NAMES, - PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES, -) - -HEADLESS = True - -# launch omniverse app -simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app - -"""Rest everything follows.""" - -import sys -from copy import copy, deepcopy -from pathlib import Path -from types import SimpleNamespace - -import numpy as np import pytest import torch -import warp as wp 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 newton import JointTargetMode, JointType, ModelBuilder, ModelFlags -from newton.solvers import SolverMuJoCo +from newton import ModelFlags -from pxr import UsdPhysics +from pxr import Gf, 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 -import isaaclab.utils.string as string_utils -from isaaclab.actuators import ( - IdealPDActuatorCfg, - ImplicitActuator, - ImplicitActuatorCfg, -) +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg -from isaaclab.assets.articulation.ordering_resolvers import get_articulation_name_ordering -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.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.version import get_isaac_sim_version, has_kit -from isaaclab.utils.warp.proxy_array import ProxyArray - -## -# Pre-defined configs -## -from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG # isort:skip -# , SHADOW_HAND_CFG # isort:skip - - -SIM_CFGs = { - "humanoid": SimulationCfg( - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=80, - nconmax=25, - ls_iterations=20, - cone="pyramidal", - update_data_interval=2, - integrator="implicitfast", - impratio=1, - ), - num_substeps=2, - debug_mode=False, - ), - ), - "anymal": SimulationCfg( - dt=1 / 200, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=70, - nconmax=70, - ls_iterations=40, - cone="elliptic", - impratio=100, - integrator="implicitfast", - ), - num_substeps=2, - debug_mode=True, - ), - ), - "panda": SimulationCfg( - dt=1 / 120, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=20, - nconmax=20, - ls_iterations=20, - cone="pyramidal", - impratio=1, - integrator="implicitfast", - ), - num_substeps=1, - debug_mode=False, - ), - ), - # "panda" with 4 solver substeps: at a single 1/120 substep, MJWarp implicitfast - # carries a ~0.4-1.0 mm integration limit cycle under a gravity load that never - # settles; 4 substeps integrate the same frozen per-control-step torques to a - # dead-still ~1 um hold. Used by the gravity-compensation precision test. - "panda_fine": SimulationCfg( - dt=1 / 120, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=20, - nconmax=20, - ls_iterations=20, - cone="pyramidal", - impratio=1, - integrator="implicitfast", - ), - num_substeps=4, - debug_mode=False, - ), - ), - "single_joint_implicit": SimulationCfg( - dt=1 / 120, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=20, - nconmax=20, - ls_iterations=20, - cone="pyramidal", - impratio=1, - integrator="implicitfast", - ), - num_substeps=1, - debug_mode=False, - ), - ), - "single_joint_explicit": SimulationCfg( - dt=1 / 120, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=20, - nconmax=20, - ls_iterations=20, - cone="pyramidal", - impratio=1, - integrator="implicitfast", - ), - num_substeps=1, - debug_mode=False, - ), - ), - "shadow_hand": SimulationCfg( - dt=1 / 120, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=70, - nconmax=70, - ls_iterations=40, - cone="elliptic", - impratio=100, - integrator="implicitfast", - ), - num_substeps=2, - debug_mode=True, - ), - ), -} - - -class CustomDrive(ImplicitActuator): - """Implicit actuator with a class name that does not encode its execution type.""" - - -def generate_articulation_cfg( - articulation_type: str, - stiffness: float | None = 10.0, - damping: float | None = 2.0, - actuator_velocity_limit: float | None = None, - actuator_effort_limit: float | None = None, - joint_velocity_limit: float | None = None, - joint_effort_limit: float | None = None, -) -> ArticulationCfg: - """Generate an articulation configuration. - - Args: - articulation_type: Type of articulation to generate. - It should be one of: "humanoid", "panda", "anymal", "shadow_hand", "single_joint_implicit", - "single_joint_explicit". - stiffness: Stiffness value for the articulation's actuators. Only currently used for "humanoid". - Defaults to 10.0. - damping: Damping value for the articulation's actuators. Only currently used for "humanoid". - Defaults to 2.0. - actuator_velocity_limit: Velocity limit for the actuators. Only currently used for "single_joint_implicit" - and "single_joint_explicit". - actuator_effort_limit: Effort limit for explicit actuators. Only currently used for - "single_joint_explicit". - joint_velocity_limit: Velocity limit for the actuators (set into the simulation). - Only currently used for "single_joint_implicit" and "single_joint_explicit". - joint_effort_limit: Effort limit for the actuators (set into the simulation). - Only currently used for "single_joint_implicit" and "single_joint_explicit". - - Returns: - The articulation configuration for the requested articulation type. - - """ - if articulation_type == "humanoid": - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/Humanoid/humanoid_instanceable.usd" - ), - init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)), - actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, - ) - elif articulation_type == "panda": - articulation_cfg = FRANKA_PANDA_CFG - elif articulation_type == "anymal": - articulation_cfg = ANYMAL_C_CFG - elif articulation_type == "shadow_hand": - pytest.skip("Shadow hand is not supported in Newton") - # articulation_cfg = SHADOW_HAND_CFG - elif articulation_type == "single_joint_implicit": - articulation_cfg = ArticulationCfg( - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), - ), - actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - joint_effort_limit=joint_effort_limit, - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=actuator_velocity_limit, - stiffness=2000.0, - damping=100.0, - ), - }, - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos=({"RevoluteJoint": 1.5708}), - rot=(0.7071081, 0, 0, 0.7071055), - ), - ) - elif articulation_type == "single_joint_explicit": - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), - ), - actuators={ - "joint": IdealPDActuatorCfg( - joint_names_expr=[".*"], - joint_effort_limit=joint_effort_limit, - joint_velocity_limit=joint_velocity_limit, - actuator_effort_limit=actuator_effort_limit, - actuator_velocity_limit=actuator_velocity_limit, - stiffness=0.0, - damping=10.0, - ), - }, - ) - elif articulation_type == "spatial_tendon_test_asset": - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/IsaacLab/Tests/spatial_tendons.usd", - ), +from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context + +from source.isaaclab_newton.test.articulation_test_utils import author_fixed_spatial_chain + +pytestmark = pytest.mark.integration + + +@dataclass +class _ArticulationScene: + """Articulations that share one real Newton model and solver lifecycle.""" + + sim: SimulationContext + floating: Articulation + fixed: Articulation + device: str + + +def _newton_sim_context(*, device: str): + """Create one kitless Newton simulation context.""" + return build_simulation_context( + sim_cfg=SimulationCfg( + device=device, + dt=1.0 / 120.0, + gravity=(0.0, 0.0, 0.0), + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg(), use_cuda_graph=False), + ) + ) + + +def _author_two_link_articulations() -> Articulation: + """Author two local one-DOF floating articulations.""" + link_cfg = sim_utils.CuboidCfg( + size=(0.4, 0.1, 0.1), + rigid_props=sim_utils.RigidBodyBaseCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionBaseCfg(), + ) + stage = sim_utils.get_current_stage() + for env_index in range(2): + env_path = f"/World/Env_{env_index}" + robot_path = f"{env_path}/Robot" + root_path = f"{robot_path}/Root" + child_path = f"{robot_path}/Child" + sim_utils.create_prim(env_path, "Xform", translation=(2.0 * env_index, 0.0, 0.0)) + sim_utils.create_prim(robot_path, "Xform") + link_cfg.func(root_path, link_cfg, translation=(0.0, 0.0, 1.0)) + link_cfg.func(child_path, link_cfg, translation=(0.5, 0.0, 1.0)) + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath(root_path)) + joint = UsdPhysics.RevoluteJoint.Define(stage, f"{robot_path}/Joint") + joint.CreateBody0Rel().SetTargets([root_path]) + joint.CreateBody1Rel().SetTargets([child_path]) + joint.CreateAxisAttr().Set("Z") + joint.CreateLocalPos0Attr().Set(Gf.Vec3f(0.25, 0.0, 0.0)) + joint.CreateLocalPos1Attr().Set(Gf.Vec3f(-0.25, 0.0, 0.0)) + joint.CreateLowerLimitAttr().Set(-90.0) + joint.CreateUpperLimitAttr().Set(90.0) + + return Articulation( + ArticulationCfg( + prim_path="/World/Env_[^/]*/Robot", + articulation_root_prim_path="/Root", actuators={ "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=2000.0, - damping=100.0, - ), - }, - ) - else: - raise ValueError( - f"Invalid articulation type: {articulation_type}, valid options are 'humanoid', 'panda', 'anymal'," - " 'shadow_hand', 'single_joint_implicit', 'single_joint_explicit' or 'spatial_tendon_test_asset'." - ) - - return articulation_cfg - - -def fix_reversed_joints(stage): - """Fix reversed joints on the USD stage. - - Some USD assets have joints where physics:body0 is the child and physics:body1 is the parent, - which is the opposite of what Newton expects. This function detects reversed joints by building - a graph of body connections and identifying the root body (the one attached to world via a joint - with a missing body target). Any joint where body0 is closer to the root than body1 is swapped. - """ - from pxr import UsdPhysics - - # First pass: find root bodies (bodies with a joint that has only one target, i.e. attached to world) - root_bodies: set[str] = set() - joints_to_check = [] - for prim in stage.Traverse(): - if not prim.IsA(UsdPhysics.Joint): - continue - body0_targets = prim.GetRelationship("physics:body0").GetTargets() - body1_targets = prim.GetRelationship("physics:body1").GetTargets() - if body0_targets and not body1_targets: - root_bodies.add(str(body0_targets[0])) - elif body1_targets and not body0_targets: - root_bodies.add(str(body1_targets[0])) - elif body0_targets and body1_targets: - joints_to_check.append(prim) - - if not root_bodies: - return - - # Second pass: for each joint with two bodies, ensure body0 is the parent (closer to root) - for prim in joints_to_check: - body0_rel = prim.GetRelationship("physics:body0") - body1_rel = prim.GetRelationship("physics:body1") - body0_path = str(body0_rel.GetTargets()[0]) - body1_path = str(body1_rel.GetTargets()[0]) - - # Determine if we need to swap: body1 is root or ancestor → need to swap - body1_is_parent = body1_path in root_bodies or body0_path.startswith(body1_path + "/") - body0_is_parent = body0_path in root_bodies or body1_path.startswith(body0_path + "/") - - if body0_is_parent or not body1_is_parent: - continue # already correct or ambiguous - - # Swap body0 and body1 - body0_rel.SetTargets(body1_rel.GetTargets()) - body1_rel.SetTargets([body0_path]) - - # Swap local transforms - for attr_suffix in ("localPos", "localRot"): - attr0 = prim.GetAttribute(f"physics:{attr_suffix}0") - attr1 = prim.GetAttribute(f"physics:{attr_suffix}1") - val0, val1 = attr0.Get(), attr1.Get() - if val0 is not None and val1 is not None: - attr0.Set(val1) - attr1.Set(val0) - - -_REVERSED_JOINT_USD_FILES = {"revolute_articulation.usd"} -"""USD filenames with known reversed joint body0/body1 ordering.""" - - -_ANYMAL_C_BODY_NAMES = ( - "base", - "LF_HIP", - "LF_THIGH", - "LF_SHANK", - "LF_FOOT", - "LH_HIP", - "LH_THIGH", - "LH_SHANK", - "LH_FOOT", - "RF_HIP", - "RF_THIGH", - "RF_SHANK", - "RF_FOOT", - "RH_HIP", - "RH_THIGH", - "RH_SHANK", - "RH_FOOT", -) -_ANYMAL_C_ROOT_PRESERVING_REVERSED_BODY_NAMES = (_ANYMAL_C_BODY_NAMES[0], *reversed(_ANYMAL_C_BODY_NAMES[1:])) - - -_NEWTON_USER_ORDER_STATE_CACHES = ( - "_joint_pos_user", - "_joint_vel_user", - "_body_link_pose_w_user", - "_body_com_vel_w_user", -) - - -def generate_articulation( - articulation_cfg: ArticulationCfg, num_articulations: int, device: str -) -> tuple[Articulation, torch.tensor]: - """Generate an articulation from a configuration. - - Handles the creation of the articulation, the environment prims and the articulation's environment - translations - - Args: - articulation_cfg: Articulation configuration. - num_articulations: Number of articulations to generate. - device: Device to use for the tensors. - - Returns: - The articulation and environment translations. - - """ - # Generate translations of 2.5 m in x for each articulation - translations = torch.zeros(num_articulations, 3, device=device) - translations[:, 0] = torch.arange(num_articulations) * 2.5 - - # Create Top-level Xforms, one for each articulation - for i in range(num_articulations): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) - articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_[^/]*/Robot")) - - # Fix reversed joints for known-broken USD assets (body0/body1 swapped) - usd_path = getattr(articulation_cfg.spawn, "usd_path", "") - if any(name in usd_path for name in _REVERSED_JOINT_USD_FILES): - import omni.usd - - fix_reversed_joints(omni.usd.get_context().get_stage()) - - return articulation, translations - - -# --------------------------------------------------------------------------- -# Franka task-space tracking helpers (shared between IK and OSC tests). -# --------------------------------------------------------------------------- - - -def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, disable_gravity: bool = True): - """Build a Franka articulation at its configured home pose. - - Constructs :data:`FRANKA_PANDA_HIGH_PD_CFG`, optionally zeroes the - arm-actuator PD gains, resets the simulator, and teleports the - arm joints to :attr:`default_joint_pos` (the env reset path that - normally does this is not invoked for standalone tests, so the - robot would otherwise sit at the URDF-neutral pose where the - Franka wrist is near-singular). - - Args: - sim: The simulation context to use. - zero_actuator_pd: If True, sets the panda_shoulder/panda_forearm - actuator stiffness and damping to zero. Used by the OSC test - so OSC's joint-effort output is not opposed by the - implicit-PD's residual ``kp·(target − q)``. - disable_gravity: Per-body gravity flag written to the spawn config. - :data:`FRANKA_PANDA_HIGH_PD_CFG` ships with gravity disabled; - pass False for tests where the arm must feel scene gravity. - - Returns: - Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. - """ - cfg = FRANKA_PANDA_HIGH_PD_CFG.copy().replace(prim_path="/World/Env_[^/]*/Robot") - if zero_actuator_pd: - cfg.actuators["panda_shoulder"].stiffness = 0.0 - cfg.actuators["panda_shoulder"].damping = 0.0 - cfg.actuators["panda_forearm"].stiffness = 0.0 - cfg.actuators["panda_forearm"].damping = 0.0 - cfg.spawn.rigid_props.disable_gravity = disable_gravity - sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) - robot = Articulation(cfg) - sim.reset() - assert robot.is_initialized - - ee_frame_idx = robot.find_bodies("panda_hand")[0][0] - ee_jacobi_idx = ee_frame_idx - 1 - arm_joint_ids = robot.find_joints(["panda_joint.*"])[0] - - robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch[:, :].clone()) - robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch[:, :].clone()) - return robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids - - -def _compute_ee_pose_root(robot, ee_frame_idx): - """Return ``(ee_pos_b, ee_quat_b, root_pose_w)`` in the root frame.""" - ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx] - root_pose_w = robot.data.root_pose_w.torch - ee_pos_b, ee_quat_b = subtract_frame_transforms( - root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] - ) - return ee_pos_b, ee_quat_b, root_pose_w - - -def _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids): - """Return the EE Jacobian sliced to ``arm_joint_ids`` and rotated to the root frame.""" - jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, arm_joint_ids] - base_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_pose_w.torch[:, 3:7])) - jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) - jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) - return jacobian - - -def _compute_ee_vel_root(jacobian_b, joint_vel): - """Return the EE 6D velocity in the root frame as ``J · q_dot``. - - Required to make OSC's ``kd * ee_vel_b`` damping term meaningful. - Passing zero EE velocity (the convenient hack) leaves the impedance - undamped and the EE oscillates around the target. We use ``J · q_dot`` - rather than reading ``data.body_vel_w`` because Newton's lazy - 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``. - """ - return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) - - -def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device): - """Build a target pose = (current EE pose) + ``delta_xyz``, preserving orientation.""" - initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - target_pos_b = initial_ee_pos_b + torch.tensor([list(delta_xyz)], device=device, dtype=initial_ee_pos_b.dtype) - 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.""" - tail_slice = history[-tail:] - return min(tail_slice), sum(tail_slice) / len(tail_slice) - - -@pytest.fixture -def sim(request): - """Create simulation context with the specified device.""" - device = request.getfixturevalue("device") - if "gravity_enabled" in request.fixturenames: - gravity_enabled = request.getfixturevalue("gravity_enabled") - else: - gravity_enabled = True # default to gravity enabled - if "add_ground_plane" in request.fixturenames: - add_ground_plane = request.getfixturevalue("add_ground_plane") - else: - add_ground_plane = False # default to no ground plane - articulation_type = request.getfixturevalue("articulation_type") - sim_cfg = deepcopy(SIM_CFGs[articulation_type]) - sim_cfg.device = device - if "use_newton_actuators" in request.fixturenames: - sim_cfg.use_newton_actuators = request.getfixturevalue("use_newton_actuators") - # ``gravity_enabled`` is silently ignored by ``build_simulation_context`` - # when an explicit ``sim_cfg`` is also passed; apply it here so the - # fixture honors what its parameter advertises. - if not gravity_enabled: - sim_cfg.gravity = (0.0, 0.0, 0.0) - with build_simulation_context( - device=device, - auto_add_lighting=True, - gravity_enabled=gravity_enabled, - add_ground_plane=add_ground_plane, - sim_cfg=sim_cfg, - ) as sim: - sim._app_control_on_stop_handle = None - yield sim - - -def _make_target_mode_builder( - joint_names: list[str], target_modes: list[JointTargetMode], stiffness: list[float], damping: list[float] -) -> ModelBuilder: - """Build a zero-gain articulated model builder for target-mode tests.""" - builder = ModelBuilder() - inertia = wp.mat33(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) - parent = -1 - joint_ids = [] - for joint_name in joint_names: - link = builder.add_link(mass=1.0, inertia=inertia, label=f"/World/Env_0/Robot/{joint_name}_link") - joint_ids.append( - builder.add_joint_revolute( - parent, - link, - target_ke=0.0, - target_kd=0.0, - label=f"/World/Env_0/Robot/{joint_name}", - ) - ) - parent = link - builder.add_articulation(joint_ids, label="/World/Env_0/Robot") - builder.articulation_label = ["/World/Env_0/Robot"] - builder.joint_target_mode = [int(mode) for mode in target_modes] - builder.joint_target_ke = stiffness - builder.joint_target_kd = damping - 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])) - - -def test_prepare_native_actuators_does_not_zero_solver_gains(monkeypatch): - """Leave solver gains untouched until collection construction resolves actuator defaults.""" - gain_writes = [] - articulation = SimpleNamespace( - _sim_cfg=SimpleNamespace(use_newton_actuators=True), - device="cpu", - find_joints=lambda _: ([0], ["joint"]), - 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 == [] - - -@pytest.mark.parametrize( - ("actuator_cfg", "expected_mode", "expected_actuator_indices"), - [ - ( - ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=0.0), - JointTargetMode.POSITION, - [0, 1], - ), - ( - ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=2.0), - JointTargetMode.VELOCITY, - [-2, -3], - ), - ( - ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=2.0), - JointTargetMode.POSITION_VELOCITY, - [0, -2, 1, -3], - ), - ( - ImplicitActuatorCfg( - class_type=f"{__name__}:CustomDrive", joint_names_expr=[".*"], stiffness=10.0, damping=2.0 - ), - JointTargetMode.POSITION_VELOCITY, - [0, -2, 1, -3], - ), - ( - ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0), - JointTargetMode.EFFORT, - None, - ), - ( - IdealPDActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=2.0), - JointTargetMode.EFFORT, - None, - ), - ], -) -def test_actuator_cfg_sets_newton_target_mode_before_solver_init( - actuator_cfg, expected_mode, expected_actuator_indices -): - """Resolve configured modes before finalization constructs MuJoCo actuators.""" - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_[^/]*/Robot", - articulation_root_prim_path="", - actuators={"joint": actuator_cfg}, - ) - builder = _make_target_mode_builder( - ["left_joint", "right_joint"], [JointTargetMode.NONE, JointTargetMode.NONE], [0.0, 0.0], [0.0, 0.0] - ) - _configure_builder_joint_target_modes(builder, articulation_cfg) - model = builder.finalize(device="cpu") - solver = SolverMuJoCo(model, use_mujoco_cpu=True) - assert model.joint_target_mode.numpy().tolist() == [int(expected_mode), int(expected_mode)] - assert ( - solver.mjc_actuator_to_newton_idx.numpy().tolist() if solver.mjc_actuator_to_newton_idx is not None else None - ) == expected_actuator_indices - - -def test_actuator_cfg_matches_explicit_descendant_articulation_root(): - """Match target modes against an explicitly configured descendant articulation root.""" - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_[^/]*/Robot", - articulation_root_prim_path="/base", - actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=0.0)}, - ) - builder = _make_target_mode_builder(["joint"], [JointTargetMode.NONE], [0.0], [0.0]) - builder.articulation_label = ["/World/Env_0/Robot/base"] - _configure_builder_joint_target_modes(builder, articulation_cfg) - assert builder.joint_target_mode == [int(JointTargetMode.POSITION)] - - -def test_actuator_cfg_matches_clone_plan_root_expr(monkeypatch): - """Match builder labels against the clone slot spelling clone-plan root resolution returns.""" - articulation_cfg = ArticulationCfg( - prim_path="{ENV_REGEX_NS}/Robot", - actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=0.0)}, - ) - monkeypatch.setattr( - "isaaclab_newton.assets.articulation.articulation.resolve_matching_prims_from_source", - lambda *_args, **_kwargs: [(None, "/World/envs/env_[^/]+/Robot/base")], - ) - builder = _make_target_mode_builder(["joint"], [JointTargetMode.NONE], [0.0], [0.0]) - builder.articulation_label = ["/World/envs/env_0/Robot/base"] - _configure_builder_joint_target_modes(builder, articulation_cfg) - assert builder.joint_target_mode == [int(JointTargetMode.POSITION)] - - -@pytest.mark.parametrize("joint_type", [JointType.FREE, JointType.FIXED]) -def test_actuator_cfg_leaves_excluded_joint_types_imported(joint_type): - """Leave target modes for free and fixed joints unchanged.""" - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_[^/]*/Robot", - articulation_root_prim_path="", - actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=0.0)}, - ) - builder = _make_target_mode_builder(["joint"], [JointTargetMode.NONE], [0.0], [0.0]) - builder.joint_type[0] = joint_type - _configure_builder_joint_target_modes(builder, articulation_cfg) - assert builder.joint_target_mode == [int(JointTargetMode.NONE)] - - -def test_actuator_cfg_keeps_imported_newton_target_mode_for_none_gain(): - """Retain the imported stiffness when an implicit actuator config leaves it unset.""" - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_[^/]*/Robot", - articulation_root_prim_path="", - actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=None, damping=0.0)}, - ) - builder = _make_target_mode_builder(["joint"], [JointTargetMode.EFFORT], [10.0], [0.0]) - _configure_builder_joint_target_modes(builder, articulation_cfg) - assert builder.joint_target_mode == [int(JointTargetMode.POSITION)] - - -def test_actuator_cfg_leaves_unconfigured_newton_target_modes_imported(): - """Leave target modes for DOFs outside an actuator group unchanged.""" - subset_cfg = ArticulationCfg( - prim_path="/World/Env_[^/]*/Robot", - articulation_root_prim_path="", - actuators={ - "shoulder": ImplicitActuatorCfg(joint_names_expr=["left_shoulder"], stiffness=10.0, damping=0.0), - }, - ) - builder = _make_target_mode_builder( - ["left_shoulder", "right_shoulder"], - [JointTargetMode.NONE, JointTargetMode.VELOCITY], - [0.0, 0.0], - [0.0, 2.0], - ) - _configure_builder_joint_target_modes(builder, subset_cfg) - assert builder.joint_target_mode == [int(JointTargetMode.POSITION), int(JointTargetMode.VELOCITY)] - - -@pytest.mark.parametrize( - ("stiffness", "damping", "expected_modes"), - [ - ({"left_joint": 10.0}, 0.0, [JointTargetMode.POSITION, JointTargetMode.EFFORT]), - ({"left_joint": 10.0}, {"right_joint": 2.0}, [JointTargetMode.POSITION, JointTargetMode.VELOCITY]), - ], -) -def test_actuator_cfg_aligns_partial_dictionary_gains_by_joint_name(stiffness, damping, expected_modes): - """Resolve sparse stiffness and damping dictionaries independently by joint name.""" - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_[^/]*/Robot", - articulation_root_prim_path="", - actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, - ) - builder = _make_target_mode_builder( - ["left_joint", "right_joint"], [JointTargetMode.NONE, JointTargetMode.NONE], [0.0, 0.0], [0.0, 0.0] - ) - _configure_builder_joint_target_modes(builder, articulation_cfg) - 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) - 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" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - ) - ) - - 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", ["cuda:0", "cpu"]) -@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): - """Resolve the documented Newton ``joint_ordering="physx"`` sim-to-sim workflow on a branching asset. - - Mirrors :func:`isaaclab_physx.test.assets.test_articulation.test_branching_fixture_resolves_distinct_conventions` - with the backend roles swapped: here the live backend is Newton (depth-first, so its native view is - the MJWarp order), and the request is ``physx``/``body_ordering="physx"``. Cross-backend discovery must - resolve the breadth-first PhysX order and reorder the public joint/body axes to it. This is the headline - workflow documented in - ``docs/source/overview/core-concepts/physical-backends/joint_and_body_ordering.rst``. - - 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. - """ - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - joint_ordering="physx", - body_ordering="physx", - ) - ) - - sim.reset() - assert articulation.is_initialized - - # Newton's native traversal is depth-first, so the live backend view already reflects MJWarp order. - assert tuple(articulation.backend_joint_names) == BRANCHING_MJWARP_JOINT_NAMES - assert tuple(articulation.backend_body_names) == BRANCHING_MJWARP_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 - - # The requested PhysX ordering reorders the public joint/body axes to the BFS convention. - assert tuple(articulation.joint_names) == BRANCHING_PHYSX_JOINT_NAMES - assert tuple(articulation.body_names) == BRANCHING_PHYSX_BODY_NAMES - assert articulation.joint_ordering is not None - assert articulation.body_ordering is not None - - -def test_num_shapes_per_body_follows_public_body_order() -> None: - """Align Newton shape counts with the public body-name axis.""" - - class _ShapeCountSurface: - backend_num_shapes_per_body = Articulation.backend_num_shapes_per_body - num_shapes_per_body = Articulation.num_shapes_per_body - - articulation = _ShapeCountSurface() - articulation._num_shapes_per_body_backend = None - articulation._root_view = SimpleNamespace( - body_shapes=((), (object(), object()), (object(), object(), object())), - ) - articulation.body_ordering = SimpleNamespace( - user_to_backend_indices=(2, 0, 1), - ) - - assert articulation.num_shapes_per_body == [3, 0, 2] - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) # consumed by the sim fixture -@pytest.mark.parametrize("use_newton_actuators", [True]) # consumed by the sim fixture -def test_newton_native_actuator_gain_write_maps_public_joint_subset_to_backend( - sim, articulation_type, use_newton_actuators, device -): - """Map selected public joint IDs to Newton-controller columns.""" - articulation_cfg = generate_articulation_cfg("anymal").replace( - actuators={ - "legs": IdealPDActuatorCfg( - joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ) - }, - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)), - ) - articulation, _ = generate_articulation(articulation_cfg, 2, device=sim.device) - sim.reset() - assert articulation.joint_ordering is not None - assert articulation.newton_actuator_adapter is not None - - def gather_stiffness() -> torch.Tensor: - stiffness = torch.zeros( - (articulation.num_instances, articulation.num_joints), - device=articulation.device, - ) - for actuator in articulation.newton_actuator_adapter.actuators: - if hasattr(actuator.controller, "kp"): - stiffness += wp.to_torch( - articulation.root_view.get_actuator_parameter(actuator, actuator.controller, "kp") + joint_names_expr=["Joint"], + stiffness=20.0, + damping=2.0, ) - return stiffness - - stiffness_before = gather_stiffness() - env_ids = torch.tensor([1], device=articulation.device, dtype=torch.long) - joint_ids = torch.tensor([1, 6, 10], device=articulation.device, dtype=torch.long) - stiffness = torch.tensor([[101.0, 106.0, 110.0]], device=articulation.device) - - with pytest.warns(DeprecationWarning, match="write_actuator_stiffness_to_sim"): - articulation.write_actuator_stiffness_to_sim( - stiffness=stiffness, - env_ids=env_ids, - joint_ids=joint_ids, + }, ) - - backend_joint_ids = torch.tensor( - articulation.joint_ordering.user_to_backend_indices, - device=articulation.device, - dtype=torch.long, - )[joint_ids] - expected_stiffness = stiffness_before.clone() - expected_stiffness[env_ids.unsqueeze(1), backend_joint_ids.unsqueeze(0)] = stiffness - torch.testing.assert_close(gather_stiffness(), expected_stiffness) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.parametrize("state_kind", ["pose", "velocity"]) -def test_newton_ordered_body_state_cache_invalidates_on_same_timestamp_root_write( - sim, num_articulations, device, gravity_enabled, articulation_type, state_kind -): - """Refresh ordered body state after a root write at the current simulation timestamp.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - body_ordering=_ANYMAL_C_ROOT_PRESERVING_REVERSED_BODY_NAMES ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) - - data = articulation.data - assert data.body_ordering is not None - root_body_idx = articulation.find_bodies("base")[0][0] - sim_timestamp = data._sim_timestamp - - if state_kind == "pose": - cached_body_state = data.body_link_pose_w.torch[:, root_body_idx].clone() - written_root_state = data.root_link_pose_w.torch.clone() - written_root_state[:, 0] += 0.25 - articulation.write_root_link_pose_to_sim_index(root_pose=written_root_state) - - assert data._sim_timestamp == sim_timestamp - torch.testing.assert_close(data.root_link_pose_w.torch, written_root_state) - refreshed_body_state = data.body_link_pose_w.torch[:, root_body_idx] - else: - cached_body_state = data.body_com_vel_w.torch[:, root_body_idx].clone() - written_root_state = torch.tensor( - [[1.0, 2.0, 3.0, 4.0, 5.0, 6.0]], device=device, dtype=cached_body_state.dtype - ) - articulation.write_root_com_velocity_to_sim_index(root_velocity=written_root_state) - - assert data._sim_timestamp == sim_timestamp - torch.testing.assert_close(data.root_com_vel_w.torch, written_root_state) - refreshed_body_state = data.body_com_vel_w.torch[:, root_body_idx] - - torch.testing.assert_close(refreshed_body_state, written_root_state) - assert not torch.equal(refreshed_body_state, cached_body_state) -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.parametrize("ordering_mode", ["none", "reversed"]) -def test_newton_ordered_state_caches_invalidate_on_rebind( - sim, num_articulations, device, gravity_enabled, articulation_type, ordering_mode -): - """Rebind public state to recreated Newton arrays and invalidate ordered caches.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - if ordering_mode == "reversed": - articulation_cfg = articulation_cfg.replace( - joint_ordering=tuple(reversed(PANDA_JOINT_NAMES)), - body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES, +@pytest.fixture(scope="module") +def articulation_scene() -> _ArticulationScene: + """Initialize every real Newton articulation once for this module.""" + device = "cpu" + with _newton_sim_context(device=device) as sim: + floating = _author_two_link_articulations() + fixed = author_fixed_spatial_chain( + prim_paths=("/World/Env_0/FixedRobot", "/World/Env_1/FixedRobot"), + prim_path_expr="/World/Env_[^/]*/FixedRobot", ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - sim.reset() - assert articulation.is_initialized - has_ordering = ordering_mode == "reversed" - assert (articulation.data.joint_ordering is not None) is has_ordering - assert (articulation.data.body_ordering is not None) is has_ordering - - sim.step() - articulation.update(sim.cfg.dt) - - data = articulation.data - primed_joint_vel_values = ( - np.arange(np.prod(data._sim_bind_joint_vel.shape), dtype=np.float32).reshape(data._sim_bind_joint_vel.shape) - + 100.0 - ) - body_velocity_shape = (*data._sim_bind_body_com_vel_w.shape, 6) - primed_body_com_vel_values = ( - np.arange(np.prod(body_velocity_shape), dtype=np.float32).reshape(body_velocity_shape) + 200.0 - ) - data._sim_bind_joint_vel.assign( - wp.array(primed_joint_vel_values, dtype=wp.float32, device=data._sim_bind_joint_vel.device) - ) - data._sim_bind_body_com_vel_w.assign( - wp.array( - primed_body_com_vel_values, - dtype=wp.spatial_vectorf, - device=data._sim_bind_body_com_vel_w.device, - ) - ) - # The raw sim-bind writes above simulate the solver advancing state; in the - # real pipeline the post-step callback republishes the passthrough shadows in - # the same step. Mirror that here so ``joint_acc`` (which reads the passthrough - # ``joint_vel`` shadow) observes the primed backend state. No-op under identity - # ordering, where the getters alias the sim-bound arrays directly. - data._refresh_user_order_state() - data.update(sim.cfg.dt) - primed_joint_acc = data.joint_acc.warp.numpy().copy() - primed_body_com_acc_w = data.body_com_acc_w.warp.numpy().copy() - assert data._joint_acc.timestamp == data._sim_timestamp - assert data._body_com_acc_w.timestamp == data._sim_timestamp - assert np.any(primed_joint_acc != 0.0) - assert np.any(primed_body_com_acc_w != 0.0) - - public_to_binding = { - "joint_pos": "_sim_bind_joint_pos", - "joint_vel": "_sim_bind_joint_vel", - "body_link_pose_w": "_sim_bind_body_link_pose_w", - "body_com_vel_w": "_sim_bind_body_com_vel_w", - } - public_to_shadow = { - "joint_pos": "_joint_pos_user", - "joint_vel": "_joint_vel_user", - "body_link_pose_w": "_body_link_pose_w_user", - "body_com_vel_w": "_body_com_vel_w_user", - } - old_bindings = {name: getattr(data, name) for name in public_to_binding.values()} - old_binding_ptrs = {name: int(array.ptr) for name, array in old_bindings.items()} - old_public_proxies = {name: getattr(data, name) for name in public_to_binding} - implicit_executor = articulation.actuators._implicit_executor - assert implicit_executor is not None - actuator_state_inputs = [implicit_executor.kernel_inputs] - data.joint_pos_limits.torch.clone() - assert data._joint_pos_limits_timestamp == data._sim_timestamp - # The Tier-1 state shadows are plain wp.arrays (no timestamp): they are - # allocated for non-identity ordering and stay ``None`` for identity ordering. - if has_ordering: - for cache_name in _NEWTON_USER_ORDER_STATE_CACHES: - assert getattr(data, cache_name) is not None - else: - for cache_name in _NEWTON_USER_ORDER_STATE_CACHES: - assert getattr(data, cache_name) is None - - old_state = SimulationManager.get_state_0() - old_model = SimulationManager.get_model() - new_state = copy(old_state) - new_model = copy(old_model) - - joint_q_values = np.arange(len(old_state.joint_q), dtype=np.float32) + 1000.0 - joint_qd_values = np.arange(len(old_state.joint_qd), dtype=np.float32) + 2000.0 - body_indices = np.arange(len(old_state.body_q), dtype=np.float32)[:, None] - body_q_values = np.zeros((len(old_state.body_q), 7), dtype=np.float32) - body_q_values[:, :3] = 3000.0 + 10.0 * body_indices + np.arange(3, dtype=np.float32) - body_q_values[:, 6] = 1.0 - body_qd_values = 4000.0 + 10.0 * body_indices + np.arange(6, dtype=np.float32) - limit_indices = np.arange(len(old_model.joint_limit_lower), dtype=np.float32) - limit_lower_values = -5000.0 - limit_indices - limit_upper_values = 5000.0 + limit_indices - - new_state.joint_q = wp.array(joint_q_values, dtype=wp.float32, device=old_state.joint_q.device) - new_state.joint_qd = wp.array(joint_qd_values, dtype=wp.float32, device=old_state.joint_qd.device) - new_state.body_q = wp.array(body_q_values, dtype=wp.transformf, device=old_state.body_q.device) - new_state.body_qd = wp.array(body_qd_values, dtype=wp.spatial_vectorf, device=old_state.body_qd.device) - new_model.joint_limit_lower = wp.array( - limit_lower_values, dtype=wp.float32, device=old_model.joint_limit_lower.device - ) - new_model.joint_limit_upper = wp.array( - limit_upper_values, dtype=wp.float32, device=old_model.joint_limit_upper.device - ) - SimulationManager._state_0 = new_state - SimulationManager._model = new_model - - body_velocities = articulation.root_view.get_link_velocities(new_state) - assert body_velocities is not None - new_source_bindings = { - "_sim_bind_joint_pos": articulation.root_view.get_dof_positions(new_state)[:, 0], - "_sim_bind_joint_vel": articulation.root_view.get_dof_velocities(new_state)[:, 0], - "_sim_bind_body_link_pose_w": articulation.root_view.get_link_transforms(new_state)[:, 0], - "_sim_bind_body_com_vel_w": body_velocities[:, 0], - "_sim_bind_joint_pos_limits_lower": articulation.root_view.get_attribute("joint_limit_lower", new_model)[:, 0], - "_sim_bind_joint_pos_limits_upper": articulation.root_view.get_attribute("joint_limit_upper", new_model)[:, 0], - } - for binding_name, new_source in new_source_bindings.items(): - if binding_name in old_binding_ptrs: - assert int(new_source.ptr) != old_binding_ptrs[binding_name] - - data._create_simulation_bindings() - - for binding_name, old_binding in old_bindings.items(): - rebound = getattr(data, binding_name) - assert rebound is not old_binding - assert int(rebound.ptr) != old_binding_ptrs[binding_name] - assert int(rebound.ptr) == int(new_source_bindings[binding_name].ptr) - - for inputs in actuator_state_inputs: - assert inputs[3].ptr == data.joint_pos.warp.ptr - assert inputs[4].ptr == data.joint_vel.warp.ptr - - assert data._joint_pos_limits_timestamp == -1.0 - assert data._joint_acc.timestamp == -1.0 - assert data._body_com_acc_w.timestamp == -1.0 - - joint_user_to_backend = ( - np.asarray(articulation.joint_ordering.user_to_backend_indices) - if articulation.joint_ordering is not None - else np.arange(articulation.num_joints) - ) - body_user_to_backend = ( - np.asarray(articulation.body_ordering.user_to_backend_indices) - if articulation.body_ordering is not None - else np.arange(articulation.num_bodies) - ) - expected_previous_joint_vel = new_source_bindings["_sim_bind_joint_vel"].numpy()[:, joint_user_to_backend] - expected_previous_body_com_vel = new_source_bindings["_sim_bind_body_com_vel_w"].numpy() - np.testing.assert_array_equal(data._previous_joint_vel.numpy(), expected_previous_joint_vel) - np.testing.assert_array_equal(data._previous_body_com_vel.numpy(), expected_previous_body_com_vel) - - joint_acc = data.joint_acc.warp.numpy() - body_com_acc_w = data.body_com_acc_w.warp.numpy() - np.testing.assert_array_equal(joint_acc, np.zeros_like(expected_previous_joint_vel)) - np.testing.assert_array_equal( - body_com_acc_w, - np.zeros_like(expected_previous_body_com_vel[:, body_user_to_backend]), - ) - assert data._joint_acc.timestamp == data._sim_timestamp - assert data._body_com_acc_w.timestamp == data._sim_timestamp - assert not np.array_equal(joint_acc, primed_joint_acc) - assert not np.array_equal(body_com_acc_w, primed_body_com_acc_w) - - expected_public = { - "joint_pos": new_source_bindings["_sim_bind_joint_pos"].numpy()[:, joint_user_to_backend], - "joint_vel": new_source_bindings["_sim_bind_joint_vel"].numpy()[:, joint_user_to_backend], - "body_link_pose_w": new_source_bindings["_sim_bind_body_link_pose_w"].numpy()[:, body_user_to_backend], - "body_com_vel_w": new_source_bindings["_sim_bind_body_com_vel_w"].numpy()[:, body_user_to_backend], - } - for property_name, expected in expected_public.items(): - proxy = getattr(data, property_name) - assert proxy is not old_public_proxies[property_name] - binding_name = public_to_binding[property_name] - assert int(proxy.warp.ptr) != old_binding_ptrs[binding_name] - if has_ordering: - shadow = getattr(data, public_to_shadow[property_name]) - assert int(proxy.warp.ptr) == int(shadow.ptr) - else: - assert int(proxy.warp.ptr) == int(getattr(data, binding_name).ptr) - np.testing.assert_array_equal(proxy.warp.numpy(), expected) - - expected_limits = np.stack( - ( - new_source_bindings["_sim_bind_joint_pos_limits_lower"].numpy()[:, joint_user_to_backend], - new_source_bindings["_sim_bind_joint_pos_limits_upper"].numpy()[:, joint_user_to_backend], - ), - axis=-1, - ) - np.testing.assert_array_equal(data.joint_pos_limits.warp.numpy(), expected_limits) - assert data._joint_pos_limits_timestamp == data._sim_timestamp - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.parametrize("ordering_mode", ["none", "reversed"]) -def test_newton_rebind_preserves_lab_owned_actuator_gains( - sim, num_articulations, device, gravity_enabled, articulation_type, ordering_mode -): - """Keep Lab-owned actuator gains across a rebind that re-seeds the solver's sim gains. - - Part 2 (D3) regression: named actuator groups own their actuator kp/kd; the solver's - sim gains are deliberately zeroed for explicit DOFs. A full sim reset recreates the solver arrays. - Rebind must NOT resync the actuator-owned values from freshly rebuilt (here: sentinel) solver gains. - ``none`` is the identity-ordering control that must pass with or without the fix. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - actuators={ - "legs": IdealPDActuatorCfg( - joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ) - }, - ) - if ordering_mode == "reversed": - articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES))) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - sim.reset() - assert articulation.is_initialized - - has_ordering = ordering_mode == "reversed" - data = articulation.data - assert (data.joint_ordering is not None) is has_ordering - - # Prime: explicit (IdealPD) actuators keep their PD in actuator-owned records, - # while the solver's sim gains are zeroed so it applies no PD on these DOFs. - np.testing.assert_allclose(articulation.actuators["legs"].stiffness.cpu().numpy(), 40.0) - np.testing.assert_allclose(articulation.actuators["legs"].damping.cpu().numpy(), 5.0) - np.testing.assert_allclose(data._sim_bind_joint_stiffness_sim.numpy(), 0.0) - np.testing.assert_allclose(data._sim_bind_joint_damping_sim.numpy(), 0.0) - - # Simulate a full sim reset: shallow-copy the model, swap the joint gain arrays - # for sentinel-filled arrays (standing in for whatever the solver rebuilds), and - # rebind the data-side sim bindings. - old_model = SimulationManager.get_model() - new_model = copy(old_model) - sentinel_ke = 12345.0 - sentinel_kd = 678.0 - new_model.joint_target_ke = wp.array( - np.full(len(old_model.joint_target_ke), sentinel_ke, dtype=np.float32), - dtype=wp.float32, - device=old_model.joint_target_ke.device, - ) - new_model.joint_target_kd = wp.array( - np.full(len(old_model.joint_target_kd), sentinel_kd, dtype=np.float32), - dtype=wp.float32, - device=old_model.joint_target_kd.device, - ) - SimulationManager._model = new_model - data._create_simulation_bindings() - - # The actuator-owned gains must survive the rebind unchanged... - np.testing.assert_allclose(articulation.actuators["legs"].stiffness.cpu().numpy(), 40.0) - np.testing.assert_allclose(articulation.actuators["legs"].damping.cpu().numpy(), 5.0) - # ...while the sim-owned mirrors track the solver's freshly seeded (sentinel) gains. - if has_ordering: - np.testing.assert_allclose(data._joint_stiffness_user.numpy(), sentinel_ke) - np.testing.assert_allclose(data._joint_damping_user.numpy(), sentinel_kd) - else: - np.testing.assert_allclose(data._sim_bind_joint_stiffness_sim.numpy(), sentinel_ke) - np.testing.assert_allclose(data._sim_bind_joint_damping_sim.numpy(), sentinel_kd) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_newton_post_step_hook_publishes_ordered_state_inside_step( - sim, num_articulations, device, gravity_enabled, articulation_type -): - """Republish the user-order Tier-1 shadows inside the sim step, without any read. + sim.reset() + yield _ArticulationScene(sim=sim, floating=floating, fixed=fixed, device=device) - Part 1 (D1) regression: with non-identity ordering the passthrough state getters no - longer reorder on read; the post-step callback republishes the shadows from live - backend state inside the stepped region. We deliberately clobber the four shadows, - step the simulation WITHOUT reading any state property, and assert the shadows again - equal the reordered backend state -- which can only hold if the hook ran inside the - step. Under the lazy design (no hook), only a property read would refresh them, so - the clobbered shadows would stay stale and the assertions would fail. - Ships the eager-mode invariant variant: CUDA-graph capture is not reliably reachable - from this CPU test harness, and this invariant directly proves the in-step republish. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - actuators={"legs": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0)}, - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)), - body_ordering=_ANYMAL_C_ROOT_PRESERVING_REVERSED_BODY_NAMES, - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - sim.reset() +def test_articulation_initialization_and_partial_state(articulation_scene: _ArticulationScene) -> None: + """Prove floating articulation discovery and isolated indexed state writes.""" + articulation = articulation_scene.floating + device = articulation_scene.device assert articulation.is_initialized - - data = articulation.data - assert data.joint_ordering is not None - assert data.body_ordering is not None - joint_u2b = np.asarray(articulation.joint_ordering.user_to_backend_indices) - body_u2b = np.asarray(articulation.body_ordering.user_to_backend_indices) - - # Clobber every Tier-1 shadow with a large sentinel so a stale (unrepublished) shadow - # is unmistakably detectable -- the true backend joint/velocity state sits near zero, - # so a plain zero-fill would coincide with it. Then step WITHOUT touching joint_pos / - # joint_vel / body_link_pose_w / body_com_vel_w. - data._joint_pos_user.fill_(1000.0) - data._joint_vel_user.fill_(1000.0) - data._body_link_pose_w_user.fill_(wp.transformf(1000.0, 1000.0, 1000.0, 0.0, 0.0, 0.0, 1.0)) - data._body_com_vel_w_user.fill_(wp.spatial_vectorf(1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0)) - sim.step() - - np.testing.assert_allclose(data._joint_pos_user.numpy(), data._sim_bind_joint_pos.numpy()[:, joint_u2b]) - np.testing.assert_allclose(data._joint_vel_user.numpy(), data._sim_bind_joint_vel.numpy()[:, joint_u2b]) - np.testing.assert_allclose( - data._body_link_pose_w_user.numpy(), data._sim_bind_body_link_pose_w.numpy()[:, body_u2b] + assert not articulation.is_fixed_base + assert articulation.num_instances == 2 + assert articulation.num_bodies == 2 + assert articulation.num_joints == 1 + assert articulation.joint_names == ["Joint"] + assert articulation.data.body_mass.shape == (2, 2) + assert articulation.data.body_com_pos_b.shape == (2, 2) + assert articulation.data.body_inertia.shape == (2, 2, 9) + + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([0], dtype=torch.int32, device=device) + initial_joint_pos = articulation.data.joint_pos.torch.clone() + initial_joint_vel = articulation.data.joint_vel.torch.clone() + target_joint_pos = torch.tensor([[0.25]], device=device) + target_joint_vel = torch.tensor([[-0.5]], device=device) + articulation.write_joint_state_to_sim_index( + position=target_joint_pos, + velocity=target_joint_vel, + env_ids=env_ids, + joint_ids=joint_ids, + ) + + torch.testing.assert_close(articulation.data.joint_pos.torch[env_ids], target_joint_pos) + torch.testing.assert_close(articulation.data.joint_vel.torch[env_ids], target_joint_vel) + torch.testing.assert_close(articulation.data.joint_pos.torch[:1], initial_joint_pos[:1]) + torch.testing.assert_close(articulation.data.joint_vel.torch[:1], initial_joint_vel[:1]) + + +def test_articulation_joint_and_body_properties_round_trip( + articulation_scene: _ArticulationScene, monkeypatch: pytest.MonkeyPatch +) -> None: + """Prove partial joint/body property writes notify the live Newton model.""" + articulation = articulation_scene.floating + device = articulation_scene.device + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + body_ids = torch.tensor([1], dtype=torch.int32, device=device) + notifications = [] + add_model_change = SimulationManager.add_model_change + + def record_model_change(change: ModelFlags) -> None: + notifications.append(change) + add_model_change(change) + + monkeypatch.setattr(SimulationManager, "add_model_change", staticmethod(record_model_change)) + joint_ids = torch.tensor([0], dtype=torch.int32, device=device) + initial_friction = articulation.data.joint_friction_coeff.torch.clone() + friction = torch.tensor([[0.3]], device=device) + articulation.write_joint_friction_coefficient_to_sim_index( + joint_friction_coeff=friction, + env_ids=env_ids, + joint_ids=joint_ids, ) - np.testing.assert_allclose(data._body_com_vel_w_user.numpy(), data._sim_bind_body_com_vel_w.numpy()[:, body_u2b]) + torch.testing.assert_close(articulation.data.joint_friction_coeff.torch[env_ids][:, joint_ids], friction) + torch.testing.assert_close(articulation.data.joint_friction_coeff.torch[:1], initial_friction[:1]) + assert notifications == [ModelFlags.JOINT_DOF_PROPERTIES] + notifications.clear() + initial_mass = articulation.data.body_mass.torch.clone() + masses = torch.tensor([[3.0]], device=device) + articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_mass.torch[env_ids][:, body_ids], masses) + torch.testing.assert_close(articulation.data.body_mass.torch[:1], initial_mass[:1]) + assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] + + notifications.clear() + initial_com = articulation.data.body_com_pos_b.torch.clone() + coms = torch.tensor([[[0.05, -0.02, 0.01]]], device=device) + articulation.set_coms_index(coms=coms, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_com_pos_b.torch[env_ids][:, body_ids], coms) + torch.testing.assert_close(articulation.data.body_com_pos_b.torch[:1], initial_com[:1]) + assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] + + notifications.clear() + initial_inertia = articulation.data.body_inertia.torch.clone() + inertias = torch.diag_embed(torch.tensor([[[2.0, 3.0, 4.0]]], device=device)).reshape(1, 1, 9) + articulation.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_inertia.torch[env_ids][:, body_ids], inertias) + torch.testing.assert_close(articulation.data.body_inertia.torch[:1], initial_inertia[:1]) + assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("gravity_enabled", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_newton_clear_callbacks_deregisters_post_step_hook( - sim, num_articulations, device, gravity_enabled, articulation_type -): - """Deregister the ordered post-step republish hook so it does not leak on the manager. - ``_create_buffers`` registers the backend-to-user state republish on - ``NewtonManager._post_step_callbacks`` for non-identity ordering. Without a - matching deregistration the bound method lingers on the class-level list - after the articulation is gone. ``_clear_callbacks`` must remove exactly that - callback and leave any other registered callback untouched. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - actuators={"legs": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0)}, - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)), - body_ordering=_ANYMAL_C_ROOT_PRESERVING_REVERSED_BODY_NAMES, - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - sim.reset() +def test_articulation_drive_and_dynamics(articulation_scene: _ArticulationScene) -> None: + """Prove fixed-root actuation, moving-link velocity, and dynamics data.""" + articulation = articulation_scene.fixed + device = articulation_scene.device assert articulation.is_initialized - assert articulation.data.joint_ordering is not None - assert articulation.data.body_ordering is not None - - # The republish hook is registered on the manager for non-identity ordering. - registered_callback = articulation._post_step_callback - assert registered_callback is not None - assert registered_callback in SimulationManager._post_step_callbacks - - # A second, independent callback stands in for another articulation's hook. - def _other_callback() -> None: - return None - - SimulationManager.register_post_step_callback(_other_callback) - - articulation._clear_callbacks() - - # The articulation's own hook is gone; the unrelated callback survives. - assert articulation._post_step_callback is None - assert registered_callback not in SimulationManager._post_step_callbacks - assert _other_callback in SimulationManager._post_step_callbacks - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@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 -): - """Gather joint targets only when non-identity joint ordering is active.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - actuators={"legs": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0)}, + assert articulation.is_fixed_base + assert articulation.num_instances == 2 + assert articulation.num_bodies == 7 + assert articulation.num_joints == 6 + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([0], dtype=torch.int32, device=device) + initial_root_pose = articulation.data.root_link_pose_w.torch.clone() + initial_joint_pos = articulation.data.joint_pos.torch.clone() + target = initial_joint_pos[env_ids][:, joint_ids].clone() + 0.05 + articulation.actuators.target_command.set_position_index( + value=target, + env_ids=env_ids, + joint_ids=joint_ids, + ) + + for _ in range(12): + articulation.write_data_to_sim() + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, initial_root_pose, atol=1e-6, rtol=0) + torch.testing.assert_close(articulation.data.joint_pos.torch[0], initial_joint_pos[0], atol=1e-6, rtol=0) + assert torch.linalg.vector_norm(articulation.data.body_link_vel_w.torch[1, -1]) > 1e-3 + jacobians = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobians.device.type == torch.device(articulation_scene.device).type + assert mass_matrix.device.type == torch.device(articulation_scene.device).type + assert jacobians.shape == (2, 6, 6, 6) + assert mass_matrix.shape == (2, 6, 6) + assert torch.isfinite(jacobians).all() + assert torch.isfinite(mass_matrix).all() + + +def test_floating_articulation_root_and_wrench_response(articulation_scene: _ArticulationScene) -> None: + """Prove floating-root state and isolated wrench delivery.""" + articulation = articulation_scene.floating + device = articulation_scene.device + assert articulation.data.root_link_pose_w.torch.shape == (2, 7) + assert articulation.data.root_com_pose_w.torch.shape == (2, 7) + assert articulation.data.body_link_pose_w.torch.shape == (2, articulation.num_bodies, 7) + assert articulation.data.body_com_pose_w.torch.shape == (2, articulation.num_bodies, 7) + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + initial_pose = articulation.data.root_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids].clone() + target_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3], device=device) + articulation.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[env_ids], target_pose) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_pose[:1]) + + initial_velocity = articulation.data.root_com_lin_vel_w.torch.clone() + articulation.permanent_wrench_composer.set_forces_and_torques_index( + forces=torch.tensor([[[8.0, 0.0, 0.0]]], device=device), + torques=torch.zeros((1, 1, 3), device=device), + env_ids=env_ids, + body_ids=torch.tensor([0], dtype=torch.int32, device=device), ) - if ordering_mode == "reversed": - articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES))) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - sim.reset() - assert articulation.is_initialized - - 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) 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" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - body_ordering="physx", - ) - ) - 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, - ) - 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), - ) - - -@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", ["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. - - 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=sim.device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - - # Check if articulation is initialized - assert articulation.is_initialized - # Check that is fixed 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, 21) - - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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) - 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] - global_gravity = wp.to_torch(model.gravity)[-1].clone() - assert articulation.data.GRAVITY_VEC_W.warp.ptr == model_gravity_arr.ptr - assert articulation.data.GRAVITY_VEC_W.shape == (num_articulations,) - - # Mutate model.gravity per-env in place, as randomize_physics_scene_gravity does. - new_gravity = torch.tensor( - [[0.1 * (i + 1), 0.2 * (i + 1), -3.0 - float(i)] for i in range(num_articulations)], - device=device, - dtype=torch.float32, - ) - wp.to_torch(model_gravity_arr).copy_(new_gravity) - SimulationManager.add_model_change(ModelFlags.MODEL_PROPERTIES) - - # Live view: new per-env values are visible immediately, no invalidation step. - torch.testing.assert_close(articulation.data.GRAVITY_VEC_W.torch, new_gravity) - torch.testing.assert_close(wp.to_torch(model.gravity)[-1], global_gravity) - - # Recompute the lazily-cached projected_gravity_b without sim.step (which would - # drift root orientation from the reset state). Project against the same quat - # buffer the kernel reads so the expectation holds for any default orientation. - articulation.update(sim.cfg.dt) - root_quat = articulation.data.root_link_quat_w.torch - expected = math_utils.quat_apply_inverse(root_quat, torch.nn.functional.normalize(new_gravity, dim=-1)) - 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) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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 actuator.is_implicit_model == 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("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. - - 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) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 9) - 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 actuator.is_implicit_model == 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 - sim.reset() - assert articulation.is_fixed_base - - # command a step away from the default pose so the distal bodies move - joint_pos_target = articulation.data.default_joint_pos.torch.clone() - joint_pos_target[:, 1] += 0.5 - prev_body_pos = articulation.data.body_link_pos_w.torch.clone() - reported_max = [] - fin_diff_max = [] - for _ in range(20): - articulation.set_joint_position_target(joint_pos_target) - articulation.write_data_to_sim() - sim.step() - articulation.update(sim.cfg.dt) - 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()) - prev_body_pos = body_pos.clone() - reported_max = torch.stack(reported_max).amax() - fin_diff_max = torch.stack(fin_diff_max).amax() - - # the commanded motion genuinely moves the bodies - assert fin_diff_max > 0.1 - # and the reported body velocities track it (identically zero under the regression) - 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) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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 actuator.is_implicit_model == 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"]) -def test_initialization_hand_with_tendons(sim, num_articulations, device, articulation_type): - """Test initialization for fixed base articulated hand with tendons. - - 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 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) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 24) - 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 actuator.is_implicit_model == is_implicit_model_cfg - - # 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("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.""" - 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) - - root = sim_utils.get_first_matching_child_prim( - "/World/Env_0/Robot", - lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), - stage=sim.stage, - ) - assert root is not None and root.HasAPI(UsdPhysics.RigidBodyAPI) - assert sim_utils.find_global_fixed_joint_prim("/World/Env_0/Robot", stage=sim.stage) is not None - - sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base - - -@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.articulation_props.fix_root_link = True - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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 - 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("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_initialization_fixed_base_made_floating_base( - sim, num_articulations, device, add_ground_plane, articulation_type -): - """Test initialization for fixed base made floating-base using schema properties. - - This test verifies that: - 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 - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).copy() - # Unfix root link by making it non-kinematic - articulation_cfg.spawn.articulation_props.fix_root_link = False - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # 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) - 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("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that the default joint position from configuration is out of range. - - This test verifies that: - 1. The articulation fails to initialize when joint positions are out of range - 2. The error is 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).copy() - articulation_cfg.init_state.joint_pos = { - "panda_joint1": 10.0, - "panda_joint[2, 4]": -20.0, - } - - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(ValueError): - sim.reset() - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_out_of_range_default_joint_vel(sim, device, articulation_type): - """Test that the default joint velocity from configuration is out of range. - - This test verifies that: - 1. The articulation fails to initialize when joint velocities are out of range - 2. The error is properly handled - """ - articulation_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot") - articulation_cfg.init_state.joint_vel = { - "panda_joint1": 100.0, - "panda_joint[2, 4]": -60.0, - } - articulation = Articulation(articulation_cfg) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(ValueError): - 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) - - # Play sim - 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) - - # Minimal env wrapper exposing scene["robot"] - class _Env: - def __init__(self, art): - self.scene = {"robot": art} - - env = _Env(articulation) - robot_all = SceneEntityCfg(name="robot") - - 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. - - This test verifies that: - 1. External forces can be applied correctly - 2. Force buffers are updated properly - 3. Zero forces are handled correctly - - 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 - 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) - - # reset articulation - articulation.reset() - - # 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, - ) - - # 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) - - -@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(sim, num_articulations, device, articulation_type): - """Test application of external force on the base of the articulation. - - 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 - - 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 - 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[..., 1] = 100.0 - - # Now we are ready! - for _ in range(5): - # 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 - 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(5): - # 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 - 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(5): - # 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 - 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 - - # Now we are ready! - for i in range(5): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - 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() - - 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 - - # apply force - 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 - for i in range(num_articulations): - # since there is a moment applied on the articulation, the articulation should rotate - assert torch.abs(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", ["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. - - This test verifies that: - 1. Gains are loaded correctly from USD file - 2. Default gains are applied when not specified - 3. The gains match the expected values - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=None, damping=None) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play sim - sim.reset() - - # Expected gains - # -- Stiffness values - expected_stiffness = { - ".*_waist.*": 20.0, - ".*_upper_arm.*": 10.0, - "pelvis": 10.0, - ".*_lower_arm": 2.0, - ".*_thigh:0": 10.0, - ".*_thigh:1": 20.0, - ".*_thigh:2": 10.0, - ".*_shin": 5.0, - ".*_foot.*": 2.0, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_stiffness, articulation.joint_names - ) - expected_stiffness = torch.zeros(articulation.num_instances, articulation.num_joints, device=articulation.device) - expected_stiffness[:, indices_list] = torch.tensor(values_list, device=articulation.device) - # -- Damping values - expected_damping = { - ".*_waist.*": 5.0, - ".*_upper_arm.*": 5.0, - "pelvis": 5.0, - ".*_lower_arm": 1.0, - ".*_thigh:0": 5.0, - ".*_thigh:1": 5.0, - ".*_thigh:2": 5.0, - ".*_shin": 0.1, - ".*_foot.*": 1.0, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_damping, articulation.joint_names - ) - expected_damping = torch.zeros_like(expected_stiffness) - expected_damping[:, indices_list] = torch.tensor(values_list, device=articulation.device) - - # 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()) -@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 - ) - - # Play sim - 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()) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_setting_gains_from_cfg_dict(sim, num_articulations, device, articulation_type): - """Test that gains are loaded from the configuration dictionary correctly. - - This test verifies that: - 1. Gains are loaded correctly from configuration dictionary - 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 - ) - # Play sim - 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()) -@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 -): - """Test setting of velocity limit for implicit 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 - - Args: - sim: The simulation fixture - 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 - """ - # create simulation - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - 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 - 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 = articulation_cfg.spawn.joint_drive_props.max_joint_velocity - 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()) -@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 -def test_setting_velocity_limit_explicit( - sim, articulation_type, num_articulations, device, joint_velocity_limit, vel_limit -): - """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 - 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 = articulation_cfg.spawn.joint_drive_props.max_joint_velocity - # 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()) -@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 - 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 = articulation_cfg.spawn.joint_drive_props.max_force - 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()) -@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 -def test_setting_effort_limit_explicit( - sim, articulation_type, num_articulations, device, joint_effort_limit, actuator_effort_limit -): - """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, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - 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 - - 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) - - 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 - 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) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_reset(sim, num_articulations, device, articulation_type, monkeypatch): - """Test 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 - ) - - # Play the simulator - sim.reset() - - # Now we are ready! - # reset articulation - actuator = next(iter(articulation.actuators.values())) - actuator_reset = actuator.reset - reset_env_ids = [] - - def record_actuator_reset(env_ids=None): - reset_env_ids.append(env_ids) - actuator_reset(env_ids) - - monkeypatch.setattr(actuator, "reset", record_actuator_reset) - 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 - - -@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 - ) - - # Play the simulator - 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) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) -def test_body_root_state(sim, num_articulations, device, with_offset, 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 - - 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) - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10, "Possible reference leak for articulation" - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized, "Articulation is not initialized" - # Check that fixed base - assert articulation.is_fixed_base, "Articulation is not a fixed base" - - # 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] - - # create com offsets — apply offset to the Arm body - num_bodies = articulation.num_bodies - com = wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())) - link_offset = [1.0, 0.0, 0.0] # the offset from CenterPivot to Arm frames - new_com = torch.tensor(offset, device=device).repeat(num_articulations, 1, 1) - com[:, 0, arm_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 - ) - - for i in range(50): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # 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 - body_link_pose_w = articulation.data.body_link_pose_w.torch - body_link_vel_w = articulation.data.body_link_vel_w.torch - 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 - 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) - - -@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 -): - """A root pose/velocity write must refresh the derived cross-frame caches without a sim step. - - 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. - """ - 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) - - # Play sim - 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 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) - - 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) - - # perform a step then update the buffers - sim.step() - articulation.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. - _ = articulation.data.root_link_pose_w.torch - _ = articulation.data.root_com_pose_w.torch - _ = 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:]) - - body_com_pose_b = articulation.data.body_com_pose_b.torch - if state_location == "com": - # the com pose/vel was written, so the derived link pose/vel must be refreshed - root_com_pose_w = articulation.data.root_com_pose_w.torch - root_com_vel_w = articulation.data.root_com_vel_w.torch - expected_root_link_pos, expected_root_link_quat = math_utils.combine_frame_transforms( - root_com_pose_w[:, :3], - root_com_pose_w[:, 3:], - math_utils.quat_rotate( - math_utils.quat_inv(body_com_pose_b[:, root_idx, 3:7]), -body_com_pose_b[:, root_idx, :3] - ), - math_utils.quat_inv(body_com_pose_b[:, root_idx, 3:7]), - ) - expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) - root_link_pose_w = articulation.data.root_link_pose_w.torch - root_link_vel_w = articulation.data.root_link_vel_w.torch - torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) - # skip lin_vel because it differs from the link frame; 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:]) - else: - # the link pose/vel was written, so the derived com pose/vel must be refreshed - root_link_pose_w = articulation.data.root_link_pose_w.torch - root_link_vel_w = articulation.data.root_link_vel_w.torch - expected_com_pos, expected_com_quat = math_utils.combine_frame_transforms( - root_link_pose_w[:, :3], - root_link_pose_w[:, 3:], - body_com_pose_b[:, root_idx, :3], - body_com_pose_b[:, root_idx, 3:7], - ) - expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) - root_com_pose_w = articulation.data.root_com_pose_w.torch - root_com_vel_w = articulation.data.root_com_vel_w.torch - 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:]) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_setting_articulation_root_prim_path(sim, device, articulation_type): - """Test that the articulation root prim path can be set explicitly.""" - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation_cfg.articulation_root_prim_path = "/torso" - articulation, _ = generate_articulation(articulation_cfg, 1, device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation._is_initialized - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_setting_invalid_articulation_root_prim_path(sim, device, articulation_type): - """Test that the articulation root prim path can be set explicitly.""" - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation_cfg.articulation_root_prim_path = "/non_existing_prim_path" - articulation, _ = generate_articulation(articulation_cfg, 1, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises((RuntimeError, KeyError)): - sim.reset() - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@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. - - 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 - device: The device to run the simulation on - """ - 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)]) - - # Play sim - sim.reset() - - 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) - - from torch.distributions import Uniform - - joint_pos_limits = articulation.data.joint_pos_limits.torch - joint_vel_limits = articulation.data.joint_vel_limits.torch - pos_dist = Uniform(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) - vel_dist = Uniform(-joint_vel_limits, joint_vel_limits) - - original_body_link_pose_w = articulation.data.body_link_pose_w.torch.clone() - original_body_com_vel_w = articulation.data.body_com_vel_w.torch.clone() - - rand_joint_pos = pos_dist.sample() - rand_joint_vel = vel_dist.sample() - - articulation.write_joint_position_to_sim_index(position=rand_joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=rand_joint_vel) - # make sure valued updated - body_link_pose_w = articulation.data.body_link_pose_w.torch - body_com_vel_w = articulation.data.body_com_vel_w.torch - original_body_states = torch.cat([original_body_link_pose_w, original_body_com_vel_w], dim=-1) - body_state_w = torch.cat([body_link_pose_w, body_com_vel_w], dim=-1) - 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 - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # validate link - com conistency - body_com_pos_b = articulation.data.body_com_pos_b.torch - body_com_quat_b = articulation.data.body_com_quat_b.torch - expected_com_pos, expected_com_quat = math_utils.combine_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:].view(-1, 4), - 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("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["shadow_hand"]) -@pytest.mark.skip(reason="Spatial tendons are not supported in Newton yet.") -def test_spatial_tendons(sim, num_articulations, device, articulation_type): - """Test spatial tendons apis. - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation has spatial tendons - 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 - """ - # skip test if Isaac Sim version is less than 5.0 - if has_kit() and get_isaac_sim_version().major < 5: - pytest.skip("Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later.") - return - articulation_cfg = generate_articulation_cfg(articulation_type="spatial_tendon_test_asset") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 3) - 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) - assert articulation.num_spatial_tendons == 1 - - articulation.set_spatial_tendon_stiffness_index(stiffness=10.0) - articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=10.0) - articulation.set_spatial_tendon_damping_index(damping=10.0) - articulation.set_spatial_tendon_offset_index(offset=10.0) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@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 - ) - - # Play the simulator - sim.reset() - - friction = torch.rand(num_articulations, articulation.num_joints, device=device) - 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("selector_kind", ["index", "mask"]) -@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.""" - articulation_cfg = generate_articulation_cfg(articulation_type) - articulation_cfg.actuators["panda_shoulder"].viscous_friction = 0.25 - articulation, _ = generate_articulation(articulation_cfg, 1, device) - sim.reset() - - shoulder_joint_ids = articulation.actuators["panda_shoulder"].joint_indices - expected_viscous_friction = torch.full((articulation.num_instances, 4), 0.25, device=device) - torch.testing.assert_close( - articulation.data.joint_viscous_friction_coeff.torch[:, shoulder_joint_ids], expected_viscous_friction - ) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_attribute("joint_damping", SimulationManager.get_model()))[ - :, 0, shoulder_joint_ids - ], - expected_viscous_friction, - ) - - expected_pd_damping = torch.full_like(expected_viscous_friction, 4.0) - torch.testing.assert_close(articulation.data.joint_damping.torch[:, shoulder_joint_ids], expected_pd_damping) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_attribute("joint_target_kd", SimulationManager.get_model()))[ - :, 0, shoulder_joint_ids - ], - expected_pd_damping, - ) - - values = torch.full((articulation.num_instances, articulation.num_joints), 0.25, device=device) - if selector_kind == "index": - articulation.write_joint_viscous_friction_coefficient_to_sim_index( - joint_viscous_friction_coeff=values, - ) - else: - articulation.write_joint_viscous_friction_coefficient_to_sim_mask( - joint_viscous_friction_coeff=values, - ) - - torch.testing.assert_close(articulation.data.joint_viscous_friction_coeff.torch, values) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_attribute("joint_damping", SimulationManager.get_model()))[:, 0], - values, - ) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_body_q_consistent_after_root_write(num_articulations, device, articulation_type): - """Test that body_q is fresh when collide() runs after a root pose write. - - Regression test for a NaN bug where collide() used stale body_q after env - reset because eval_fk was not called between write_root_pose and collide. - - Uses ``use_mujoco_contacts=False`` so the Newton collision pipeline is - active, then patches ``_simulate_physics_only`` to capture body_q at - the moment collide() is called and asserts it matches joint_q. - """ - from unittest.mock import patch - - sim_cfg = SimulationCfg( - dt=1 / 200, - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=70, - nconmax=70, - integrator="implicitfast", - use_mujoco_contacts=False, - ), - num_substeps=1, - use_cuda_graph=False, - ), - ) - with build_simulation_context(sim_cfg=sim_cfg, device=device) as sim: - 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) - - sim.reset() - - model = SimulationManager.get_model() - jc_starts = model.joint_coord_world_start.numpy() - body_starts = model.body_world_start.numpy() - - for _ in range(5): - sim.step() - articulation.update(sim.cfg.dt) - - # Teleport env 0 by 10m (simulating a reset) - new_pose = articulation.data.default_root_pose.torch.clone() - new_pose[0, 0] += 10.0 - new_pose[0, 1] += 5.0 - articulation.write_root_pose_to_sim_index( - root_pose=new_pose[0:1], - env_ids=torch.tensor([0], device=device, dtype=torch.int32), - ) - - # Patch _simulate_physics_only to capture body_q before collide runs - captured = {} - original_simulate = SimulationManager._simulate_physics_only.__func__ - - @classmethod # type: ignore[misc] - def _patched_simulate(cls): - if cls._needs_collision_pipeline: - bq = wp.to_torch(cls._state_0.body_q) - jq = wp.to_torch(cls._state_0.joint_q) - b0 = int(body_starts[0]) - jc0 = int(jc_starts[0]) - captured["bq_root"] = bq[b0, :3].clone() - captured["jq_root"] = jq[jc0 : jc0 + 3].clone() - original_simulate(cls) - - with patch.object(SimulationManager, "_simulate_physics_only", _patched_simulate): - sim.step() - articulation.update(sim.cfg.dt) - - assert captured, "collision pipeline did not run — _needs_collision_pipeline is False" - - bq_root = captured["bq_root"] - jq_root = captured["jq_root"] - diff = (jq_root - bq_root).abs().max().item() - assert diff < 0.01, ( - f"body_q was stale when collide() ran: diff={diff:.4f}m, jq={jq_root.tolist()}, bq={bq_root.tolist()}" - ) - - -@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_set_material_properties(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test getting and setting material properties (friction/restitution) via view-level APIs.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Get friction/restitution bindings via view-level API - model = SimulationManager.get_model() - friction_binding = articulation._root_view.get_attribute("shape_material_mu", model)[:, 0] - restitution_binding = articulation._root_view.get_attribute("shape_material_restitution", model)[:, 0] - num_shapes = friction_binding.shape[1] - - # Test 1: Set all shapes via in-place writes to the warp binding - friction = torch.empty(num_articulations, num_shapes, device=device).uniform_(0.4, 0.8) - restitution = torch.empty(num_articulations, num_shapes, device=device).uniform_(0.0, 0.2) - - wp.to_torch(friction_binding)[:] = friction - wp.to_torch(restitution_binding)[:] = restitution - SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) - - # Simulate physics - sim.step() - articulation.update(sim.cfg.dt) - - # Verify by reading back from the binding - mu = wp.to_torch(friction_binding) - restitution_check = wp.to_torch(restitution_binding) - torch.testing.assert_close(mu, friction) - torch.testing.assert_close(restitution_check, restitution) - - # Test 2: Set subset of shapes (only shape 0) - if num_shapes > 1: - subset_friction = torch.empty(num_articulations, device=device).uniform_(0.1, 0.2) - subset_restitution = torch.empty(num_articulations, device=device).uniform_(0.5, 0.6) - - wp.to_torch(friction_binding)[:, 0] = subset_friction - wp.to_torch(restitution_binding)[:, 0] = subset_restitution - SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) - - sim.step() - articulation.update(sim.cfg.dt) - - # Check only the subset was updated - mu_updated = wp.to_torch(friction_binding) - restitution_updated = wp.to_torch(restitution_binding) - torch.testing.assert_close(mu_updated[:, 0], subset_friction) - torch.testing.assert_close(restitution_updated[:, 0], subset_restitution) - - -@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) - - 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) - - -@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) - - 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", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_jacobians_shape_fixed_base(sim, num_articulations, device, articulation_type): - """Fixed-base ``body_link_jacobian_w`` must drop the fixed-root row. - - Contract: shape ``(N, num_bodies - 1, 6, num_joints)``. Catches - regressions of (a) the link_offset fix that drops Newton's row 0 for - fixed-base, and (b) the per-articulation output sizing — using - model-wide ``max_links`` here would over-allocate in heterogeneous - scenes. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base, "panda fixture must be fixed-base for this test" - - J = articulation.data.body_link_jacobian_w.torch - - expected_shape = (num_articulations, articulation.num_bodies - 1, 6, articulation.num_joints) - assert J.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(J.shape)}" - assert J.dtype == torch.float32 - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations, device, articulation_type): - """Fixed-base ``mass_matrix`` shape + non-singularity. - - Contract: shape ``(N, num_joints, num_joints)`` and the matrix must be - non-singular. The non-singularity check catches the heterogeneous - padding bug — if the wrapper accidentally returns ``model.max_dofs`` - sized output, the padded zero rows/cols make the matrix rank-deficient. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - sim.step() - articulation.update(sim.cfg.dt) - - M = articulation.data.mass_matrix.torch - - expected_shape = (num_articulations, articulation.num_joints, articulation.num_joints) - assert M.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(M.shape)}" - assert M.dtype == torch.float32 - - # Each diagonal entry is a joint's effective inertia and must be strictly - # positive for any physical articulation. Padded zero rows/cols (the - # heterogeneous bug) would surface as zero diagonal entries — much more - # sensitive than checking the determinant, which can be small purely from - # numerical conditioning of a well-formed 9x9 mass matrix (Franka det - # is ~1e-13 in practice). - diag = M.diagonal(dim1=-2, dim2=-1) - assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_gravity_compensation_forces_shape_fixed_base(sim, num_articulations, device, articulation_type): - """Fixed-base ``gravity_compensation_forces`` shape ``(N, num_joints)``. - - No floating-base entries on the DoF axis, and per-articulation output - sizing. Heterogeneous-scene indexing is pinned separately by - ``test_heterogeneous_scene_per_view_shapes``. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base, "panda fixture must be fixed-base for this test" - - g = articulation.data.gravity_compensation_forces.torch - - expected_shape = (num_articulations, articulation.num_joints) - assert g.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(g.shape)}" - assert g.dtype == torch.float32 - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.isaacsim_ci -def test_get_jacobians_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): - """Floating-base ``body_link_jacobian_w`` keeps every body row and prepends 6 base-DoF columns. - - Contract for floating-base: shape - ``(N, num_bodies, 6, num_joints + num_base_dofs)`` — no fixed-root row - to drop, and the leading 6 DoF columns are the floating-base spatial- - velocity columns Newton's ``eval_jacobian`` writes for the free root - joint. Matches the cross-library industry convention (Pinocchio, Drake, - MuJoCo, RBDL, OCS2, iDynTree). - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - assert not articulation.is_fixed_base, "anymal fixture must be floating-base for this test" - - J = articulation.data.body_link_jacobian_w.torch - - expected_shape = ( - num_articulations, - articulation.num_bodies, - 6, - articulation.num_joints + articulation.num_base_dofs, - ) - assert J.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(J.shape)}" - assert J.dtype == torch.float32 - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.isaacsim_ci -def test_get_mass_matrix_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): - """Floating-base ``mass_matrix`` shape ``(N, num_joints + 6, num_joints + 6)``. - - Includes the 6 floating-base rows/cols on the DoF axis, matching the - cross-library industry convention. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - sim.step() - articulation.update(sim.cfg.dt) - - M = articulation.data.mass_matrix.torch - - expected_dofs = articulation.num_joints + articulation.num_base_dofs - expected_shape = (num_articulations, expected_dofs, expected_dofs) - assert M.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(M.shape)}" - assert M.dtype == torch.float32 - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.isaacsim_ci -def test_get_gravity_compensation_forces_shape_floating_base( - sim, num_articulations, device, add_ground_plane, articulation_type -): - """Floating-base ``gravity_compensation_forces`` shape ``(N, num_joints + 6)``. - - Includes the 6 floating-base entries on the DoF axis, matching the - cross-library industry convention. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - assert not articulation.is_fixed_base, "anymal fixture must be floating-base for this test" - - g = articulation.data.gravity_compensation_forces.torch - - expected_shape = (num_articulations, articulation.num_joints + articulation.num_base_dofs) - assert g.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(g.shape)}" - assert g.dtype == torch.float32 - - -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.isaacsim_ci -def test_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, articulation_type): - """Mixed-articulation scene: each view returns ITS OWN asset's shape. - - Direct regression test for the Codex round-2 finding. With Franka - (9 DoFs) and Anymal-C (18 DoFs) co-resident in the model, - ``model.max_dofs_per_articulation == 18`` and - ``model.max_joints_per_articulation == anymal.num_bodies``. The Franka - view's ``body_link_jacobian_w`` / ``mass_matrix`` outputs must use - Franka's per-asset counts, NOT the model-wide maxima — otherwise - Franka's mass matrix would carry zero-padded rows/cols and be - singular. - - Uses the ``anymal`` ``SIM_CFGs`` entry (more capable solver settings) - for the host sim; the ``articulation_type`` parametrize is only there - so the ``sim`` fixture picks a config — the test itself constructs - both Anymal and Franka articulations directly. - """ - # ``num_per_type=1`` keeps the actuator-default replication path off — - # Newton's USD default loader hits a (1, num_joints) vs (num_envs, - # num_joints) shape mismatch with multi-instance multi-type scenes; one - # of each is the minimum heterogeneous setup that still exercises the - # per-articulation shape gate without that pre-existing quirk. - num_per_type = 1 - - franka_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Env_franka_[^/]*/Robot") - anymal_cfg = ANYMAL_C_CFG.replace(prim_path="/World/Env_anymal_[^/]*/Robot") - - for i in range(num_per_type): - sim_utils.create_prim(f"/World/Env_franka_{i}", "Xform", translation=(2.5 * i, 0.0, 0.0)) - sim_utils.create_prim(f"/World/Env_anymal_{i}", "Xform", translation=(2.5 * i, 5.0, 0.0)) - - franka = Articulation(franka_cfg) - anymal = Articulation(anymal_cfg) - sim.reset() - assert franka.is_initialized and anymal.is_initialized - assert franka.is_fixed_base and not anymal.is_fixed_base - - # Sanity: the model-wide maxima are larger than at least one view's - # per-asset count, so a regression to model-wide sizing would manifest - # as wrong shapes here. Assert that precondition explicitly so the test - # fails clearly if the fixture stops being heterogeneous. - model = SimulationManager.get_model() - assert model.max_dofs_per_articulation > min(franka.num_joints, anymal.num_joints), ( - "scene is no longer heterogeneous; this test relies on model.max_dofs > one view's num_joints" - ) - - franka_J = franka.data.body_link_jacobian_w.torch - anymal_J = anymal.data.body_link_jacobian_w.torch - - # Each view's output uses its OWN per-asset count, not the model-wide max. - # Floating-base assets prepend ``num_base_dofs`` floating-base columns; fixed-base - # assets have ``num_base_dofs == 0``. - franka_dofs = franka.num_joints + franka.num_base_dofs - anymal_dofs = anymal.num_joints + anymal.num_base_dofs - assert franka_J.shape == torch.Size((num_per_type, franka.num_bodies - 1, 6, franka_dofs)), ( - f"Franka jacobian leaked model-wide shape: got {tuple(franka_J.shape)}" - ) - assert anymal_J.shape == torch.Size((num_per_type, anymal.num_bodies, 6, anymal_dofs)), ( - f"Anymal jacobian leaked model-wide shape: got {tuple(anymal_J.shape)}" - ) - - sim.step() - franka.update(sim.cfg.dt) - anymal.update(sim.cfg.dt) - - franka_M = franka.data.mass_matrix.torch - anymal_M = anymal.data.mass_matrix.torch - - assert franka_M.shape == torch.Size((num_per_type, franka_dofs, franka_dofs)) - assert anymal_M.shape == torch.Size((num_per_type, anymal_dofs, anymal_dofs)) - - # Each view's mass matrix must have positive diagonals — padded zero - # rows/cols (the round-2 bug) would surface as zero diagonals on the - # smaller-DoF view. Using a per-diagonal check here instead of det() - # because det of a real Franka mass matrix is naturally ~1e-13. - assert (franka_M.diagonal(dim1=-2, dim2=-1) > 1e-6).all(), ( - "Franka mass matrix has non-positive diagonal under heterogeneous scene" - ) - assert (anymal_M.diagonal(dim1=-2, dim2=-1) > 1e-6).all(), ( - "Anymal mass matrix has non-positive diagonal under heterogeneous scene" - ) - - # Gravity compensation gathers a FLAT model-wide DoF buffer, so a padded-layout - # regression (indexing by ``art_id * max_dofs`` instead of - # ``joint_qd_start[articulation_start[art_id]]``) is numerically invisible in - # homogeneous scenes — this mixed scene is the only place it can surface. - franka_g = franka.data.gravity_compensation_forces.torch - anymal_g = anymal.data.gravity_compensation_forces.torch - assert franka_g.shape == torch.Size((num_per_type, franka_dofs)) - assert anymal_g.shape == torch.Size((num_per_type, anymal_dofs)) - assert franka_g.abs().max() > 1e-3, "Franka gravity compensation is all-zero under heterogeneous scene" - 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 post-step. 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) - 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) - sim.step() - 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("articulation_type", ["panda", "anymal"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci -def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulation_type, gravity_enabled): - """The joint-space mass matrix ``M(q)`` must be square, symmetric, and positive-definite. - - This pins three structural properties of - :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`: - - * **Square**: shape ``(N, num_joints + num_base_dofs, num_joints + num_base_dofs)``. - A transposed gather or a non-square scratch buffer would be caught - here before downstream OSC inversion silently propagates garbage. - * **Symmetric**: ``M == M.T`` to numerical precision. The joint- - space inertia tensor is symmetric by construction; an asymmetric - result indicates a wrong-axis gather, half-populated buffer, or - Cholesky-input bug. - * **Positive-definite**: ``torch.linalg.cholesky(M)`` succeeds. OSC - computes ``M_b = (J · M^-1 · J^T)^-1`` which requires PD on every - step. A non-PD M would fail downstream as ``LinAlgError``; this - test catches it earlier and pinpoints the source. - - Parameterized on both fixed-base (panda) and floating-base (anymal). - Both backends include the floating-base DoF rows/cols on the front of - the DoF axis for floating-base assets. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - sim.step() - articulation.update(sim.cfg.dt) - - M = articulation.data.mass_matrix.torch # (N, J, J) - assert M.dim() == 3, f"expected 3-D mass matrix, got shape {tuple(M.shape)}" - assert M.shape[0] == num_articulations - assert M.shape[1] == M.shape[2], f"mass matrix is not square: {tuple(M.shape)}" - - # Symmetric to numerical precision. - 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" - - # Positive-definite via Cholesky. Adds a tiny diagonal jitter to - # tolerate the floor of float32 PD eigenvalues without masking real - # non-PD bugs (the jitter is well below realistic inertia scales). - eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) - torch.linalg.cholesky(M + 1e-6 * eye) - - -@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.parametrize("ordering_mode", ["none", "reversed"]) -@pytest.mark.isaacsim_ci -def test_get_gravity_compensation_forces_matches_jacobian_gravity( - sim, num_articulations, device, add_ground_plane, articulation_type, ordering_mode -): - """``g(q)`` must equal ``-sum_b J_com_b^T (m_b * g_w)`` at the current configuration. - - 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 - root pose). - - With ``ordering_mode="reversed"`` a nonidentity joint ordering is active and - both sides of the identity must be expressed in user joint order: the - 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. - """ - 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) - sim.reset() - assert articulation.is_initialized - - # Non-trivial configuration via manual writes (no sim step, so the assert - # compares both quantities at exactly this state): random joint offsets, - # and for floating-base a non-identity root pose. - torch.manual_seed(0) - q = articulation.data.default_joint_pos.torch + 0.3 * torch.randn( - num_articulations, articulation.num_joints, device=device - ) - articulation.write_joint_position_to_sim_index(position=q) - if not articulation.is_fixed_base: - root_pose = articulation.data.default_root_pose.torch.clone() - root_pose[:, 2] += 1.0 - # (x, y, z, w) quaternion — 30 deg roll about x. - root_pose[:, 3:] = torch.tensor([0.2588, 0.0, 0.0, 0.9659], device=device) - articulation.write_root_pose_to_sim_index(root_pose=root_pose) - # Guard against a vacuous identity: if root-pose FK invalidation ever - # regressed, both sides would be evaluated at the stale identity pose and - # 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) - - 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); - # the compensation force is its negation. The COM-referenced Jacobian is exactly the - # right lever arm for a point gravity force acting at each body's COM. - J_com = articulation.data.body_com_jacobian_w.torch # (N, B_jac, 6, D) - masses = articulation.data.body_mass.torch # (N, num_bodies) - if articulation.is_fixed_base: - # jacobi_body_idx == body_idx - 1 for fixed-base (fixed-root row excluded). - masses = masses[:, 1:] - model = SimulationManager.get_model() - gravity_w = wp.to_torch(model.gravity[: model.world_count]) # (num_worlds, 3) - assert gravity_w.shape[0] == num_articulations, "fixture must place one articulation per world" - f_gravity = masses.unsqueeze(-1) * gravity_w.unsqueeze(1) # (N, B_jac, 3) - g_expected = -torch.einsum("nbij,nbi->nj", J_com[:, :, 0:3, :], f_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.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci -def test_jacobian_refreshes_after_manual_joint_write( - sim, num_articulations, device, articulation_type, gravity_enabled -): - """After ``write_joint_position_to_sim_index`` (no sim step), the Jacobian read - must reflect the new joint state — not the previous one. - - Catches: - - Missing FK trigger in :attr:`body_com_jacobian_w` (eval_jacobian uses stale - ``state.body_q``). - - Missing FK trigger in :attr:`body_link_jacobian_w` shift kernel. - - The contract: ``J`` read directly after a manual write must equal ``J`` read - after ``sim.step + update`` — the latter is the ground-truth fresh-FK reference. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) - - # Read J / M 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() - - # Manually write a different joint state — large delta to make Jacobian change visible. - # No sim.step / update — FK becomes 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) - - # If the FK trigger works: forward() runs, body_q is refreshed to match q_target, - # eval_jacobian / shift kernel see fresh body poses, J reflects q_target → differs from J at baseline. - # If the trigger is missing: body_q stays at baseline, J unchanged from J_link_0 / J_com_0. - J_link_1 = articulation.data.body_link_jacobian_w.torch.clone() - J_com_1 = articulation.data.body_com_jacobian_w.torch.clone() - - assert not torch.allclose(J_link_0, J_link_1, atol=1e-3), ( - "body_link_jacobian_w did not change after manual joint write — " - "FK trigger likely missing (eval_jacobian / shift kernel reading stale state.body_q)." - ) - assert not torch.allclose(J_com_0, J_com_1, atol=1e-3), ( - "body_com_jacobian_w did not change after manual joint write — FK trigger likely missing before eval_jacobian." - ) - - -@pytest.mark.parametrize("num_articulations", [1]) -@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_mass_matrix_refreshes_after_manual_joint_write( - sim, num_articulations, device, articulation_type, gravity_enabled -): - """After ``write_joint_position_to_sim_index`` (no sim step), the mass matrix read - must reflect the new joint state. - - The mass matrix depends on ``q`` (joint positions) through the body-spatial-inertia - transformation in eval_mass_matrix's ``compute_body_spatial_inertia`` step, which - reads ``state.body_q``. Same FK-staleness pattern as the Jacobian. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) - - M_0 = articulation.data.mass_matrix.torch.clone() - q_target = articulation.data.joint_pos.torch.clone() + 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) - M_1 = articulation.data.mass_matrix.torch.clone() - - assert not torch.allclose(M_0, M_1, atol=1e-3), ( - "mass_matrix did not change after manual joint write — " - "FK trigger likely missing before eval_mass_matrix (compute_body_spatial_inertia " - "reads stale state.body_q)." - ) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_gravity_compensation_refreshes_after_manual_joint_write(sim, num_articulations, device, articulation_type): - """After ``write_joint_position_to_sim_index`` (no sim step), the gravity - compensation read must reflect the new joint state. - - ``g(q)`` depends on ``q`` through the RNEA pass in ``eval_inverse_dynamics_passive``, - which reads ``state.body_q``. Same FK-staleness pattern as the Jacobian and - the mass matrix. Gravity stays enabled (the default) — with gravity off, - ``g(q)`` is identically zero and the assert would be vacuous. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) - - g_0 = articulation.data.gravity_compensation_forces.torch.clone() - q_target = articulation.data.joint_pos.torch.clone() + 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) - g_1 = articulation.data.gravity_compensation_forces.torch.clone() - - assert not torch.allclose(g_0, g_1, atol=1e-3), ( - "gravity_compensation_forces did not change after manual joint write — " - "FK trigger likely missing before eval_inverse_dynamics_passive." - ) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulations, device, articulation_type): - """Newton accuracy: ``τ_gc`` must hold the manipulator in static equilibrium. - - Newton-side variant of the PhysX test of the same name (backend parity). - The contract is the EOM identity ``M(q) q̈ + C(q,q̇) q̇ + g(q) = τ_input``. - Setting ``τ_input = g(q)`` at ``q̇ = 0`` gives ``q̈ = 0`` — the arm should - not move. This pins - :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` - in isolation: sign errors, frame errors, and DoF-ordering errors all - surface as joint drift, while a controller-level test would have those - bugs averaged out by PD damping. - """ - base_cfg = generate_articulation_cfg(articulation_type=articulation_type) - # Replace default Franka actuators with a passthrough implicit actuator - # (stiffness = 0, damping = 0). With both gains zero the effort target - # we set IS the joint torque applied — no PD spring-damper masks the - # gravity-comp signal. Default Franka cfg has stiffness=80 / damping=4 - # which would absorb gravity through PD bias and hide accessor bugs. - cfg = base_cfg.replace( - actuators={ - "all": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=0.0, - damping=0.0, - ), - }, - ) - # FRANKA_PANDA_CFG has rigid_props.disable_gravity=False already, but be - # defensive — gravity must be ON for τ_gc to have anything to cancel. - cfg = cfg.replace( - spawn=cfg.spawn.replace( - rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False), - ), - ) - - articulation, _ = generate_articulation(cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - # Force a clean static state: default joint positions, zero velocities. - # ``sim.reset`` may leave residual ``q_dot`` from solver settling under - # gravity, so we pin it explicitly here. - default_q = articulation.data.default_joint_pos.torch.clone() - default_qd = torch.zeros_like(default_q) - articulation.write_joint_position_to_sim_index(position=default_q) - articulation.write_joint_velocity_to_sim_index(velocity=default_qd) - articulation.update(sim.cfg.dt) - - # Default joint pose from FRANKA_PANDA_CFG bends the elbow - # (joint2=-0.569, joint4=-2.81, joint6=3.04) so several links carry a - # gravity load — τ_gc is non-trivial in this configuration. A natural- - # hang pose (all zeros) would produce near-zero τ_gc and make this - # test uninformative. - init_q = articulation.data.joint_pos.torch.clone() - - # Step 100 times applying only τ_gc as joint efforts. - for _ in range(100): - # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)`` - # — leading ``num_base_dofs`` floating-base entries (0 on fixed-base) followed - # by the actuated-joint entries. Slice past the floating-base entries so the - # remaining tensor aligns with ``set_joint_effort_target_index`` (actuated only). - tau_gc = articulation.data.gravity_compensation_forces.torch[:, articulation.num_base_dofs :] - articulation.set_joint_effort_target_index(target=tau_gc) - articulation.write_data_to_sim() - sim.step() - articulation.update(sim.cfg.dt) - - final_q = articulation.data.joint_pos.torch - drift = (final_q - init_q).abs().max() - # Tight bound: 5e-3 rad ≈ 0.3°. Numerical integration over 100 steps will - # accumulate some floor (sub-millirad on Franka), but a sign or frame bug - # in τ_gc produces drift of at least a degree per step on bent-elbow - # poses. This bound separates "correct" from "broken" cleanly. - assert drift < 5e-3, ( - f"max joint drift {drift:.5f} rad after 100 gravity-comp-only steps —" - " τ_gc did not hold static equilibrium. Check sign, DoF ordering, and" - " whether gravity_compensation_forces returns g(q) (positive) or" - " its negation." - ) - - -@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]) -@pytest.mark.isaacsim_ci -def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_enabled): - """Newton-side OSC pose tracking sentinel. - - 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 - (: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 - oracle. - - OSC runs with ``gravity_compensation=False`` and scene gravity disabled - so the sentinel isolates the J/M bridge; the gravity-compensation path is - covered by :func:`test_franka_osc_gravity_compensation_precision`. - ``inertial_dynamics_decoupling=True`` - exercises ``mass_matrix`` and the Newton COM-referenced J → - M_b → J product. The actuator PD is zeroed at cfg time so OSC's - joint-effort output is not opposed by ``kp·(target − q)``. - """ - robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim, zero_actuator_pd=True) - - osc = OperationalSpaceController( - OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ), - num_envs=1, - device=device, - ) - - 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) - - pos_history: list[float] = [] - rot_history: list[float] = [] - for _ in range(800): - jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) - mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] - ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) - joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] - ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) - - osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) - joint_efforts = osc.compute( - jacobian_b=jacobian_b, - current_ee_pose_b=ee_pose_b, - current_ee_vel_b=ee_vel_b, - mass_matrix=mass_matrix, - gravity=None, - ) - - robot.set_joint_effort_target(joint_efforts, 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(f"OSC_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. 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 - # 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 - # ``mass_matrix`` per step. - assert pos_mean < 5e-3, f"OSC pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" - assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" - - -@pytest.mark.parametrize("device", ["cuda:0"]) -@pytest.mark.parametrize("articulation_type", ["panda_fine"]) -@pytest.mark.parametrize("gravity_enabled", [True]) -@pytest.mark.isaacsim_ci -def test_franka_osc_gravity_compensation_precision(sim, device, articulation_type, gravity_enabled): - """Two-phase EE hold: gravity sag without compensation, tight hold with it. - - Same OSC pose-hold loop as :func:`test_franka_osc_tracking_accuracy`, but - with scene and per-body gravity ON and the target pinned to the initial EE - pose, so any steady-state error is pure gravity sag. Phase 1 runs with - ``gravity_compensation=False`` and must sag past a floor; phase 2 flips - ``osc.cfg.gravity_compensation`` — read per :meth:`compute` call, so the - flag is the only variable across phases (the gravity tensor is fetched and - passed in both) — and must recover the hold to under 0.1 mm. - - The floor assertion keeps the test discriminating: if the task stiffness - is ever raised high enough to mask gravity, phase 1 stops clearing the - floor and the test fails loudly instead of silently passing on a - non-discriminating setup. The gravity feed-forward consumes - :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` - (Newton RNEA via ``eval_inverse_dynamics_passive``) live in the loop, covering the - FK-staleness refresh on every step of phase 2. - - The task stiffness (500) deliberately matches the PhysX-side OSC - gravity-compensation test for a cross-backend-comparable setup, and the - ``panda_fine`` sim config (4 solver substeps) is load-bearing: at a single - 1/120 substep MJWarp implicitfast mis-integrates the gravity-loaded hold - into a 0.4-1.0 mm limit cycle that never settles (dt-linear, worsened by - higher damping gains, gone in zero gravity — solver integration error, - not a compensation error). With 4 substeps the same per-control-step - torques hold dead-still at ~1 um, quieter than PhysX (OVPhysX) at ~8 um. - The uncompensated sag agrees with PhysX to ~1% (23.7 vs 23.9 mm), - independently validating the gravity forces. Both phases reach a true - steady state here, enforced by tail-half stationarity guards. - """ - robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( - sim, zero_actuator_pd=True, disable_gravity=False - ) - - osc = OperationalSpaceController( - OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ), - num_envs=1, - device=device, - ) - - sim.step() - robot.update(sim.cfg.dt) - # Hold the initial EE pose: phase-1 steady-state error is pure gravity sag. - target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.0, 0.0, 0.0), device) - - def run_phase(num_steps: int) -> list[float]: - pos_history: list[float] = [] - for _ in range(num_steps): - jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) - mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] - gravity = robot.data.gravity_compensation_forces.torch[:, arm_joint_ids] - ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) - joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] - ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) - - osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) - joint_efforts = osc.compute( - jacobian_b=jacobian_b, - current_ee_pose_b=ee_pose_b, - current_ee_vel_b=ee_vel_b, - mass_matrix=mass_matrix, - gravity=gravity, - ) - robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) - robot.write_data_to_sim() - sim.step() - robot.update(sim.cfg.dt) - - pos_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()) - return pos_history - - def _stationary_tail_mean(history, label): - """Mean of the last 200 samples, asserting the two tail halves agree within 25%. - - The relative check carries a 10 µm absolute floor: at the ~1 µm solver noise - floor of the compensated hold, tail jitter is far below the 0.1 mm verdict - threshold and cannot flip the outcome, so demanding 25% relative agreement - of micrometer-scale means would only add GPU-dependent flakiness. - """ - a = sum(history[-200:-100]) / 100 - b = sum(history[-100:]) / 100 - mean = (a + b) / 2.0 - assert abs(a - b) < 0.25 * max(mean, 1e-5), ( - f"{label} not stationary: tail halves {a:.6f} vs {b:.6f} — extend the phase" - ) - return mean - - hist_off = run_phase(400) - osc.cfg.gravity_compensation = True - hist_on = run_phase(600) - - 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" - assert pos_on < 1e-4, f"compensated hold {pos_on:.6f} > 0.1 mm — gravity compensation inaccurate" - assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.6f} (<10x)" - - -if __name__ == "__main__": - pytest.main([__file__, "-v", "--maxfail=1"]) + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + assert articulation.data.root_com_lin_vel_w.torch[1, 0] > initial_velocity[1, 0] + torch.testing.assert_close(articulation.data.root_com_lin_vel_w.torch[0], initial_velocity[0], atol=1e-6, rtol=0) 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 06d516c12137..b3a2c5d5fdd1 100644 --- a/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py +++ b/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py @@ -10,6 +10,8 @@ import warp as wp from isaaclab_newton.assets.articulation import kernels as articulation_kernels +pytestmark = pytest.mark.unit + def _selector(values: list[int], dtype: type) -> wp.array: """Create a CPU Warp selector with the requested integer width.""" 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 1e99e95edd88..5575a80af33a 100644 --- a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py +++ b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py @@ -3,985 +3,152 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PD actuator equivalence tests on ANYmal-C (floating-base quadruped). +"""Kitless real-solver equivalence test for Newton-native actuators.""" -Compares IsaacLab-native actuators against Newton-native actuators (created -from the same Lab configs via USD authoring) on the Newton physics backend. -Both paths must produce identical joint trajectories within tolerance. - -Using ANYmal-C — a 12-DOF quadruped on a floating base — exercises the -coordinate-vs-DOF index separation that is critical when free joints shift -the mapping between ``joint_q`` (coordinate layout) and ``joint_qd`` -(DOF layout). - -Each test class overrides ANYmal's default actuators with a specific Lab -config (IdealPD, DCMotor, or mixed) and verifies Lab vs Newton equivalence. -""" - -from isaaclab.app import AppLauncher - -simulation_app = AppLauncher(headless=True).app - -import functools -import os -import unittest - -import numpy as np +import pytest import torch -import warp as wp from isaaclab_newton.assets import Articulation from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics import NewtonManager as SimulationManager +from pxr import Gf, UsdPhysics + import isaaclab.sim as sim_utils from isaaclab.actuators import IdealPDActuatorCfg -from isaaclab.actuators.newton import read_group_parameter -from isaaclab.actuators.newton.kernels import sync_torque_telemetry +from isaaclab.assets import ArticulationCfg 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, - MIXED_WITH_IMPLICIT_ACTUATORS, - ActuatorStateResetBase, - EquivalenceAssertionsMixin, - MockEnv, - build_dr_term, - make_dummy_lstm_checkpoint, - make_dummy_mlp_checkpoint, -) -from isaaclab.test.utils.articulation_ordering import assert_articulation_ordering_trace_matches - -from isaaclab_assets import ANYMAL_C_CFG -from isaaclab_assets.robots.spot import joint_parameter_lookup as SPOT_KNEE_LOOKUP - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- - -NUM_ENVS = 2 -NUM_STEPS = 10 -DT = 1.0 / 120.0 -TARGET_OFFSET = 0.1 # [rad] added to initial joint positions - -NEWTON_CFG = NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=500, - nconmax=500, - ls_iterations=20, - cone="pyramidal", - impratio=1, - integrator="implicitfast", - ), - num_substeps=1, - debug_mode=False, - use_cuda_graph=False, -) - -# --------------------------------------------------------------------------- -# Simulation runner -# --------------------------------------------------------------------------- - -def _run_simulation( - actuators: dict, - use_newton_actuators: bool, - *, - dt: float = DT, - newton_cfg: NewtonCfg = NEWTON_CFG, - num_steps: int = NUM_STEPS, - decimation: int = 1, - feedforward: float | None = None, - joint_ordering: tuple[str, ...] | None = None, - permutation_sensitive_commands: bool = False, -) -> dict: - """Run ANYmal-C and return recorded trajectories + telemetry. +pytestmark = pytest.mark.integration - Always records ``joint_pos``, ``joint_vel``, ``computed_effort``, and - ``applied_effort`` so callers don't need a separate "with telemetry" - runner. Optionally applies a constant per-DOF feedforward effort target. - Args: - actuators: Actuator config dict overriding ANYmal's defaults. - use_newton_actuators: Use Newton-native actuators when ``True``. - dt: Physics timestep [s]. - newton_cfg: Newton physics configuration. - num_steps: Number of policy-level steps. - decimation: Actuator steps per policy step (Newton's CUDA-graph - d-loop is used when all-graphable; otherwise an explicit Python - inner loop). - feedforward: When not ``None``, set a constant per-DOF feedforward - effort target. Used by the implicit-FF equivalence test. - joint_ordering: Optional explicit public joint-name order. - permutation_sensitive_commands: Whether to command distinct position, velocity, and effort values by - physical joint name. - - Returns: - Recorded joint-name metadata, commands, public trajectories and torque telemetry, and backend-order - adapter effort traces. - """ - sim_cfg = SimulationCfg(dt=dt, physics=newton_cfg, use_newton_actuators=use_newton_actuators) - 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=actuators, - prim_path="/World/Env_[^/]*/Robot", - joint_ordering=joint_ordering, - ) - articulation = Articulation(art_cfg) - sim.reset() - assert articulation.is_initialized - - if use_newton_actuators and decimation > 1: - SimulationManager.set_decimation(decimation) - - handles_dec = ( - use_newton_actuators - and decimation > 1 - and SimulationManager._is_all_graphable() - and SimulationManager._decimation > 1 - ) - - joint_names = tuple(articulation.joint_names) - backend_joint_names = tuple(articulation.backend_joint_names) - installed_ordering = articulation.joint_ordering - joint_ordering_state = ( - None - if installed_ordering is None - else { - "user_names": joint_names, - "backend_names": backend_joint_names, - "user_to_backend_indices": installed_ordering.user_to_backend_indices, - "backend_to_user_indices": installed_ordering.backend_to_user_indices, - "is_identity": False, - } - ) - init_pos = wp.to_torch(articulation.data.joint_pos).clone() - if permutation_sensitive_commands: - scale_by_name = {name: index + 1 for index, name in enumerate(backend_joint_names)} - joint_scale = torch.tensor( - [scale_by_name[name] for name in joint_names], - device=articulation.device, - dtype=init_pos.dtype, - ).unsqueeze(0) - joint_scale = joint_scale.expand_as(init_pos) - target_pos = init_pos + 0.01 * joint_scale - target_vel = 0.001 * joint_scale - effort_target = 0.1 * joint_scale - else: - target_pos = init_pos + TARGET_OFFSET - target_vel = torch.zeros_like(init_pos) - effort_target = None if feedforward is None else torch.full_like(init_pos, feedforward) - - articulation.set_joint_position_target_index(target=target_pos) - articulation.set_joint_velocity_target_index(target=target_vel) - if effort_target is not None: - articulation.set_joint_effort_target_index(target=effort_target) - - recorded_pos, recorded_vel = [], [] - recorded_computed_effort, recorded_applied_effort = [], [] - recorded_adapter_applied = [] - for _ in range(num_steps): - if handles_dec: - articulation.write_data_to_sim() - sim.step() - articulation.update(dt * decimation) - else: - for _ in range(decimation): - articulation.write_data_to_sim() - sim.step() - articulation.update(dt) - recorded_pos.append(wp.to_torch(articulation.data.joint_pos).clone()) - recorded_vel.append(wp.to_torch(articulation.data.joint_vel).clone()) - recorded_computed_effort.append(articulation.actuators.computed_effort.torch.clone()) - recorded_applied_effort.append(articulation.actuators.applied_effort.torch.clone()) - if use_newton_actuators: - recorded_adapter_applied.append(wp.to_torch(articulation.data._sim_bind_joint_effort).clone()) - - return { - "joint_names": joint_names, - "backend_joint_names": backend_joint_names, - "joint_ordering": joint_ordering_state, - "adapter_joint_names": backend_joint_names, - "joint_pos": recorded_pos, - "joint_vel": recorded_vel, - "computed_effort": recorded_computed_effort, - "applied_effort": recorded_applied_effort, - "adapter_applied_effort": recorded_adapter_applied, - "target_pos": target_pos.clone(), - "target_vel": target_vel.clone(), - "effort_target": None if effort_target is None else effort_target.clone(), - } - - -def test_newton_actuator_rollout_matches_reversed_joint_ordering() -> None: - """Match Newton-backend actuator traces under reversed public joint ordering.""" - identity_result = _run_simulation( - IDEAL_PD_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, - use_newton_actuators=True, - joint_ordering=requested_joint_names, - permutation_sensitive_commands=True, +def _author_two_link_articulations() -> Articulation: + """Author two local one-DOF articulations for actuator equivalence.""" + link_cfg = sim_utils.CuboidCfg( + size=(0.4, 0.1, 0.1), + rigid_props=sim_utils.RigidBodyBaseCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionBaseCfg(), ) - - installed_ordering = reversed_result["joint_ordering"] - assert installed_ordering is not None - assert not installed_ordering["is_identity"] - assert_articulation_ordering_trace_matches(identity_result, reversed_result, requested_joint_names) - - -# --------------------------------------------------------------------------- -# Base test class -# --------------------------------------------------------------------------- - - -class _EquivalenceTestBase(EquivalenceAssertionsMixin, unittest.TestCase): - """Base for Lab-vs-Newton equivalence tests. - - Subclasses set ``actuators`` to the config under test. ``setUpClass`` - runs the simulation with both ``use_newton_actuators=False`` (Lab path) - and ``True`` (Newton path) and stores the results. The ``test_*_match`` - oracles come from :class:`EquivalenceAssertionsMixin`. - """ - - __test__ = False - actuators: dict = {} - feedforward: float | None = None - dt: float = DT - newton_cfg: NewtonCfg = NEWTON_CFG - num_steps: int = NUM_STEPS - decimation: int = 1 - - @classmethod - def setUpClass(cls): - kwargs = dict( - feedforward=cls.feedforward, - dt=cls.dt, - newton_cfg=cls.newton_cfg, - num_steps=cls.num_steps, - decimation=cls.decimation, - ) - cls.lab_result = _run_simulation(cls.actuators, use_newton_actuators=False, **kwargs) - cls.newton_result = _run_simulation(cls.actuators, use_newton_actuators=True, **kwargs) - - -# --------------------------------------------------------------------------- -# Equivalence tests with different actuator types -# --------------------------------------------------------------------------- - - -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.""" - - __test__ = True - actuators = DC_MOTOR_ACTUATORS - - -class TestMixedWithImplicitEquivalence(_EquivalenceTestBase): - """Implicit HAA + IdealPD HFE + DCMotor KFE: Lab vs Newton. - - Verifies that implicit actuators (handled by the physics engine's - built-in joint drives) coexist correctly with explicit Newton actuators. - """ - - __test__ = True - actuators = MIXED_WITH_IMPLICIT_ACTUATORS - - -# --------------------------------------------------------------------------- -# Implicit + non-zero feedforward effort target -# --------------------------------------------------------------------------- - - -class TestImplicitWithFeedforwardEquivalence(_EquivalenceTestBase): - """Implicit-only actuators with a non-zero feedforward effort target. - - Verifies that the user's FF effort lands additively on top of the - simulator's joint-drive PD identically on both Lab and Newton paths. - """ - - __test__ = True - actuators = IMPLICIT_ONLY_ACTUATORS - feedforward = 2.0 - torque_atol = 0.5 - - -# --------------------------------------------------------------------------- -# Multi-articulation Newton scene (regression test for class-attr clobber) -# --------------------------------------------------------------------------- - - -def _run_anymal_and_cartpole(use_newton_actuators: bool, *, num_steps: int = NUM_STEPS) -> dict: - """Spawn ANYmal-C + Cartpole per env (different DOF counts, different base types).""" - from isaaclab_assets import CARTPOLE_CFG # noqa: PLC0415 - - sim_cfg = SimulationCfg(dt=DT, physics=NEWTON_CFG, use_newton_actuators=use_newton_actuators) - 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 - - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 6.0, 0, 0)) - - anymal_cfg = ANYMAL_C_CFG.replace(actuators=IDEAL_PD_ACTUATORS, prim_path="/World/Env_[^/]*/Anymal") - cartpole_cfg = CARTPOLE_CFG.replace( - actuators=CARTPOLE_EXPLICIT_ACTUATORS, - prim_path="/World/Env_[^/]*/Cartpole", - ) - # Stand the cartpole well clear of the anymal. - cartpole_cfg.init_state = cartpole_cfg.init_state.replace(pos=(0.0, 3.0, 2.0)) - - anymal = Articulation(anymal_cfg) - cartpole = Articulation(cartpole_cfg) - sim.reset() - assert anymal.is_initialized and cartpole.is_initialized - - init_anymal = wp.to_torch(anymal.data.joint_pos).clone() - init_cartpole = wp.to_torch(cartpole.data.joint_pos).clone() - anymal.set_joint_position_target_index(target=init_anymal + TARGET_OFFSET) - anymal.set_joint_velocity_target_index(target=torch.zeros_like(init_anymal)) - cartpole.set_joint_position_target_index(target=init_cartpole + TARGET_OFFSET) - cartpole.set_joint_velocity_target_index(target=torch.zeros_like(init_cartpole)) - - pos_anymal, pos_cartpole = [], [] - for _ in range(num_steps): - anymal.write_data_to_sim() - cartpole.write_data_to_sim() - sim.step() - anymal.update(DT) - cartpole.update(DT) - pos_anymal.append(wp.to_torch(anymal.data.joint_pos).clone()) - pos_cartpole.append(wp.to_torch(cartpole.data.joint_pos).clone()) - - return {"joint_pos_anymal": pos_anymal, "joint_pos_cartpole": pos_cartpole} - - -class TestHeterogeneousMultiArticulationNewton(unittest.TestCase): - """Two structurally-different articulations (ANYmal floating + Cartpole fixed) on Newton. - - Regression for the singleton-clobber bug in ``NewtonManager._adapter`` - / ``_post_actuator_callback`` — fixed by the global-adapter refactor - + callback-list multiplexing. Heterogeneous DOF counts (12 vs 2) and - base types (floating vs fixed) stress the global adapter's handling - of varied actuator index patterns. Equivalence against the Lab - actuator path is the meaningful end-to-end check: divergence on - either robot would indicate broken stepping. - """ - - @classmethod - def setUpClass(cls): - cls.lab_result = _run_anymal_and_cartpole(use_newton_actuators=False) - cls.newton_result = _run_anymal_and_cartpole(use_newton_actuators=True) - - def test_anymal_matches_lab(self): - for step_i, (lab, newton) in enumerate( - zip(self.lab_result["joint_pos_anymal"], self.newton_result["joint_pos_anymal"]) - ): - torch.testing.assert_close( - newton, - lab, - atol=2e-3, - rtol=1e-3, - msg=f"ANYmal joint_pos diverged from Lab path at step {step_i}", - ) - - def test_cartpole_matches_lab(self): - for step_i, (lab, newton) in enumerate( - zip(self.lab_result["joint_pos_cartpole"], self.newton_result["joint_pos_cartpole"]) - ): - torch.testing.assert_close( - newton, - lab, - atol=2e-3, - rtol=1e-3, - msg=f"Cartpole joint_pos diverged from Lab path at step {step_i}", - ) - - -# --------------------------------------------------------------------------- -# Domain randomization via events.py — Newton backend -# --------------------------------------------------------------------------- - - -class TestRandomizeActuatorGainsViaEventsNewton(unittest.TestCase): - """End-to-end DR test for the Newton backend. - - Drives ``randomize_actuator_gains`` and verifies that kp/kd values reach - the controllers of the articulation's Newton actuators through - ``write_group_parameter``; the assertions read the - controllers back via the public ``read_group_parameter``. - - With ``operation="abs"`` and ``distribution="uniform"`` over a - degenerate range ``(K, K)``, every randomized cell is set to exactly - ``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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=IDEAL_PD_ACTUATORS, - prim_path="/World/Env_[^/]*/Robot", + stage = sim_utils.get_current_stage() + for env_index in range(2): + env_path = f"/World/Env_{env_index}" + robot_path = f"{env_path}/Robot" + root_path = f"{robot_path}/Root" + child_path = f"{robot_path}/Child" + sim_utils.create_prim(env_path, "Xform", translation=(2.0 * env_index, 0.0, 0.0)) + sim_utils.create_prim(robot_path, "Xform") + link_cfg.func(root_path, link_cfg, translation=(0.0, 0.0, 1.0)) + link_cfg.func(child_path, link_cfg, translation=(0.5, 0.0, 1.0)) + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath(root_path)) + joint = UsdPhysics.RevoluteJoint.Define(stage, f"{robot_path}/Joint") + joint.CreateBody0Rel().SetTargets([root_path]) + joint.CreateBody1Rel().SetTargets([child_path]) + joint.CreateAxisAttr().Set("Z") + joint.CreateLocalPos0Attr().Set(Gf.Vec3f(0.25, 0.0, 0.0)) + joint.CreateLocalPos1Attr().Set(Gf.Vec3f(-0.25, 0.0, 0.0)) + joint.CreateLowerLimitAttr().Set(-90.0) + joint.CreateUpperLimitAttr().Set(90.0) + + articulation_cfg = ArticulationCfg( + prim_path="/World/Env_[^/]*/Robot", + articulation_root_prim_path="/Root", + actuators={ + "joint": IdealPDActuatorCfg( + joint_names_expr=["Joint"], + stiffness=20.0, + damping=2.0, + actuator_effort_limit=50.0, ) - anymal = Articulation(art_cfg) - 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 - - 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 6.0, 0, 0)) - - anymal_cfg = ANYMAL_C_CFG.replace(actuators=IDEAL_PD_ACTUATORS, prim_path="/World/Env_[^/]*/Anymal") - cartpole_cfg = CARTPOLE_CFG.replace( - actuators=CARTPOLE_EXPLICIT_ACTUATORS, - prim_path="/World/Env_[^/]*/Cartpole", - ) - cartpole_cfg.init_state = cartpole_cfg.init_state.replace(pos=(0.0, 3.0, 2.0)) - anymal = Articulation(anymal_cfg) - cartpole = Articulation(cartpole_cfg) - sim.reset() - - self.assertIsNotNone(SimulationManager._adapter) - - anymal_read = functools.partial(read_group_parameter, anymal.actuators) - 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() - cartpole_stiffness_before = cartpole_read("all_joints", "controller", "kp").clone() - cartpole_damping_before = cartpole_read("all_joints", "controller", "kd").clone() - - env = MockEnv({"anymal": anymal, "cartpole": cartpole}, NUM_ENVS, anymal.device) - term, asset_cfg = build_dr_term(env, "cartpole") - 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", - ) - - n_cp = cartpole.num_joints - torch.testing.assert_close( - cartpole_read("all_joints", "controller", "kp")[0], torch.full((n_cp,), 100.0, device=anymal.device) - ) - torch.testing.assert_close( - cartpole_read("all_joints", "controller", "kd")[0], torch.full((n_cp,), 5.0, device=anymal.device) - ) - - # ANYmal is untouched (DR was scoped to cartpole). - torch.testing.assert_close(anymal_read("legs", "controller", "kp"), anymal_stiffness_before) - torch.testing.assert_close(anymal_read("legs", "controller", "kd"), anymal_damping_before) - - # Cartpole's other envs are also untouched (env_ids=[0] only). - for env_idx in range(1, NUM_ENVS): - torch.testing.assert_close( - cartpole_read("all_joints", "controller", "kp")[env_idx], cartpole_stiffness_before[env_idx] - ) - torch.testing.assert_close( - cartpole_read("all_joints", "controller", "kd")[env_idx], cartpole_damping_before[env_idx] - ) - - -# --------------------------------------------------------------------------- -# DelayedPD equivalence: PD with actuator command delay -# --------------------------------------------------------------------------- - - -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. - """ - - __test__ = True - actuators = DELAYED_PD_ACTUATORS - - -class TestDelayedPDAuthoring(unittest.TestCase): - """Verify DelayedPDActuatorCfg is authored with NewtonActuatorDelayAPI.""" - - @classmethod - def setUpClass(cls): - cls.result = _run_authoring_introspection(DELAYED_PD_ACTUATORS) - - def test_has_delay(self): - for a in self.result["actuator_info"]: - self.assertTrue(a["has_delay"], "Delay not found on delayed PD actuator") - - def test_controller_is_pd(self): - for a in self.result["actuator_info"]: - self.assertEqual(a["controller_type"], "ControllerPD") - - -# --------------------------------------------------------------------------- -# Decimation tests: re-run equivalence with decimation > 1 + CUDA graph capture -# --------------------------------------------------------------------------- - -NEWTON_CFG_DEC = NewtonCfg( - solver_cfg=MJWarpSolverCfg( - njmax=500, - nconmax=500, - ls_iterations=20, - cone="pyramidal", - impratio=1, - integrator="implicitfast", - ), - num_substeps=2, - debug_mode=False, - use_cuda_graph=True, -) - - -class _DecimationMixin: - """Common knobs for decimation/CUDA-graph variants of equivalence classes.""" - - __test__ = True - dt = 1.0 / 100.0 - newton_cfg = NEWTON_CFG_DEC - num_steps = 5 - 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 -# --------------------------------------------------------------------------- - - -class TestActuatorStateReset(ActuatorStateResetBase, unittest.TestCase): - """Per-env actuator state reset isolation on the Newton backend. - - The scenario and assertions live in :class:`ActuatorStateResetBase`; - this subclass provides the Newton sim config and the model-wide adapter. - """ - - def _make_sim_cfg(self, use_newton_actuators: bool) -> SimulationCfg: - return SimulationCfg(dt=DT, physics=NEWTON_CFG, use_newton_actuators=use_newton_actuators) - - def _make_articulation(self) -> Articulation: - return Articulation(ANYMAL_C_CFG.replace(actuators=DELAYED_PD_ACTUATORS, prim_path="/World/Env_.*/Robot")) - - def _get_adapter(self, articulation): - return SimulationManager._adapter - - -# --------------------------------------------------------------------------- -# RemotizedPD actuator: PD + delay + position-based clamping lookup table -# --------------------------------------------------------------------------- - - -def _remotized_pd_actuators() -> dict: - """RemotizedPD (Spot knee lookup) on KFE with IdealPD on HAA/HFE.""" - from isaaclab.actuators.actuator_pd_cfg import RemotizedPDActuatorCfg # noqa: PLC0415 - - return { - "hips": IdealPDActuatorCfg( - joint_names_expr=[".*HAA", ".*HFE"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ), - "knees": RemotizedPDActuatorCfg( - joint_names_expr=[".*KFE"], - stiffness=60.0, - damping=1.5, - actuator_effort_limit=80.0, - 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 + }, + ) + articulation_cfg._post_spawn(stage) + return Articulation(articulation_cfg) - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=actuator_cfgs, - prim_path="/World/Env_[^/]*/Robot", - ) - articulation = Articulation(art_cfg) +def _run_actuator_path(*, use_newton_actuators: bool) -> dict[str, object]: + """Run the local articulation through one actuator execution path.""" + sim_cfg = SimulationCfg( + device="cpu", + dt=1.0 / 120.0, + gravity=(0.0, 0.0, 0.0), + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg(), use_cuda_graph=False), + use_newton_actuators=use_newton_actuators, + ) + with build_simulation_context(sim_cfg=sim_cfg) as sim: + articulation = _author_two_link_articulations() 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): + adapter = SimulationManager._adapter + execution_state = { + "has_newton_actuators": articulation._has_newton_actuators, + "native_path_active": articulation._actuator_control.native_actuator_path_active, + "manager_path_active": SimulationManager._use_newton_actuators_active, + "adapter_bound": adapter is not None and articulation.newton_actuator_adapter is adapter, + "model_actuator_count": len(SimulationManager.get_model().actuators), + "adapter_actuator_count": 0 if adapter is None else len(adapter.actuators), + "adapter_state_count": 0 if adapter is None else len(adapter._states_a), + } + initial_position = articulation.data.joint_pos.torch.clone() + target_position = initial_position + torch.tensor([[0.10], [-0.15]]) + articulation.actuators.target_command.set_position_index(value=target_position) + articulation.actuators.target_command.set_velocity_index(value=torch.zeros_like(target_position)) + + joint_position = [] + joint_velocity = [] + computed_effort = [] + applied_effort = [] + for _ in range(5): 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, + articulation.update(sim.cfg.dt) + joint_position.append(articulation.data.joint_pos.torch.clone()) + joint_velocity.append(articulation.data.joint_vel.torch.clone()) + computed_effort.append(articulation.actuators.computed_effort.torch.clone()) + applied_effort.append(articulation.actuators.applied_effort.torch.clone()) + + return { + "execution_state": execution_state, + "initial_position": initial_position, + "target_position": target_position, + "joint_position": joint_position, + "joint_velocity": joint_velocity, + "computed_effort": computed_effort, + "applied_effort": applied_effort, + } + + +def test_newton_actuator_real_equivalence() -> None: + """Match one real IdealPD rollout between Isaac Lab and Newton-native execution.""" + lab_result = _run_actuator_path(use_newton_actuators=False) + newton_result = _run_actuator_path(use_newton_actuators=True) + + assert lab_result["execution_state"] == { + "has_newton_actuators": False, + "native_path_active": False, + "manager_path_active": False, + "adapter_bound": False, + "model_actuator_count": 0, + "adapter_actuator_count": 0, + "adapter_state_count": 0, } - - -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"], "ControllerPD") - - 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 - - @classmethod - def setUpClass(cls): - cls.actuators = _remotized_pd_actuators() - super().setUpClass() - - -class TestDecimationRemotizedPD(_DecimationMixin, TestRemotizedPDEquivalence): - """RemotizedPD — decimation=2 + CUDA graph.""" - - -# --------------------------------------------------------------------------- -# Neural network actuator authoring: MLP and LSTM -# --------------------------------------------------------------------------- - - -class TestNeuralMLPAuthoring(unittest.TestCase): - """Verify ActuatorNetMLPCfg is authored as Newton NeuralMLP controller - with DC motor clamping. - """ - - @classmethod - def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetMLPCfg # noqa: PLC0415 - - cls.mlp_path = make_dummy_mlp_checkpoint() - cls.result = _run_authoring_introspection( - { - "mlp_legs": ActuatorNetMLPCfg( - joint_names_expr=[".*HAA"], - network_file=cls.mlp_path, - saturation_effort=120.0, - actuator_effort_limit=80.0, - actuator_velocity_limit=7.5, - pos_scale=-1.0, - vel_scale=1.0, - torque_scale=1.0, - 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"] == "ControllerNeuralMLP"] - 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"] == "ControllerNeuralMLP"] - 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"], - 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"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ), - } - ) - - @classmethod - def tearDownClass(cls): - 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"] == "ControllerNeuralLSTM"] - self.assertTrue(len(lstm_acts) > 0, "No NeuralLSTM controller found") - - def test_lstm_has_dc_motor_clamping(self): - lstm_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "ControllerNeuralLSTM"] - 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_sync_torque_telemetry_keeps_user_order_effort_buffers_unmapped() -> None: - """Report torque telemetry directly from user-order actuator buffers.""" - joint_pos = wp.zeros((1, 3), 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") - user_effort = wp.array(np.asarray([[100.0, 200.0, 300.0]], dtype=np.float32), dtype=wp.float32, device="cpu") - user_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, - wp.zeros_like(joint_pos), - wp.zeros_like(joint_pos), - wp.zeros_like(joint_pos), - wp.zeros_like(joint_pos), - wp.zeros_like(joint_pos), - wp.full((1, 3), 1000.0, dtype=wp.float32, device="cpu"), - joint_modes, - user_effort, - user_computed_effort, - user_to_backend, - False, - ], - outputs=[computed, applied], - device="cpu", - ) - - np.testing.assert_allclose(computed.numpy(), np.asarray([[10.0, 200.0, 30.0]], dtype=np.float32)) - np.testing.assert_allclose(applied.numpy(), np.asarray([[100.0, 200.0, 300.0]], dtype=np.float32)) - - -if __name__ == "__main__": - unittest.main() + assert newton_result["execution_state"] == { + "has_newton_actuators": True, + "native_path_active": True, + "manager_path_active": True, + "adapter_bound": True, + "model_actuator_count": 1, + "adapter_actuator_count": 1, + "adapter_state_count": 1, + } + torch.testing.assert_close(newton_result["target_position"], lab_result["target_position"]) + for key in ("joint_position", "joint_velocity", "computed_effort", "applied_effort"): + for newton_value, lab_value in zip(newton_result[key], lab_result[key], strict=True): + torch.testing.assert_close(newton_value, lab_value, atol=1e-5, rtol=1e-5) + + for result in (lab_result, newton_result): + assert max(value.abs().max() for value in result["computed_effort"]) > 1e-3 + assert max(value.abs().max() for value in result["applied_effort"]) > 1e-3 + initial_error = (result["target_position"] - result["initial_position"]).abs() + final_error = (result["target_position"] - result["joint_position"][-1]).abs() + joint_motion = (result["joint_position"][-1] - result["initial_position"]).abs() + assert torch.all(joint_motion > 1e-5) + assert torch.all(final_error < initial_error) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index f00e5a6011ad..673ba3d20759 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -3,1389 +3,202 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Launch Isaac Sim Simulator first.""" - -from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices - -# launch omniverse app -simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app - -"""Rest everything follows.""" - -import sys -from typing import Literal +"""Kitless real-solver integration tests for Newton rigid objects.""" import pytest import torch import warp as wp -from flaky import flaky from isaaclab_newton.assets import RigidObject from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics import NewtonManager as SimulationManager from newton import ModelFlags +from pxr import UsdPhysics + import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg from isaaclab.sim import SimulationCfg, build_simulation_context -from isaaclab.sim.spawners import materials -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, - quat_rotate, - random_orientation, -) - -NEWTON_SIM_CFG = SimulationCfg( - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg(), - ), -) +pytestmark = pytest.mark.integration -def _newton_sim_context(device, gravity_enabled=True, dt=None, **kwargs): - """Helper to create a Newton simulation context with the correct device. - - When sim_cfg is provided to build_simulation_context, the device, gravity_enabled, and dt - kwargs are ignored. This helper applies them to the shared NEWTON_SIM_CFG before calling. - """ - NEWTON_SIM_CFG.device = device - NEWTON_SIM_CFG.gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - if dt is not None: - NEWTON_SIM_CFG.dt = dt - return build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, **kwargs) - - -def generate_cubes_scene( - num_cubes: int = 1, - height=1.0, - api: Literal["none", "rigid_body", "articulation_root"] = "rigid_body", - kinematic_enabled: bool = False, - device: str = "cuda:0", -) -> tuple[RigidObject, torch.Tensor]: - """Generate a scene with the provided number of cubes. - - Args: - num_cubes: Number of cubes to generate. - height: Height of the cubes. - api: The type of API that the cubes should have. - kinematic_enabled: Whether the cubes are kinematic. - device: Device to use for the simulation. - - Returns: - A tuple containing the rigid object representing the cubes and the origins of the cubes. +_DEVICES = [ + "cpu", + pytest.param( + "cuda:0", + marks=pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available"), + ), +] - """ - origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=origin) - # Resolve spawn configuration - if api == "none": - # since no rigid body properties defined, this is just a static collider - spawn_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - elif api == "rigid_body": - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - elif api == "articulation_root": - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/RigidObject/Cube/dex_cube_instanceable_with_articulation_root.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), +def _newton_sim_context(device: str): + """Create a fresh kitless Newton simulation context.""" + return build_simulation_context( + sim_cfg=SimulationCfg( + device=device, + dt=1.0 / 60.0, + gravity=(0.0, 0.0, 0.0), + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg()), ) - else: - raise ValueError(f"Unknown api: {api}") - - # Create rigid object - cube_object_cfg = RigidObjectCfg( - prim_path="/World/Env_[^/]*/Object", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height)), ) - cube_object = RigidObject(cfg=cube_object_cfg) - - return cube_object, origins - - -@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 - 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.skip(reason="Newton does not support kinematic rigid bodies") -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_kinematic_enabled(num_cubes, device): - """Test that initialization for prim with kinematic flag enabled.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - 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) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - # check that the object is kinematic - default_root_pose = cube_object.data.default_root_pose.torch.clone() - default_root_vel = cube_object.data.default_root_vel.torch.clone() - default_root_pose[:, :3] += origins - torch.testing.assert_close(cube_object.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, default_root_vel) - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_no_rigid_body(num_cubes, device): - """Test that initialization fails when no rigid body is found 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, api="none", device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() +def _spawn_cubes() -> RigidObject: + """Author two local dynamic cuboids and return their Newton asset.""" + for env_index in range(2): + sim_utils.create_prim(f"/World/Env_{env_index}", "Xform", translation=(2.0 * env_index, 0.0, 0.0)) + return RigidObject( + RigidObjectCfg( + prim_path="/World/Env_[^/]*/Object", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyBaseCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionBaseCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + ) -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_articulation_root(num_cubes, device): - """Test that initialization fails when an articulation root is found 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, api="articulation_root", device=device) - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 +def _author_invalid_rigid_object(kind: str) -> RigidObject: + """Author a local prim that is invalid for the Newton rigid-object adapter.""" + dynamic_cfg = sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + collision_props=sim_utils.CollisionBaseCfg(), + rigid_props=sim_utils.RigidBodyBaseCfg(), + ) + if kind == "static": + dynamic_cfg.func("/World/ValidAnchor", dynamic_cfg, translation=(2.0, 0.0, 1.0)) + static_cfg = sim_utils.CuboidCfg(size=(0.2, 0.2, 0.2), collision_props=sim_utils.CollisionBaseCfg()) + static_cfg.func("/World/InvalidObject", static_cfg, translation=(0.0, 0.0, 1.0)) + prim_path = "/World/InvalidObject" + else: + dynamic_cfg.func("/World/InvalidArticulation/Root", dynamic_cfg, translation=(0.0, 0.0, 1.0)) + dynamic_cfg.func("/World/InvalidArticulation/Root/Child", dynamic_cfg, translation=(0.0, 0.0, 0.5)) + stage = sim_utils.get_current_stage() + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath("/World/InvalidArticulation/Root")) + joint = UsdPhysics.FixedJoint.Define(stage, "/World/InvalidArticulation/Root/Joint") + joint.CreateBody0Rel().SetTargets(["/World/InvalidArticulation/Root"]) + joint.CreateBody1Rel().SetTargets(["/World/InvalidArticulation/Root/Child"]) + prim_path = "/World/InvalidArticulation/Root" + return RigidObject(RigidObjectCfg(prim_path=prim_path)) + + +@pytest.mark.parametrize("kind", ["static", "articulation_root"]) +def test_rigid_object_rejects_invalid_local_schema(kind: str) -> None: + """Reject local static and articulation-root prims instead of treating them as rigid objects.""" + with _newton_sim_context("cpu") as sim: + rigid_object = _author_invalid_rigid_object(kind) - # Play sim with pytest.raises(RuntimeError): sim.reset() + assert not rigid_object.is_initialized -@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) - - # play the simulator - 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. - - In this test, 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 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) - - # Play the simulator +@pytest.mark.parametrize("device", _DEVICES) +def test_rigid_object_real_newton_seams(device: str, monkeypatch) -> None: + """Exercise local initialization, partial state/property writes, gravity, and wrench delivery.""" + with _newton_sim_context(device) as sim: + rigid_object = _spawn_cubes() 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 - 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() - - is_global = False - if i % 2 == 0: - is_global = True - positions = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] - else: - positions = None - - # apply force - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=positions, - 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) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - cube_object.data.root_pos_w.torch[0::2, 2], torch.ones(num_cubes // 2, device=sim.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) - - # Play the simulator - 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 - 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() - - is_global = False - if i % 2 == 0: - is_global = True - body_com_pos_w = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] - 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 - - # apply force - cube_object.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, - ) - cube_object.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(5): - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) - - # The first object should be rotating around it's X axis - assert torch.all(torch.abs(cube_object.data.root_ang_vel_b.torch[0::2, 0]) > 0.1) - # 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.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: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - 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), - } - - # 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 - 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) - - if i % 2 == 0: - # reset object - 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 - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_set_material_properties(num_cubes, device): - """Test getting and setting material properties of rigid object via view-level APIs.""" - 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 sim - sim.reset() - - # Get friction/restitution bindings via view-level API - model = SimulationManager.get_model() - friction_binding = cube_object._root_view.get_attribute("shape_material_mu", model)[:, 0] - restitution_binding = cube_object._root_view.get_attribute("shape_material_restitution", model)[:, 0] - num_shapes = friction_binding.shape[1] - - # Set material properties via in-place writes to the warp binding - friction = torch.empty(num_cubes, num_shapes, device=device).uniform_(0.4, 0.8) - restitution = torch.empty(num_cubes, num_shapes, device=device).uniform_(0.0, 0.2) - - wp.to_torch(friction_binding)[:] = friction - wp.to_torch(restitution_binding)[:] = restitution - SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) - - # Simulate physics - sim.step() - cube_object.update(sim.cfg.dt) - - # Verify by reading back from the binding - mu = wp.to_torch(friction_binding) - restitution_check = wp.to_torch(restitution_binding) - torch.testing.assert_close(mu, friction) - torch.testing.assert_close(restitution_check, restitution) - - -def _set_newton_material_properties(cube_object, friction_val, restitution_val, device): - """Helper to set material properties via Newton view-level APIs.""" - model = SimulationManager.get_model() - friction_binding = cube_object._root_view.get_attribute("shape_material_mu", model)[:, 0] - restitution_binding = cube_object._root_view.get_attribute("shape_material_restitution", model)[:, 0] - num_envs = friction_binding.shape[0] - num_shapes = friction_binding.shape[1] - - friction_tensor = torch.full((num_envs, num_shapes), friction_val, device=device) - restitution_tensor = torch.full((num_envs, num_shapes), restitution_val, device=device) - - wp.to_torch(friction_binding)[:] = friction_tensor - wp.to_torch(restitution_binding)[:] = restitution_tensor - SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) - - -@pytest.mark.isaacsim_ci -@pytest.mark.skip(reason="MuJoCo contact at height=0 does not settle the same as PhysX — cube falls on z-axis") -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_no_friction(num_cubes, device): - """Test that a rigid object with no friction will maintain it's velocity when sliding across a plane.""" - 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, height=0.0, device=device) - - # Create ground plane with near-zero friction - # Note: MuJoCo requires friction >= MJ_MINMU (1e-5), so we use 1e-4 - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - static_friction=1e-4, - dynamic_friction=1e-4, - restitution=0.0, - ) + assert rigid_object.is_initialized + assert rigid_object.num_instances == 2 + assert rigid_object.body_names == ["Object"] + assert rigid_object.data.body_mass.shape == (2, 1) + assert rigid_object.data.body_com_pos_b.shape == (2, 1) + assert rigid_object.data.body_inertia.shape == (2, 1, 9) + torch.testing.assert_close( + rigid_object.data.root_link_pose_w.torch, + rigid_object.data.body_link_pose_w.torch.squeeze(1), ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set material friction to near-zero via view-level API - _set_newton_material_properties(cube_object, friction_val=1e-4, restitution_val=0.0, device=device) - - # Set initial velocity - # Initial velocity in X to get the block moving - initial_velocity = torch.zeros((num_cubes, 6), device=sim.cfg.device) - initial_velocity[:, 0] = 0.1 - - cube_object.write_root_velocity_to_sim_index(root_velocity=initial_velocity) - - # Simulate physics - for _ in range(5): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Non-deterministic when on GPU, so we use different tolerances - if device.startswith("cuda"): - tolerance = 1e-2 - else: - tolerance = 1e-5 - - torch.testing.assert_close( - cube_object.data.root_lin_vel_w.torch, initial_velocity[:, :3], rtol=1e-5, atol=tolerance - ) - - -@pytest.mark.isaacsim_ci -@pytest.mark.skip(reason="MuJoCo uses Coulomb friction (single mu), no static/dynamic distinction") -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_with_static_friction(num_cubes, device): - """Test that static friction applied to rigid object works as expected. - - This test works by applying a force to the object and checking if the object moves or not based on the - mu (coefficient of static friction) value set for the object. We set the static friction to be non-zero and - apply a force to the object. When the force applied is below mu, the object should not move. When the force - applied is above mu, the object should move. - """ - with _newton_sim_context(device, dt=0.01, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.03125, device=device) - - # Create ground plane - static_friction_coefficient = 0.5 - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - static_friction=static_friction_coefficient, - dynamic_friction=static_friction_coefficient, - ) + torch.testing.assert_close( + rigid_object.data.root_com_pose_w.torch, + rigid_object.data.body_com_pose_w.torch.squeeze(1), ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set friction via view-level API - _set_newton_material_properties( - cube_object, - friction_val=static_friction_coefficient, - restitution_val=0.0, - device=device, + torch.testing.assert_close( + rigid_object.data.body_link_pose_w.torch, + rigid_object.data.body_com_pose_w.torch, ) - - # let everything settle - for _ in range(100): - sim.step() - cube_object.update(sim.cfg.dt) - cube_object.write_root_velocity_to_sim_index(root_velocity=torch.zeros((num_cubes, 6), device=sim.device)) - cube_mass = cube_object.data.body_mass.torch - gravity_magnitude = abs(sim.cfg.gravity[2]) - # 2 cases: force applied is below and above mu - # below mu: block should not move as the force applied is <= mu - # above mu: block should move as the force applied is > mu - for force in "below_mu", "above_mu": - # set initial velocity to zero - cube_object.write_root_velocity_to_sim_index(root_velocity=torch.zeros((num_cubes, 6), device=sim.device)) - - external_wrench_b = torch.zeros((num_cubes, 1, 6), device=sim.device) - if force == "below_mu": - external_wrench_b[..., 0] = static_friction_coefficient * cube_mass * gravity_magnitude * 0.99 - else: - external_wrench_b[..., 0] = static_friction_coefficient * cube_mass * gravity_magnitude * 1.01 - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - ) - - # Get root state - initial_root_pos = cube_object.data.root_pos_w.torch.clone() - # Simulate physics - for _ in range(200): - # apply the wrench - cube_object.write_data_to_sim() - sim.step() - # update object - cube_object.update(sim.cfg.dt) - if force == "below_mu": - # Assert that the block has not moved - torch.testing.assert_close( - cube_object.data.root_pos_w.torch, initial_root_pos, rtol=2e-3, atol=2e-3 - ) - if force == "above_mu": - assert (cube_object.data.root_pos_w.torch[..., 0] - initial_root_pos[..., 0] > 0.02).all() - - -@pytest.mark.isaacsim_ci -@pytest.mark.skip(reason="MuJoCo restitution model differs from PhysX — inelastic collisions still bounce") -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_with_restitution(num_cubes, device): - """Test that restitution when applied to rigid object works as expected. - - This test works by dropping a block from a height and checking if the block bounces or not based on the - restitution value set for the object. We set the restitution to be non-zero and drop the block from a height. - When the restitution is 0, the block should not bounce. When the restitution is between 0 and 1, the block - should bounce with less energy. - """ - for expected_collision_type in "partially_elastic", "inelastic": - with _newton_sim_context(device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - # Set static friction to be non-zero - if expected_collision_type == "inelastic": - restitution_coefficient = 0.0 - elif expected_collision_type == "partially_elastic": - restitution_coefficient = 0.5 - - # Create ground plane - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - restitution=restitution_coefficient, - ) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - root_pose = torch.zeros(num_cubes, 7, device=sim.device) - root_pose[:, 3] = 1.0 # To make orientation a quaternion - for i in range(num_cubes): - root_pose[i, 1] = 1.0 * i - root_pose[:, 2] = 1.0 # Set an initial drop height - root_vel = torch.zeros(num_cubes, 6, device=sim.device) - root_vel[:, 2] = -1.0 # Set an initial downward velocity - - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # Set restitution via view-level API - _set_newton_material_properties( - cube_object, - friction_val=0.0, - restitution_val=restitution_coefficient, - device=device, - ) - - curr_z_velocity = cube_object.data.root_lin_vel_w.torch[:, 2].clone() - - for _ in range(100): - sim.step() - - # update object - cube_object.update(sim.cfg.dt) - curr_z_velocity = cube_object.data.root_lin_vel_w.torch[:, 2].clone() - - if expected_collision_type == "inelastic": - # assert that the block has not bounced by checking that the z velocity is less than or equal to 0 - assert (curr_z_velocity <= 0.0).all() - - if torch.all(curr_z_velocity <= 0.0): - # Still in the air - prev_z_velocity = curr_z_velocity - else: - # collision has happened, exit the for loop - break - - if expected_collision_type == "partially_elastic": - # Assert that the block has lost some energy by checking that the z velocity is less - assert torch.all(torch.le(abs(curr_z_velocity), abs(prev_z_velocity))) - assert (curr_z_velocity > 0.0).all() - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_set_mass(num_cubes, device): - """Test that selected mass writes update inverse mass and inertia across static transitions.""" - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - for index in range(num_cubes): - sim_utils.create_prim(f"/World/Env_{index}", "Xform", translation=(float(index), 0.0, 1.0)) - cube_object = RigidObject( - RigidObjectCfg( - prim_path="/World/Env_[^/]*/Object", - spawn=sim_utils.CuboidCfg( - size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), - ), - ) + torch.testing.assert_close( + rigid_object.data.root_com_vel_w.torch[..., 3:], + rigid_object.data.body_com_vel_w.torch.squeeze(1)[..., 3:], ) - # Play sim - sim.reset() + model_changes = [] + add_model_change = SimulationManager.add_model_change - # Get masses before updating one environment. - original_masses = cube_object.data.body_mass.torch.clone() - raw_model_inv_mass = cube_object.root_view.get_attribute("body_inv_mass", SimulationManager.get_model())[:, 0] - raw_model_inv_inertia = cube_object.root_view.get_attribute("body_inv_inertia", SimulationManager.get_model())[ - :, 0 - ] - assert cube_object.data._sim_bind_body_inv_mass.ptr == raw_model_inv_mass.ptr - assert cube_object.data._sim_bind_body_inv_inertia.ptr == raw_model_inv_inertia.ptr - model_inv_mass = cube_object.data._sim_bind_body_inv_mass - model_inv_inertia = cube_object.data._sim_bind_body_inv_inertia - original_inv_mass = wp.to_torch(model_inv_mass).clone() - original_inv_inertia = wp.to_torch(model_inv_inertia).clone() + def record_model_change(change: ModelFlags) -> None: + model_changes.append(change) + add_model_change(change) - assert original_masses.shape == (num_cubes, 1) + monkeypatch.setattr(SimulationManager, "add_model_change", staticmethod(record_model_change)) + initial_pose = rigid_object.data.root_link_pose_w.torch.clone() env_ids = torch.tensor([1], dtype=torch.int32, device=device) body_ids = torch.tensor([0], dtype=torch.int32, device=device) - - # A positive-to-zero transition makes the selected body static. - zero_mass = torch.zeros(1, 1, device=device) - cube_object.set_masses_index(masses=zero_mass, env_ids=env_ids, body_ids=body_ids) - torch.testing.assert_close(wp.to_torch(model_inv_mass)[1], torch.zeros_like(original_inv_mass[1])) - torch.testing.assert_close(wp.to_torch(model_inv_inertia)[1], torch.zeros_like(original_inv_inertia[1])) - torch.testing.assert_close(wp.to_torch(model_inv_mass)[0], original_inv_mass[0]) - torch.testing.assert_close(wp.to_torch(model_inv_inertia)[0], original_inv_inertia[0]) - - # Inertia writes keep inverse mass unchanged and respect the body's current static state. - wp.to_torch(model_inv_inertia)[1].copy_(torch.eye(3, device=device).reshape(1, 3, 3)) - inertia_matrix = torch.diag(torch.tensor([2.0, 3.0, 4.0], device=device)) - inertias = inertia_matrix.reshape(1, 1, 9) - cube_object.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) - torch.testing.assert_close(wp.to_torch(model_inv_mass)[1], torch.zeros_like(original_inv_mass[1])) - torch.testing.assert_close(wp.to_torch(model_inv_inertia)[1], torch.zeros_like(original_inv_inertia[1])) - - # A zero-to-positive transition restores both inverse arrays from current primary data. - masses = original_masses[env_ids][:, body_ids] + 4.0 - cube_object.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) - torch.testing.assert_close(cube_object.data.body_mass.torch[env_ids][:, body_ids], masses) - torch.testing.assert_close(wp.to_torch(model_inv_mass)[env_ids][:, body_ids], masses.reciprocal()) + target_pose = initial_pose[env_ids].clone() + target_pose[..., :3] += torch.tensor([0.5, -0.25, 0.75], device=device) + rigid_object.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + + torch.testing.assert_close(rigid_object.data.root_link_pose_w.torch[env_ids], target_pose) + torch.testing.assert_close(rigid_object.data.root_link_pose_w.torch[:1], initial_pose[:1]) + + initial_mass = rigid_object.data.body_mass.torch.clone() + initial_inv_mass = wp.to_torch(rigid_object.data._sim_bind_body_inv_mass).clone() + masses = torch.tensor([[4.0]], device=device) + rigid_object.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(rigid_object.data.body_mass.torch[env_ids][:, body_ids], masses) torch.testing.assert_close( - wp.to_torch(model_inv_inertia)[env_ids][:, body_ids], - torch.linalg.inv(inertia_matrix).reshape(1, 1, 3, 3), + wp.to_torch(rigid_object.data._sim_bind_body_inv_mass)[env_ids][:, body_ids], masses.reciprocal() ) + torch.testing.assert_close(rigid_object.data.body_mass.torch[:1], initial_mass[:1]) + torch.testing.assert_close(wp.to_torch(rigid_object.data._sim_bind_body_inv_mass)[:1], initial_inv_mass[:1]) + assert model_changes == [ModelFlags.BODY_INERTIAL_PROPERTIES] - # Simulate physics - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - masses_to_check = cube_object.data.body_mass.torch[env_ids][:, body_ids] - - # Check if mass is set correctly - 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 - 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"]) -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``. - - 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`). - """ - with _newton_sim_context(device, gravity_enabled=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - sim.reset() - - # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. + model_changes.clear() model = SimulationManager.get_model() - model_gravity_arr = model.gravity[: model.world_count] - global_gravity = wp.to_torch(model.gravity)[-1].clone() - assert cube_object.data.GRAVITY_VEC_W.warp.ptr == model_gravity_arr.ptr - assert cube_object.data.GRAVITY_VEC_W.shape == (num_cubes,) - - # Mutate model.gravity per-env in place, as randomize_physics_scene_gravity does. - new_gravity = torch.tensor( - [[0.1 * (i + 1), 0.2 * (i + 1), -3.0 - float(i)] for i in range(num_cubes)], - device=device, - dtype=torch.float32, - ) - wp.to_torch(model_gravity_arr).copy_(new_gravity) + material_mu = rigid_object.root_view.get_attribute("shape_material_mu", model) + initial_material_mu = wp.to_torch(material_mu).clone() + material_values = wp.full(material_mu.shape, value=0.75, dtype=wp.float32, device=device) + material_mask = wp.array([[False], [True]], dtype=wp.bool, device=device) + rigid_object.root_view.set_attribute("shape_material_mu", model, material_values, material_mask) + SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) + expected_material_mu = initial_material_mu.clone() + expected_material_mu[1, 0] = 0.75 + torch.testing.assert_close(wp.to_torch(material_mu), expected_material_mu) + assert model_changes == [ModelFlags.SHAPE_PROPERTIES] + + model_changes.clear() + model_gravity = model.gravity[: model.world_count] + new_gravity = torch.tensor([[0.0, 0.0, -2.0], [0.0, -3.0, -4.0]], device=device) + wp.to_torch(model_gravity).copy_(new_gravity) SimulationManager.add_model_change(ModelFlags.MODEL_PROPERTIES) + rigid_object.update(0.0) + torch.testing.assert_close(rigid_object.data.GRAVITY_VEC_W.torch, new_gravity) + torch.testing.assert_close( + rigid_object.data.projected_gravity_b.torch, + torch.nn.functional.normalize(new_gravity, dim=-1), + atol=1e-6, + rtol=1e-6, + ) + assert model_changes == [ModelFlags.MODEL_PROPERTIES] - # Live view: new per-env values are visible immediately, no invalidation step. - torch.testing.assert_close(cube_object.data.GRAVITY_VEC_W.torch, new_gravity) - torch.testing.assert_close(wp.to_torch(model.gravity)[-1], global_gravity) - - # Recompute the lazily-cached projected_gravity_b without sim.step, so cube - # orientation stays at identity and the projection equals unit-direction gravity. - cube_object.update(sim.cfg.dt) - expected = torch.nn.functional.normalize(new_gravity, dim=-1) - torch.testing.assert_close(cube_object.data.projected_gravity_b.torch, expected, atol=1e-5, rtol=1e-5) - - -@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): - """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 - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - - # Play sim - 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, no quaternion) - 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) - - # random z spin velocity (bounded to keep numerical drift within the position tolerance below) - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = 0.5 * torch.randn(1, device=device).clamp(-1.0, 1.0) - - # Simulate physics - for _ in range(100): - # spin the object around Z axis (com) - cube_object.write_root_velocity_to_sim_index(root_velocity=spin_twist.repeat(num_cubes, 1)) - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # get state properties - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - 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_link_pose_w = cube_object.data.body_link_pose_w.torch - body_link_vel_w = cube_object.data.body_link_vel_w.torch - 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:]) - - -@pytest.mark.isaacsim_ci -@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_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.""" - 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) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32, device=device) - - # Play sim - 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.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): - # perform step - sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - 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) - 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 - ) - - # 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. - # The body-frame caches must already reflect the write on their own (regression: - # 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() - - 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) - - # 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 - 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 - 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:]) - - -@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): - """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`` - (Newton ``body_q``) is stale until forward kinematics is re-evaluated. The getter must call - :meth:`SimulationManager.forward` so the returned tensor matches the written pose. Without the fix, - the getter returns the pre-write value. The write must also dirty the simulator-side - ``_fk_reset_mask`` so collision queries (which read ``body_q`` directly, not via the property) - re-run FK before the next step. - """ - - def _fk_reset_mask_dirty() -> bool: - assert SimulationManager._fk_reset_mask is not None - return bool(wp.to_torch(SimulationManager._fk_reset_mask).any().item()) - - num_cubes = 2 - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.5, device=device) - - sim.reset() - assert cube_object.is_initialized - - # Step once so that _sim_timestamp > 0 and caches are primed. + wp.to_torch(model_gravity).zero_() + SimulationManager.add_model_change(ModelFlags.MODEL_PROPERTIES) + rigid_object.update(0.0) + initial_velocity = rigid_object.data.root_com_lin_vel_w.torch.clone() + forces = torch.tensor([[[6.0, 0.0, 0.0]]], device=device) + torques = torch.zeros_like(forces) + rigid_object.permanent_wrench_composer.set_forces_and_torques_index( + forces=forces, + torques=torques, + env_ids=torch.tensor([0], dtype=torch.int32, device=device), + body_ids=body_ids, + ) + rigid_object.write_data_to_sim() 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() + rigid_object.update(sim.cfg.dt) - # 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) + assert rigid_object.data.root_com_lin_vel_w.torch[0, 0] > initial_velocity[0, 0] + torch.testing.assert_close(rigid_object.data.root_com_lin_vel_w.torch[1], initial_velocity[1]) 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 b833777766de..4822af620edd 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -3,21 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Launch Isaac Sim Simulator first.""" - -from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices - -# launch omniverse app -simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app - -"""Rest everything follows.""" - -import sys +"""Kitless real-solver integration tests for Newton rigid-object collections.""" import pytest import torch @@ -30,1061 +16,136 @@ import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg, RigidObjectCollectionCfg from isaaclab.sim import SimulationCfg, build_simulation_context -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, - quat_rotate, - random_orientation, - subtract_frame_transforms, -) - -NEWTON_SIM_CFG = SimulationCfg( - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg(), - ), -) - - -def _newton_sim_context(device, gravity_enabled=True, dt=None, **kwargs): - """Helper to create a Newton simulation context with the correct device. - - When sim_cfg is provided to build_simulation_context, the device, gravity_enabled, and dt - kwargs are ignored. This helper applies them to the shared NEWTON_SIM_CFG before calling. - """ - NEWTON_SIM_CFG.device = device - NEWTON_SIM_CFG.gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - if dt is not None: - NEWTON_SIM_CFG.dt = dt - return build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, **kwargs) - - -def generate_cubes_scene( - num_envs: int = 1, - num_cubes: int = 1, - height=1.0, - has_api: bool = True, - kinematic_enabled: bool = False, - device: str = "cuda:0", - spawn_unrelated_sibling: bool = False, -) -> tuple[RigidObjectCollection, torch.Tensor]: - """Generate a scene with the provided number of cubes. - Args: - num_envs: Number of envs to generate. - num_cubes: Number of cubes to generate. - height: Height of the cubes. - has_api: Whether the cubes have a rigid body API on them. - kinematic_enabled: Whether the cubes are kinematic. - device: Device to use for the simulation. - spawn_unrelated_sibling: Whether to spawn a rigid body outside the collection in each environment. +pytestmark = pytest.mark.integration - Returns: - A tuple containing the rigid object collection representing the cubes and the origins of the cubes. - """ - origins = torch.tensor([(i * 3.0, 0, height) for i in range(num_envs)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=origin) - - # Resolve spawn configuration - if has_api: - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - else: - # since no rigid body properties defined, this is just a static collider - spawn_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - - # create the rigid object configs - cube_config_dict = {} - for i in range(num_cubes): - cube_object_cfg = RigidObjectCfg( - prim_path=f"/World/Env_[^/]*/Object_{i}", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 3 * i, height)), - ) - cube_config_dict[f"cube_{i}"] = cube_object_cfg - if spawn_unrelated_sibling: - spawn_cfg.func( - "/World/Env_[^/]*/UnrelatedObject", - spawn_cfg, - translation=(0.0, -3.0, height), - ) - # create the rigid object collection - cube_object_collection_cfg = RigidObjectCollectionCfg(rigid_objects=cube_config_dict) - cube_object_collection = RigidObjectCollection(cfg=cube_object_collection_cfg) - - return cube_object_collection, origins - - -@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 - 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, +def _newton_sim_context(device: str): + """Create a fresh kitless Newton simulation context.""" + return build_simulation_context( + sim_cfg=SimulationCfg( device=device, - spawn_unrelated_sibling=True, + dt=1.0 / 60.0, + gravity=(0.0, 0.0, 0.0), + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg()), ) - - 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 - - # Play sim - sim.reset() - - # Check if object is initialized - assert object_collection.is_initialized - assert len(object_collection.body_names) == num_cubes - - # Check buffers that exist and have correct shapes - 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): - """Masked inertial-property writes update only selected Newton inverse entries.""" - num_envs = 2 - num_cubes = 3 - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - for env_index in range(num_envs): - sim_utils.create_prim(f"/World/Env_{env_index}", "Xform", translation=(float(env_index), 0.0, 1.0)) - spawn_cfg = sim_utils.CuboidCfg( +def _local_cube_cfg(prim_path: str, y: float) -> RigidObjectCfg: + """Create one local collection-body configuration.""" + return RigidObjectCfg( + prim_path=prim_path, + spawn=sim_utils.CuboidCfg( size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + rigid_props=sim_utils.RigidBodyBaseCfg(disable_gravity=True), mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - object_collection = RigidObjectCollection( - RigidObjectCollectionCfg( - rigid_objects={ - f"cube_{body_index}": RigidObjectCfg( - prim_path=f"/World/Env_[^/]*/Object_{body_index}", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, float(body_index), 0.0)), - ) - for body_index in range(num_cubes) - } - ) - ) - sim.reset() - - env_mask = wp.array([True, False], dtype=wp.bool, device=device) - body_mask = wp.array([False, True, False], dtype=wp.bool, device=device) - selected = (0, 1) - - raw_model_inv_mass = object_collection.root_view.get_attribute("body_inv_mass", SimulationManager.get_model())[ - :, :, 0 - ] - assert object_collection.data._sim_bind_body_inv_mass.ptr == raw_model_inv_mass.ptr - model_inv_mass = object_collection.data._sim_bind_body_inv_mass - original_inv_mass = wp.to_torch(model_inv_mass).clone() - masses = object_collection.data.body_mass.torch.clone() - masses[selected] = 4.0 - object_collection.set_masses_mask(masses=masses, env_mask=env_mask, body_mask=body_mask) - - updated_inv_mass = wp.to_torch(model_inv_mass).clone() - torch.testing.assert_close(updated_inv_mass[selected], masses[selected].reciprocal()) - unselected = torch.ones_like(updated_inv_mass, dtype=torch.bool) - unselected[selected] = False - torch.testing.assert_close(updated_inv_mass[unselected], original_inv_mass[unselected]) - - raw_model_inv_inertia = object_collection.root_view.get_attribute( - "body_inv_inertia", SimulationManager.get_model() - )[:, :, 0] - assert object_collection.data._sim_bind_body_inv_inertia.ptr == raw_model_inv_inertia.ptr - model_inv_inertia = object_collection.data._sim_bind_body_inv_inertia - original_inv_inertia = wp.to_torch(model_inv_inertia).clone() - inertias = object_collection.data.body_inertia.torch.clone() - inertia_matrix = torch.diag(torch.tensor([2.0, 3.0, 5.0], device=device)) - inertias[selected] = inertia_matrix.reshape(9) - object_collection.set_inertias_mask(inertias=inertias, env_mask=env_mask, body_mask=body_mask) - - updated_inv_inertia = wp.to_torch(model_inv_inertia) - torch.testing.assert_close(updated_inv_inertia[selected], torch.linalg.inv(inertia_matrix)) - torch.testing.assert_close(updated_inv_inertia[unselected], original_inv_inertia[unselected]) - torch.testing.assert_close(wp.to_torch(model_inv_mass), updated_inv_mass) - - -@pytest.mark.skip(reason="Newton doesn't support kinematic rigid bodies yet") -@pytest.mark.parametrize("num_envs", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 3]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_kinematic_enabled(num_envs, num_cubes, device): - """Test that initialization for prim with kinematic flag enabled.""" - 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, kinematic_enabled=True, device=device + collision_props=sim_utils.CollisionBaseCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, y, 1.0)), + ) + + +def _spawn_collection() -> RigidObjectCollection: + """Author a two-by-two local collection plus an unrelated sibling body.""" + for env_index in range(2): + sim_utils.create_prim(f"/World/Env_{env_index}", "Xform", translation=(3.0 * env_index, 0.0, 0.0)) + sibling_cfg = sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyBaseCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionBaseCfg(), + ) + sibling_cfg.func("/World/Env_[^/]*/UnrelatedObject", sibling_cfg, translation=(0.0, -2.0, 1.0)) + return RigidObjectCollection( + RigidObjectCollectionCfg( + rigid_objects={ + "cube_0": _local_cube_cfg("/World/Env_[^/]*/Object_0", 0.0), + "cube_1": _local_cube_cfg("/World/Env_[^/]*/Object_1", 1.0), + } ) + ) - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(object_collection) < 10 - # Play sim +def test_rigid_object_collection_real_newton_seams(monkeypatch) -> None: + """Exercise exact model selection, partial state/property writes, and live gravity.""" + device = "cpu" + with _newton_sim_context(device) as sim: + collection = _spawn_collection() sim.reset() - # Check if object is initialized - assert object_collection.is_initialized - assert len(object_collection.body_names) == num_cubes - - # Check buffers that exist and have correct shapes - 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) - - # Simulate physics - for _ in range(2): - sim.step() - object_collection.update(sim.cfg.dt) - # check that the object is kinematic - default_body_pose = object_collection.data.default_body_pose.torch.clone() - default_body_vel = object_collection.data.default_body_vel.torch.clone() - default_body_pose[..., :3] += origins.unsqueeze(1) - torch.testing.assert_close(object_collection.data.body_link_pose_w.torch, default_body_pose) - torch.testing.assert_close(object_collection.data.body_link_vel_w.torch, default_body_vel) - - -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_no_rigid_body(num_cubes, device): - """Test that initialization fails when no rigid body is found 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_cubes=num_cubes, has_api=False, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(object_collection) < 10 - - # Play sim - with pytest.raises(RuntimeError): - 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) - 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.""" - 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) - 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 - 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 - 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() - - is_global = False - if i % 2 == 0: - positions = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] - is_global = True - else: - positions = None - - # apply force - object_collection.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=positions, - body_ids=object_ids, - 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) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - object_collection.data.body_link_pos_w.torch[:, 0::2, 2], - torch.ones_like(object_collection.data.body_link_pos_w.torch[:, 0::2, 2]), - ) - # 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) - 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 - 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 + assert collection.is_initialized + assert collection.num_instances == 2 + assert collection.body_names == ["cube_0", "cube_1"] + assert collection.root_view.count == 4 + assert collection.data.body_mass.shape == (2, 2) + assert collection.data.body_com_pos_b.shape == (2, 2) + assert collection.data.body_inertia.shape == (2, 2, 9) + torch.testing.assert_close(collection.data.body_link_pose_w.torch, collection.data.body_com_pose_w.torch) + torch.testing.assert_close( + collection.data.body_link_vel_w.torch[..., 3:], + collection.data.body_com_vel_w.torch[..., 3:], ) - # 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() - - is_global = False - if i % 2 == 0: - body_com_pos_w = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] - 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 - is_global = True - else: - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - - # apply force - object_collection.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=object_ids, - env_ids=None, - is_global=is_global, - ) - object_collection.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=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) - - # 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) - # 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", [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. - - .. note:: - 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: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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) + model_changes = [] + add_model_change = SimulationManager.add_model_change -@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) - 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()) -def test_set_material_properties(num_envs, num_cubes, device): - """Test getting and setting material properties of rigid object collection via view-level APIs.""" - 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) - sim.reset() - - # Get friction/restitution bindings via view-level API - # The collection's _root_view stores data in flat view order: (num_envs * num_cubes, ...) - model = SimulationManager.get_model() - friction_raw = object_collection._root_view.get_attribute("shape_material_mu", model) - restitution_raw = object_collection._root_view.get_attribute("shape_material_restitution", model) - - # Shape is (num_envs * num_cubes, num_shapes_per_body, 1) — slice off trailing dim - friction_binding = friction_raw[:, :, 0] - restitution_binding = restitution_raw[:, :, 0] - - # Generate random values matching the flat view shape - friction = torch.empty_like(wp.to_torch(friction_binding)).uniform_(0.4, 0.8) - restitution = torch.empty_like(wp.to_torch(restitution_binding)).uniform_(0.0, 0.2) + def record_model_change(change: ModelFlags) -> None: + model_changes.append(change) + add_model_change(change) - wp.to_torch(friction_binding)[:] = friction - wp.to_torch(restitution_binding)[:] = restitution - SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) - - # Perform simulation - sim.step() - object_collection.update(sim.cfg.dt) - - # Verify by reading back from the binding - mu = wp.to_torch(friction_binding) - restitution_check = wp.to_torch(restitution_binding) - torch.testing.assert_close(mu, friction) - torch.testing.assert_close(restitution_check, restitution) + monkeypatch.setattr(SimulationManager, "add_model_change", staticmethod(record_model_change)) + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + body_ids = torch.tensor([1], dtype=torch.int32, device=device) + initial_pose = collection.data.body_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids][:, body_ids].clone() + target_pose[..., :3] += torch.tensor([0.25, 0.5, 0.75], device=device) + collection.write_body_link_pose_to_sim_index( + body_poses=target_pose, + env_ids=env_ids, + body_ids=body_ids, + ) -@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) - - sim.reset() + torch.testing.assert_close(collection.data.body_link_pose_w.torch[env_ids][:, body_ids], target_pose) + torch.testing.assert_close(collection.data.body_link_pose_w.torch[0], initial_pose[0]) + torch.testing.assert_close(collection.data.body_link_pose_w.torch[1, :1], initial_pose[1, :1]) - # Check if gravity vector is set correctly + initial_mass = collection.data.body_mass.torch.clone() + initial_inv_mass = wp.to_torch(collection.data._sim_bind_body_inv_mass).clone() + masses = torch.tensor([[5.0]], device=device) + collection.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(collection.data.body_mass.torch[env_ids][:, body_ids], masses) torch.testing.assert_close( - object_collection.data.GRAVITY_VEC_W.torch[0], torch.tensor(expected_g, device=device) + wp.to_torch(collection.data._sim_bind_body_inv_mass)[env_ids][:, body_ids], masses.reciprocal() ) + torch.testing.assert_close(collection.data.body_mass.torch[0], initial_mass[0]) + torch.testing.assert_close(collection.data.body_mass.torch[1, :1], initial_mass[1, :1]) + torch.testing.assert_close(wp.to_torch(collection.data._sim_bind_body_inv_mass)[0], initial_inv_mass[0]) + torch.testing.assert_close(wp.to_torch(collection.data._sim_bind_body_inv_mass)[1, :1], initial_inv_mass[1, :1]) + assert model_changes == [ModelFlags.BODY_INERTIAL_PROPERTIES] - # Perform simulation - 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) - sim.reset() - - # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. + model_changes.clear() model = SimulationManager.get_model() - model_gravity_arr = model.gravity[: model.world_count] - global_gravity = wp.to_torch(model.gravity)[-1].clone() - assert object_collection.data.GRAVITY_VEC_W.warp.ptr == model_gravity_arr.ptr - assert object_collection.data.GRAVITY_VEC_W.shape == (num_envs,) - - # Mutate model.gravity per-env in place, as randomize_physics_scene_gravity does. - new_gravity = torch.tensor( - [[0.1 * (i + 1), 0.2 * (i + 1), -3.0 - float(i)] for i in range(num_envs)], - device=device, - dtype=torch.float32, - ) - wp.to_torch(model_gravity_arr).copy_(new_gravity) + material_mu = collection.root_view.get_attribute("shape_material_mu", model) + initial_material_mu = wp.to_torch(material_mu).clone() + material_values = wp.full(material_mu.shape, value=0.65, dtype=wp.float32, device=device) + material_mask = wp.array([[False, False], [False, True]], dtype=wp.bool, device=device) + collection.root_view.set_attribute("shape_material_mu", model, material_values, material_mask) + SimulationManager.add_model_change(ModelFlags.SHAPE_PROPERTIES) + expected_material_mu = initial_material_mu.clone() + expected_material_mu[1, 1] = 0.65 + torch.testing.assert_close(wp.to_torch(material_mu), expected_material_mu) + assert model_changes == [ModelFlags.SHAPE_PROPERTIES] + + model_changes.clear() + model_gravity = model.gravity[: model.world_count] + new_gravity = torch.tensor([[0.0, 0.0, -2.0], [0.0, -3.0, -4.0]], device=device) + wp.to_torch(model_gravity).copy_(new_gravity) SimulationManager.add_model_change(ModelFlags.MODEL_PROPERTIES) - torch.testing.assert_close(wp.to_torch(model.gravity)[-1], global_gravity) - - # Recompute the lazily-cached projected_gravity_b without sim.step: bodies stay - # at identity orientation, so each env's unit gravity broadcasts across its bodies. - object_collection.update(sim.cfg.dt) - expected_per_env = torch.nn.functional.normalize(new_gravity, dim=-1) - expected = expected_per_env.unsqueeze(1).expand(-1, num_cubes, -1).contiguous() - torch.testing.assert_close(object_collection.data.projected_gravity_b.torch, expected, atol=1e-5, rtol=1e-5) - - -@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): - """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 - cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) - - 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) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # initial spawn point - init_com = cube_object.data.body_com_pose_w.torch[..., :3] - - for i in range(10): - # spin the object around Z axis (com) - cube_object.write_body_com_velocity_to_sim_index(body_velocities=spin_twist.repeat(num_envs, num_cubes, 1)) - sim.step() - cube_object.update(sim.cfg.dt) - - # get state properties - object_link_pose_w = cube_object.data.body_link_pose_w.torch - object_link_vel_w = cube_object.data.body_link_vel_w.torch - 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], - ) - - torch.testing.assert_close(-offset, object_link_state_pos_rel_com, **_tol) + collection.update(0.0) - # 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) - - # 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, - ) - - # 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.""" - 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) - - 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.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): - 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 - ) - - 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) - 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) - - -@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) - - 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:]) - - -@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): - """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 - buffer, so the property read is not what becomes stale — the simulator's internal ``body_q`` used by - collision detection is. The write methods must therefore call :meth:`SimulationManager.invalidate_fk` - so downstream consumers re-run forward kinematics before the next step. Without the fix, - ``_fk_reset_mask`` remains unset after an explicit pose write. The buffer-aliasing invariant is - also pinned: a refactor that decouples ``_sim_bind_body_link_pose_w`` from the write target would - silently make the property stale, so we check the post-write pose matches the written value. - """ - - def _fk_reset_mask_dirty() -> bool: - assert SimulationManager._fk_reset_mask is not None - return bool(wp.to_torch(SimulationManager._fk_reset_mask).any().item()) - - num_envs = 2 - num_cubes = 2 - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.5, device=device) - - sim.reset() - assert cube_object.is_initialized - - 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) + torch.testing.assert_close(collection.data.GRAVITY_VEC_W.torch, new_gravity) + expected_projected = torch.nn.functional.normalize(new_gravity, dim=-1).unsqueeze(1).expand(-1, 2, -1) + torch.testing.assert_close(collection.data.projected_gravity_b.torch, expected_projected, atol=1e-6, rtol=1e-6) + assert model_changes == [ModelFlags.MODEL_PROPERTIES] diff --git a/source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py b/source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py new file mode 100644 index 000000000000..e2257ddf6e76 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py @@ -0,0 +1,72 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Focused FK invalidation tests for Newton articulation data.""" + +from types import SimpleNamespace + +import pytest +import warp as wp +from isaaclab_newton.assets.articulation.articulation_data import ArticulationData +from isaaclab_newton.physics import NewtonManager as SimulationManager + +pytestmark = pytest.mark.unit + + +def test_stale_articulation_fk_forwards_and_republishes_ordered_state_once(monkeypatch) -> None: + """A stale articulation FK stamp must forward and refresh public-order shadows exactly once.""" + calls = [] + data = object.__new__(ArticulationData) + data._sim_timestamp = 2.0 + data._fk_timestamp = -1.0 + data._refresh_user_order_body_state = lambda: calls.append("refresh") + monkeypatch.setattr(SimulationManager, "forward", classmethod(lambda cls: calls.append("forward"))) + + data._ensure_fk_fresh() + data._ensure_fk_fresh() + + assert calls == ["forward", "refresh"] + assert data._fk_timestamp == 2.0 + + +def test_joint_pose_reset_invalidates_only_selected_articulation_instances(monkeypatch) -> None: + """Joint writes must invalidate Newton FK for the selected instances and owning view IDs.""" + invalidations = [] + data = object.__new__(ArticulationData) + data._fk_timestamp = 4.0 + data._root_view = SimpleNamespace(articulation_ids=wp.array([3, 8], dtype=wp.int32, device="cpu")) + for name in ( + "_root_com_pose_w", + "_body_com_pose_w", + "_root_link_vel_w", + "_body_link_vel_w", + "_projected_gravity_b", + "_heading_w", + "_root_link_lin_vel_b", + "_root_link_ang_vel_b", + "_root_com_lin_vel_b", + "_root_com_ang_vel_b", + "_root_state_w", + "_root_link_state_w", + "_root_com_state_w", + "_body_state_w", + "_body_link_state_w", + "_body_com_state_w", + ): + setattr(data, name, None) + monkeypatch.setattr( + SimulationManager, + "invalidate_fk", + classmethod(lambda cls, **kwargs: invalidations.append(kwargs)), + ) + env_ids = wp.array([1], dtype=wp.int32, device="cpu") + + data._reset_pose(env_ids=env_ids) + + assert data._fk_timestamp == -1.0 + assert len(invalidations) == 1 + assert invalidations[0]["env_ids"] is env_ids + assert invalidations[0]["env_mask"] is None + assert invalidations[0]["articulation_ids"] is data._root_view.articulation_ids diff --git a/source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py b/source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py new file mode 100644 index 000000000000..aaa529025f0d --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py @@ -0,0 +1,116 @@ +# 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 + +"""Focused joint-property staging tests for Newton articulations.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import torch +import warp as wp +from isaaclab_newton.assets import Articulation +from isaaclab_newton.assets.articulation.articulation_data import ArticulationData +from isaaclab_newton.physics import NewtonManager as SimulationManager +from newton import ModelBuilder, ModelFlags + +from isaaclab.utils.warp.proxy_array import ProxyArray + +pytestmark = pytest.mark.unit + + +def test_partial_joint_property_stages_user_and_backend_order_and_notifies(monkeypatch) -> None: + """A partial public-order write must scatter to Newton order and emit one exact notification.""" + articulation = object.__new__(Articulation) + articulation._device = "cpu" + articulation._check_shapes = False + articulation._ALL_INDICES = wp.array([0, 1], dtype=wp.int32, device="cpu") + articulation._ALL_JOINT_INDICES = wp.array([0, 1], dtype=wp.int32, device="cpu") + user_to_backend = wp.array([1, 0], dtype=wp.int32, device="cpu") + backend_to_user = wp.array([1, 0], dtype=wp.int32, device="cpu") + user_stiffness = wp.full((2, 2), 3.0, dtype=wp.float32, device="cpu") + backend_stiffness = wp.full((2, 2), 3.0, dtype=wp.float32, device="cpu") + articulation._data = SimpleNamespace( + has_joint_ordering=True, + joint_ordering=SimpleNamespace(user_to_backend=user_to_backend, backend_to_user=backend_to_user), + _joint_stiffness_user=user_stiffness, + _sim_bind_joint_stiffness_sim=backend_stiffness, + ) + notifications = [] + monkeypatch.setattr( + SimulationManager, + "add_model_change", + classmethod(lambda cls, flag: notifications.append(flag)), + ) + + articulation.write_joint_stiffness_to_sim_index( + stiffness=wp.array([[17.0]], dtype=wp.float32, device="cpu"), + env_ids=wp.array([1], dtype=wp.int32, device="cpu"), + joint_ids=wp.array([0], dtype=wp.int32, device="cpu"), + ) + + np.testing.assert_allclose(user_stiffness.numpy(), [[3.0, 3.0], [17.0, 3.0]]) + np.testing.assert_allclose(backend_stiffness.numpy(), [[3.0, 3.0], [3.0, 17.0]]) + assert notifications == [ModelFlags.JOINT_DOF_PROPERTIES] + + +def test_num_shapes_per_body_follows_public_body_order() -> None: + """Newton collision-shape counts must use the same public axis as body names.""" + + class _ShapeCountSurface: + backend_num_shapes_per_body = Articulation.backend_num_shapes_per_body + num_shapes_per_body = Articulation.num_shapes_per_body + + articulation = _ShapeCountSurface() + articulation._num_shapes_per_body_backend = None + articulation._root_view = SimpleNamespace(body_shapes=((), (object(), object()), (object(), object(), object()))) + articulation.body_ordering = SimpleNamespace(user_to_backend_indices=(2, 0, 1)) + + assert articulation.num_shapes_per_body == [3, 0, 2] + + +def test_viscous_writer_updates_finalized_newton_binding_and_notifies(monkeypatch) -> None: + """Passive damping must reach Newton's live field without changing actuator derivative gains.""" + 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._check_shapes = False + 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") + notifications = [] + monkeypatch.setattr( + SimulationManager, + "add_model_change", + classmethod(lambda cls, flag: notifications.append(flag)), + ) + + 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])) + assert notifications == [ModelFlags.JOINT_DOF_PROPERTIES] diff --git a/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py new file mode 100644 index 000000000000..0d3ab3eafa7b --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py @@ -0,0 +1,87 @@ +# 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 + +"""Focused public-order state publication tests for Newton articulations.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import warp as wp +from isaaclab_newton.assets import Articulation +from isaaclab_newton.assets.articulation.articulation_data import ArticulationData +from isaaclab_newton.physics import NewtonManager as SimulationManager + +from isaaclab.assets.articulation.base_articulation import BaseArticulation + +pytestmark = pytest.mark.unit + + +class _LaunchCache: + def launch(self, _name, kernel, *, dim, inputs, outputs) -> None: + wp.launch(kernel, dim=dim, inputs=inputs, outputs=outputs, device="cpu") + + +def test_post_step_publish_refreshes_joint_and_body_shadows_in_public_order() -> None: + """The Newton post-step hook must publish every Tier-1 state shadow in public order.""" + data = object.__new__(ArticulationData) + data._device = "cpu" + data._num_instances = 2 + data._num_joints = 2 + data._num_bodies = 2 + user_to_backend = wp.array([1, 0], dtype=wp.int32, device="cpu") + data.joint_ordering = SimpleNamespace(user_to_backend=user_to_backend) + data.body_ordering = SimpleNamespace(user_to_backend=user_to_backend) + data._read_launch_cache = _LaunchCache() + data._sim_bind_joint_pos = wp.array([[1.0, 2.0], [3.0, 4.0]], dtype=wp.float32, device="cpu") + data._sim_bind_joint_vel = wp.array([[5.0, 6.0], [7.0, 8.0]], dtype=wp.float32, device="cpu") + data._joint_pos_user = wp.zeros((2, 2), dtype=wp.float32, device="cpu") + data._joint_vel_user = wp.zeros((2, 2), dtype=wp.float32, device="cpu") + backend_pose = np.asarray( + [ + [[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], + [[3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], [4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], + ], + dtype=np.float32, + ) + data._sim_bind_body_link_pose_w = wp.array(backend_pose, dtype=wp.transformf, device="cpu") + data._sim_bind_body_com_vel_w = wp.array( + np.arange(24, dtype=np.float32).reshape(2, 2, 6), dtype=wp.spatial_vectorf, device="cpu" + ) + data._body_link_pose_w_user = wp.zeros((2, 2), dtype=wp.transformf, device="cpu") + data._body_com_vel_w_user = wp.zeros((2, 2), dtype=wp.spatial_vectorf, device="cpu") + + data._refresh_user_order_state() + + np.testing.assert_allclose(data._joint_pos_user.numpy(), [[2.0, 1.0], [4.0, 3.0]]) + np.testing.assert_allclose(data._joint_vel_user.numpy(), [[6.0, 5.0], [8.0, 7.0]]) + np.testing.assert_allclose(data._body_link_pose_w_user.numpy(), backend_pose[:, [1, 0]]) + np.testing.assert_allclose( + data._body_com_vel_w_user.numpy(), np.arange(24, dtype=np.float32).reshape(2, 2, 6)[:, [1, 0]] + ) + + +def test_clear_callbacks_unregisters_only_the_articulation_post_step_hook(monkeypatch) -> None: + """Clearing one articulation must remove its ordered publish hook without leaking it globally.""" + articulation = object.__new__(Articulation) + + def callback() -> None: + pass + + articulation._model_init_handle = None + articulation._physics_ready_handle = None + articulation._post_step_callback = callback + unregistered = [] + monkeypatch.setattr(BaseArticulation, "_clear_callbacks", lambda self: None) + monkeypatch.setattr( + SimulationManager, + "unregister_post_step_callback", + classmethod(lambda cls, value: unregistered.append(value)), + ) + + articulation._clear_callbacks() + + assert unregistered == [callback] + assert articulation._post_step_callback is None diff --git a/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py b/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py new file mode 100644 index 000000000000..c554412ef141 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py @@ -0,0 +1,243 @@ +# 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 + +"""Focused Newton actuator-adaptation and telemetry tests.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import warp as wp +from isaaclab_newton.assets.articulation.actuator_control import NewtonActuatorControl +from isaaclab_newton.assets.articulation.articulation import _configure_builder_joint_target_modes +from isaaclab_newton.physics import NewtonManager as SimulationManager +from newton import JointTargetMode, JointType, Model, ModelBuilder + +from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg +from isaaclab.actuators.newton.adapter import resolve_actuator_component +from isaaclab.actuators.newton.kernels import sync_torque_telemetry +from isaaclab.assets import ArticulationCfg + +pytestmark = pytest.mark.unit + + +def _target_mode_builder() -> ModelBuilder: + builder = ModelBuilder() + inertia = wp.mat33(1.0) + left_link = builder.add_link(mass=1.0, inertia=inertia, label="/World/Robot/left_link") + left_joint = builder.add_joint_revolute(-1, left_link, label="/World/Robot/left_joint") + right_link = builder.add_link(mass=1.0, inertia=inertia, label="/World/Robot/right_link") + right_joint = builder.add_joint_revolute(left_link, right_link, label="/World/Robot/right_joint") + builder.add_articulation([left_joint, right_joint], label="/World/Robot") + builder.articulation_label = ["/World/Robot"] + builder.joint_target_mode = [int(JointTargetMode.NONE), int(JointTargetMode.NONE)] + builder.joint_target_ke = [0.0, 0.0] + builder.joint_target_kd = [0.0, 0.0] + return builder + + +@pytest.mark.parametrize( + ("actuator_cfg", "imported_ke", "imported_kd", "initial_modes", "joint_types", "expected_modes"), + [ + pytest.param( + ImplicitActuatorCfg(joint_names_expr=[".*_joint"], stiffness=None, damping=None), + [8.0, 0.0], + [0.0, 3.0], + [0, 0], + None, + [1, 2], + id="imported-none-gains", + ), + pytest.param( + ImplicitActuatorCfg(joint_names_expr=[".*_joint"], stiffness=0.0, damping=0.0), + [8.0, 8.0], + [3.0, 3.0], + [2, 2], + None, + [4, 4], + id="zero-gains", + ), + pytest.param( + ImplicitActuatorCfg(joint_names_expr=[".*_joint"], stiffness=8.0, damping=3.0), + [0.0, 0.0], + [0.0, 0.0], + [0, 0], + None, + [3, 3], + id="both-gains", + ), + pytest.param( + IdealPDActuatorCfg(joint_names_expr=[".*_joint"], stiffness=8.0, damping=3.0), + [0.0, 0.0], + [0.0, 0.0], + [0, 0], + None, + [4, 4], + id="explicit-effort", + ), + pytest.param( + ImplicitActuatorCfg(joint_names_expr=[".*_joint"], stiffness=8.0, damping=3.0), + [0.0, 0.0], + [0.0, 0.0], + [1, 2], + [JointType.FREE, JointType.FIXED], + [1, 2], + id="free-fixed-excluded", + ), + pytest.param( + ImplicitActuatorCfg(joint_names_expr=["left_joint"], stiffness=8.0, damping=0.0), + [0.0, 0.0], + [0.0, 0.0], + [0, 2], + None, + [1, 2], + id="unconfigured-dof-unchanged", + ), + pytest.param( + ImplicitActuatorCfg( + joint_names_expr=[".*_joint"], + stiffness={"left_joint": 10.0}, + damping={"right_joint": 2.0}, + ), + [0.0, 0.0], + [0.0, 0.0], + [0, 0], + None, + [1, 2], + id="sparse-position-velocity", + ), + ], +) +def test_builder_target_modes_cover_all_adaptation_branches( + monkeypatch, + actuator_cfg, + imported_ke: list[float], + imported_kd: list[float], + initial_modes: list[int], + joint_types, + expected_modes: list[int], +) -> None: + """Every builder branch must assign literal modes without changing excluded or unmatched DOFs.""" + cfg = ArticulationCfg(prim_path="/World/Robot", actuators={"joints": actuator_cfg}) + monkeypatch.setattr( + "isaaclab_newton.assets.articulation.articulation._resolve_articulation_root_prim_path_expr", + lambda _cfg: "/World/Robot", + ) + builder = _target_mode_builder() + builder.joint_target_ke = imported_ke + builder.joint_target_kd = imported_kd + builder.joint_target_mode = initial_modes + if joint_types is not None: + builder.joint_type = joint_types + + _configure_builder_joint_target_modes(builder, cfg) + + assert builder.joint_target_mode == expected_modes + + +def test_prepare_native_actuators_activates_only_explicit_groups_without_gain_writes(monkeypatch) -> None: + """Newton adaptation must select explicit groups without clobbering imported solver gains.""" + articulation = SimpleNamespace(_sim_cfg=SimpleNamespace(use_newton_actuators=True)) + activations = [] + monkeypatch.setattr( + SimulationManager, "activate_newton_actuator_path", classmethod(lambda cls: activations.append(1)) + ) + + groups = NewtonActuatorControl(articulation).prepare_native_actuators( + collection=None, + actuator_cfgs={ + "implicit": ImplicitActuatorCfg(joint_names_expr=["left_joint"], stiffness=10.0, damping=1.0), + "explicit": IdealPDActuatorCfg(joint_names_expr=["right_joint"], stiffness=10.0, damping=1.0), + }, + ) + + assert groups == {"explicit"} + assert activations == [1] + assert articulation._has_newton_actuators + + +def test_resolve_actuator_component_rejects_ambiguous_clamping_owner() -> None: + """Actuator adaptation must reject two clamping components exposing the same parameter.""" + actuator = SimpleNamespace( + controller=SimpleNamespace(kp=1.0), + delay=None, + clamping=[SimpleNamespace(limit=1.0), SimpleNamespace(limit=2.0)], + ) + + with pytest.raises(ValueError, match="Ambiguous clamping parameter 'limit'"): + resolve_actuator_component(actuator, "clamping", "limit") + + +@pytest.mark.parametrize( + ("layout", "expected_offset"), + [ + (SimpleNamespace(offset=10, slice=slice(3, 5), indices=None), 13), + (SimpleNamespace(offset=10, slice=None, indices=wp.array([4, 7], dtype=wp.int32, device="cpu")), 14), + ], +) +def test_articulation_dof_offset_accounts_for_each_view_selection_layout(layout, expected_offset: int) -> None: + """Heterogeneous articulation bindings must offset native actuators by their selected model DOFs.""" + control = object.__new__(NewtonActuatorControl) + control._articulation = SimpleNamespace( + _root_view=SimpleNamespace(frequency_layouts={Model.AttributeFrequency.JOINT_DOF: layout}) + ) + + assert control._joint_dof_offset() == expected_offset + + +def test_native_actuator_reset_delegates_selected_environments_to_adapter(monkeypatch) -> None: + """A partial articulation reset must preserve state in unselected native-actuator environments.""" + reset_calls = [] + control = object.__new__(NewtonActuatorControl) + control._native_actuator_path_active = True + monkeypatch.setattr( + SimulationManager, "_adapter", SimpleNamespace(reset=lambda env_ids: reset_calls.append(env_ids)) + ) + env_ids = [1] + + control.reset_native_actuators(env_ids) + + assert reset_calls == [env_ids] + + +@pytest.mark.parametrize("has_ordering", [False, True]) +def test_torque_telemetry_preserves_public_joint_order(has_ordering: bool) -> None: + """Newton telemetry must map backend buffers exactly once when public ordering is active.""" + zeros = wp.zeros((1, 3), dtype=wp.float32, device="cpu") + effort_limit = wp.full((1, 3), 1000.0, dtype=wp.float32, device="cpu") + implicit = 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") + effort = wp.array([[100.0, 200.0, 300.0]], dtype=wp.float32, device="cpu") + computed_source = wp.array([[10.0, 20.0, 30.0]], dtype=wp.float32, device="cpu") + computed = wp.zeros_like(zeros) + applied = wp.zeros_like(zeros) + + wp.launch( + sync_torque_telemetry, + dim=zeros.shape, + inputs=[ + zeros, + zeros, + zeros, + zeros, + zeros, + zeros, + effort_limit, + implicit, + effort, + computed_source, + user_to_backend, + has_ordering, + ], + outputs=[computed, applied], + device="cpu", + ) + + if has_ordering: + np.testing.assert_allclose(computed.numpy(), [[30.0, 100.0, 20.0]]) + np.testing.assert_allclose(applied.numpy(), [[300.0, 100.0, 200.0]]) + else: + np.testing.assert_allclose(computed.numpy(), [[10.0, 200.0, 30.0]]) + np.testing.assert_allclose(applied.numpy(), [[100.0, 200.0, 300.0]]) diff --git a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py new file mode 100644 index 000000000000..c6df010c39f6 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -0,0 +1,142 @@ +# 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 + +"""Runtime guard for the kitless Newton asset suites.""" + +import os +import subprocess +import sys +from pathlib import Path + +import pytest + +pytestmark = pytest.mark.unit + +_ASSET_TEST_DIR = Path(__file__).resolve().parents[1] +_TARGETS = ( + ("test_rigid_object.py", "test_rigid_object_real_newton_seams[cpu]"), + ("test_rigid_object_collection.py", "test_rigid_object_collection_real_newton_seams"), + ("test_articulation.py", "test_articulation_initialization_and_partial_state"), + ("test_articulation.py", "test_articulation_joint_and_body_properties_round_trip"), + ("test_articulation.py", "test_articulation_drive_and_dynamics"), + ("test_articulation.py", "test_floating_articulation_root_and_wrench_response"), + ("test_newton_actuators_newton.py", "test_newton_actuator_real_equivalence"), + ("../controllers/test_newton_task_space_controllers.py", "test_differential_ik_tracks_local_newton_chain"), + ( + "../controllers/test_newton_task_space_controllers.py", + "test_operational_space_consumes_newton_jacobian_mass_and_gravity", + ), + ( + "../controllers/test_newton_task_space_controllers.py", + "test_operational_space_gravity_compensation_holds_static_chain", + ), +) + +_TARGET_FILENAMES = tuple(Path(target).name for target, _ in _TARGETS) + ("articulation_test_utils.py",) + + +def _run_monitored_targets(nodes: tuple[str, ...], tmp_path: Path) -> subprocess.CompletedProcess[str]: + """Run target nodes together under import and Nucleus sentinels.""" + sitecustomize = tmp_path / "sitecustomize.py" + sitecustomize.write_text( + """ +import importlib.abc +import sys +import isaaclab.utils.assets as assets + + +class _ForbiddenFinder(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + frame = sys._getframe(1) + while frame and ( + frame.f_code.co_filename.startswith(" None: + """Run all real CPU seams while rejecting Kit, IsaacSim, and Nucleus access.""" + nodes = tuple(f"{_ASSET_TEST_DIR / target}::{node}" for target, node in _TARGETS) + result = _run_monitored_targets(nodes, tmp_path) + + assert result.returncode == 0, result.stdout + result.stderr + + +def test_runtime_nucleus_access_inside_fixture_is_rejected(tmp_path: Path) -> None: + """The guard must execute fixture helpers instead of stopping after collection.""" + source_target = _ASSET_TEST_DIR / "test_rigid_object.py" + mutated_target = tmp_path / source_target.name + mutated_target.write_text( + source_target.read_text(encoding="utf-8").replace( + ' """Author two local dynamic cuboids and return their Newton asset."""', + ' """Author two local dynamic cuboids and return their Newton asset."""\n' + " from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR\n" + ' _ = ISAAC_NUCLEUS_DIR + "/forbidden.usd"', + ), + encoding="utf-8", + ) + + result = _run_monitored_targets((f"{mutated_target}::test_rigid_object_real_newton_seams[cpu]",), tmp_path) + + assert result.returncode != 0 + assert "forbidden Nucleus asset used by Newton asset test" in result.stdout + result.stderr + + +def test_runtime_app_launcher_import_inside_fixture_is_rejected(tmp_path: Path) -> None: + """The guard must reject AppLauncher imports reached only while a fixture helper executes.""" + source_target = _ASSET_TEST_DIR / "test_rigid_object.py" + mutated_target = tmp_path / source_target.name + mutated_target.write_text( + source_target.read_text(encoding="utf-8").replace( + ' """Author two local dynamic cuboids and return their Newton asset."""', + ' """Author two local dynamic cuboids and return their Newton asset."""\n' + " from isaaclab.app import AppLauncher", + ), + encoding="utf-8", + ) + + result = _run_monitored_targets((f"{mutated_target}::test_rigid_object_real_newton_seams[cpu]",), tmp_path) + + assert result.returncode != 0 + assert "forbidden kit dependency imported: isaaclab.app.app_launcher" in result.stdout + result.stderr diff --git a/source/isaaclab_newton/test/assets/unit/test_rigid_object_collection_model_indices.py b/source/isaaclab_newton/test/assets/unit/test_rigid_object_collection_model_indices.py new file mode 100644 index 000000000000..b9036dad04be --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_collection_model_indices.py @@ -0,0 +1,26 @@ +# 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 + +"""Focused tests for Newton rigid-object collection model selection.""" + +import pytest +from isaaclab_newton.assets import RigidObjectCollection + +pytestmark = pytest.mark.unit + + +def test_combined_pattern_preserves_common_leaf_prefix_around_body_index() -> None: + """The collection wildcard must not admit unrelated sibling bodies.""" + pattern = RigidObjectCollection._build_combined_pattern( + ["/World/Env_*/Object_0", "/World/Env_*/Object_1", "/World/Env_*/Object_2"] + ) + + assert pattern == "/World/Env_*/Object_*" + + +def test_combined_pattern_rejects_different_path_depths() -> None: + """Body expressions at different hierarchy depths cannot form one Newton view.""" + with pytest.raises(ValueError, match="different segment counts"): + RigidObjectCollection._build_combined_pattern(["/World/Env_*/Object_0", "/World/Env_*/Group/Object_1"]) diff --git a/source/isaaclab_newton/test/assets/unit/test_rigid_object_fk_cache.py b/source/isaaclab_newton/test/assets/unit/test_rigid_object_fk_cache.py new file mode 100644 index 000000000000..24a612aa3af0 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_fk_cache.py @@ -0,0 +1,76 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Focused tests for Newton rigid-body FK cache invalidation.""" + +from types import SimpleNamespace + +import pytest +from isaaclab_newton.assets.rigid_object.rigid_object_data import RigidObjectData +from isaaclab_newton.assets.rigid_object_collection.rigid_object_collection_data import RigidObjectCollectionData +from isaaclab_newton.physics import NewtonManager + +pytestmark = pytest.mark.unit + + +@pytest.mark.parametrize("data_type", [RigidObjectData, RigidObjectCollectionData]) +def test_stale_fk_timestamp_forwards_once(data_type, monkeypatch) -> None: + """The first same-timestamp body-pose read refreshes FK and later reads reuse it.""" + data = object.__new__(data_type) + data._sim_timestamp = 2.0 + data._fk_timestamp = -1.0 + calls = [] + monkeypatch.setattr(NewtonManager, "forward", lambda: calls.append("forward")) + + data._ensure_fk_fresh() + data._ensure_fk_fresh() + + assert calls == ["forward"] + assert data._fk_timestamp == 2.0 + + +@pytest.mark.parametrize("data_type", [RigidObjectData, RigidObjectCollectionData]) +def test_pose_reset_invalidates_fk_for_the_view_articulation_ids(data_type, monkeypatch) -> None: + """A root/body pose write invalidates Newton FK using the owning view mapping.""" + data = object.__new__(data_type) + data._sim_timestamp = 3.0 + data._fk_timestamp = 3.0 + data._root_view = SimpleNamespace(articulation_ids="mapped-articulations") + for name in ( + "_root_com_pose_w", + "_root_link_vel_w", + "_projected_gravity_b", + "_heading_w", + "_root_link_lin_vel_b", + "_root_link_ang_vel_b", + "_root_com_lin_vel_b", + "_root_com_ang_vel_b", + "_root_state_w", + "_root_link_state_w", + "_root_com_state_w", + "_body_com_pose_w", + "_body_link_vel_w", + "_body_link_lin_vel_b", + "_body_link_ang_vel_b", + "_body_com_lin_vel_b", + "_body_com_ang_vel_b", + "_body_state_w", + "_body_link_state_w", + "_body_com_state_w", + ): + setattr(data, name, None) + calls = [] + monkeypatch.setattr(NewtonManager, "invalidate_fk", lambda **kwargs: calls.append(kwargs)) + + data._reset_pose(env_ids="selected-envs") + + assert data._fk_timestamp == -1.0 + assert calls == [ + { + "env_mask": None, + "env_ids": "selected-envs", + "articulation_ids": "mapped-articulations", + } + ] diff --git a/source/isaaclab_newton/test/assets/unit/test_rigid_object_inertial_staging.py b/source/isaaclab_newton/test/assets/unit/test_rigid_object_inertial_staging.py new file mode 100644 index 000000000000..65bffe29e52b --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_inertial_staging.py @@ -0,0 +1,75 @@ +# 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 + +"""Focused kernel tests for Newton rigid-body inertial staging.""" + +import numpy as np +import pytest +import warp as wp +from isaaclab_newton.assets import kernels + +pytestmark = pytest.mark.unit + + +def _diagonal_inertias(values: list[tuple[float, float, float]]) -> wp.array: + data = np.zeros((2, 2, 9), dtype=np.float32) + for body_index, diagonal in enumerate(values): + data[0, body_index] = np.diag(diagonal).reshape(9) + data[1, body_index] = np.diag(diagonal).reshape(9) + return wp.array(data, dtype=wp.float32, device="cpu") + + +def test_masked_mass_staging_updates_only_selected_inverse_properties() -> None: + """A selected positive-to-static transition zeros both Newton inverse arrays.""" + masses = wp.array([[1.0, 1.0], [1.0, 0.0]], dtype=wp.float32, device="cpu") + body_mass = wp.ones((2, 2), dtype=wp.float32, device="cpu") + body_inertia = _diagonal_inertias([(2.0, 3.0, 4.0), (5.0, 6.0, 8.0)]) + body_inv_mass = wp.ones((2, 2), dtype=wp.float32, device="cpu") + body_inv_inertia = wp.ones((2, 2), dtype=wp.mat33f, device="cpu") + env_mask = wp.array([False, True], dtype=wp.bool, device="cpu") + body_mask = wp.array([False, True], dtype=wp.bool, device="cpu") + body_indices = wp.array([0, 1], dtype=wp.int32, device="cpu") + + wp.launch( + kernels.write_body_mass_and_inverse_mask, + dim=(2, 2), + inputs=[masses, env_mask, body_mask, body_indices, False, body_inertia], + outputs=[body_mass, body_mass, body_inv_mass, body_inv_inertia], + device="cpu", + ) + + np.testing.assert_allclose(body_mass.numpy(), [[1.0, 1.0], [1.0, 0.0]]) + np.testing.assert_allclose(body_inv_mass.numpy(), [[1.0, 1.0], [1.0, 0.0]]) + expected_inverse = np.ones((2, 2, 3, 3), dtype=np.float32) + expected_inverse[1, 1] = 0.0 + np.testing.assert_allclose(body_inv_inertia.numpy(), expected_inverse) + + +def test_masked_inertia_staging_inverts_selected_matrix_without_changing_mass() -> None: + """A selected inertia write updates its inverse while preserving inverse mass.""" + inertias = _diagonal_inertias([(2.0, 4.0, 8.0), (3.0, 5.0, 10.0)]) + body_mass = wp.full((2, 2), value=2.0, dtype=wp.float32, device="cpu") + body_inertia = _diagonal_inertias([(1.0, 1.0, 1.0), (1.0, 1.0, 1.0)]) + body_inv_mass = wp.full((2, 2), value=0.5, dtype=wp.float32, device="cpu") + body_inv_inertia = wp.ones((2, 2), dtype=wp.mat33f, device="cpu") + env_mask = wp.array([True, False], dtype=wp.bool, device="cpu") + body_mask = wp.array([False, True], dtype=wp.bool, device="cpu") + body_indices = wp.array([0, 1], dtype=wp.int32, device="cpu") + + wp.launch( + kernels.write_body_inertia_and_inverse_mask, + dim=(2, 2), + inputs=[inertias, env_mask, body_mask, body_indices, False, body_mass], + outputs=[body_inertia, body_inertia, body_inv_mass, body_inv_inertia], + device="cpu", + ) + + expected_inertia = _diagonal_inertias([(1.0, 1.0, 1.0), (1.0, 1.0, 1.0)]).numpy() + expected_inertia[0, 1] = np.diag([3.0, 5.0, 10.0]).reshape(9) + np.testing.assert_allclose(body_inertia.numpy(), expected_inertia) + np.testing.assert_allclose(body_inv_mass.numpy(), np.full((2, 2), 0.5, dtype=np.float32)) + expected_inverse = np.ones((2, 2, 3, 3), dtype=np.float32) + expected_inverse[0, 1] = np.diag([1.0 / 3.0, 0.2, 0.1]) + np.testing.assert_allclose(body_inv_inertia.numpy(), expected_inverse, atol=1e-6) diff --git a/source/isaaclab_newton/test/assets/unit/test_rigid_object_setter_notifications.py b/source/isaaclab_newton/test/assets/unit/test_rigid_object_setter_notifications.py new file mode 100644 index 000000000000..08d2b4b7056b --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_setter_notifications.py @@ -0,0 +1,117 @@ +# 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 + +"""Focused notification tests for Newton rigid-asset inertial setters.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import warp as wp +from isaaclab_newton.assets import Articulation, RigidObject, RigidObjectCollection +from isaaclab_newton.physics import NewtonManager as SimulationManager +from newton import ModelFlags + +pytestmark = pytest.mark.unit + + +def _diagonal_inertias(num_bodies: int, diagonal: tuple[float, float, float]) -> wp.array: + data = np.zeros((2, num_bodies, 9), dtype=np.float32) + data[:] = np.diag(diagonal).reshape(9) + return wp.array(data, dtype=wp.float32, device="cpu") + + +def _minimal_asset(asset_type, num_bodies: int): + """Create a tiny array-backed asset for invoking production setters.""" + asset = object.__new__(asset_type) + asset._device = "cpu" + asset._check_shapes = False + asset._ALL_BODY_INDICES = wp.array(np.arange(num_bodies), dtype=wp.int32, device="cpu") + if asset_type in (Articulation, RigidObject): + asset._ALL_INDICES = wp.array([0, 1], dtype=wp.int32, device="cpu") + else: + asset._ALL_ENV_INDICES = wp.array([0, 1], dtype=wp.int32, device="cpu") + staged_inertia = _diagonal_inertias(num_bodies, (2.0, 3.0, 4.0)) + data = SimpleNamespace( + has_body_ordering=False, + body_ordering=None, + _body_mass_user=None, + _body_inertia_user=None, + _sim_bind_body_mass=wp.ones((2, num_bodies), dtype=wp.float32, device="cpu"), + _sim_bind_body_inv_mass=wp.ones((2, num_bodies), dtype=wp.float32, device="cpu"), + _sim_bind_body_inv_inertia=wp.ones((2, num_bodies), dtype=wp.mat33f, device="cpu"), + _sim_bind_body_inertia=staged_inertia, + _body_inertia=staged_inertia, + ) + asset._data = data + return asset, staged_inertia + + +@pytest.mark.parametrize( + ("asset_type", "num_bodies"), + [(Articulation, 2), (RigidObject, 1), (RigidObjectCollection, 2)], +) +def test_mass_setter_stages_only_selection_and_notifies_inertial_change( + asset_type, num_bodies: int, monkeypatch +) -> None: + """Production mass setters stage one selection and emit exactly one inertial notification.""" + asset, _ = _minimal_asset(asset_type, num_bodies) + notifications = [] + monkeypatch.setattr( + SimulationManager, + "add_model_change", + classmethod(lambda cls, flag: notifications.append(flag)), + ) + env_ids = wp.array([1], dtype=wp.int32, device="cpu") + body_ids = wp.array([num_bodies - 1], dtype=wp.int32, device="cpu") + + asset.set_masses_index( + masses=wp.array([[4.0]], dtype=wp.float32, device="cpu"), + env_ids=env_ids, + body_ids=body_ids, + ) + + expected_mass = np.ones((2, num_bodies), dtype=np.float32) + expected_mass[1, -1] = 4.0 + expected_inv_mass = np.ones((2, num_bodies), dtype=np.float32) + expected_inv_mass[1, -1] = 0.25 + np.testing.assert_allclose(asset.data._sim_bind_body_mass.numpy(), expected_mass) + np.testing.assert_allclose(asset.data._sim_bind_body_inv_mass.numpy(), expected_inv_mass) + assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] + + +@pytest.mark.parametrize( + ("asset_type", "num_bodies"), + [(Articulation, 2), (RigidObject, 1), (RigidObjectCollection, 2)], +) +def test_inertia_setter_stages_only_selection_and_notifies_inertial_change( + asset_type, num_bodies: int, monkeypatch +) -> None: + """Production inertia setters stage one selection and emit exactly one inertial notification.""" + asset, staged_inertia = _minimal_asset(asset_type, num_bodies) + notifications = [] + monkeypatch.setattr( + SimulationManager, + "add_model_change", + classmethod(lambda cls, flag: notifications.append(flag)), + ) + env_ids = wp.array([1], dtype=wp.int32, device="cpu") + body_ids = wp.array([num_bodies - 1], dtype=wp.int32, device="cpu") + inertias = np.zeros((1, 1, 9), dtype=np.float32) + inertias[0, 0] = np.diag([5.0, 10.0, 20.0]).reshape(9) + + asset.set_inertias_index( + inertias=wp.array(inertias, dtype=wp.float32, device="cpu"), + env_ids=env_ids, + body_ids=body_ids, + ) + + expected_inertia = _diagonal_inertias(num_bodies, (2.0, 3.0, 4.0)).numpy() + expected_inertia[1, -1] = inertias[0, 0] + np.testing.assert_allclose(staged_inertia.numpy(), expected_inertia) + expected_inv_inertia = np.ones((2, num_bodies, 3, 3), dtype=np.float32) + expected_inv_inertia[1, -1] = np.diag([0.2, 0.1, 0.05]) + np.testing.assert_allclose(asset.data._sim_bind_body_inv_inertia.numpy(), expected_inv_inertia, atol=1e-6) + assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] diff --git a/source/isaaclab_newton/test/assets/test_wrench_kernels.py b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py similarity index 54% rename from source/isaaclab_newton/test/assets/test_wrench_kernels.py rename to source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py index 4dc272cc9417..b1c55689b982 100644 --- a/source/isaaclab_newton/test/assets/test_wrench_kernels.py +++ b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py @@ -6,10 +6,13 @@ """Regression tests for Newton external-wrench packing kernels.""" import numpy as np +import pytest import warp as wp from isaaclab_newton.assets import kernels as shared_kernels from isaaclab_newton.assets.articulation import kernels as articulation_kernels +pytestmark = pytest.mark.unit + _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)) @@ -44,7 +47,7 @@ def test_update_wrench_array_rotates_body_wrenches_to_world_frame() -> None: ) -def test_update_wrench_array_ordered_rotates_and_scatter_wrenches_to_backend_order() -> None: +def test_update_wrench_array_ordered_rotates_and_scatters_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") @@ -71,3 +74,78 @@ def test_update_wrench_array_ordered_rotates_and_scatter_wrenches_to_backend_ord 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, ) + + +def test_update_wrench_array_preserves_false_mask_entries() -> None: + """False environment and body mask entries must leave destination wrenches untouched.""" + forces = wp.array( + np.asarray([[[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]], [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]]]), + dtype=wp.vec3f, + device="cpu", + ) + torques = wp.array( + np.asarray([[[10.0, 0.0, 0.0], [20.0, 0.0, 0.0]], [[30.0, 0.0, 0.0], [40.0, 0.0, 0.0]]]), + dtype=wp.vec3f, + device="cpu", + ) + poses = np.zeros((2, 2, 7), dtype=np.float32) + poses[..., 6] = 1.0 + body_link_pose_w = wp.array(poses, dtype=wp.transformf, device="cpu") + initial_wrench = np.full((2, 2, 6), -7.0, dtype=np.float32) + wrench = wp.array(initial_wrench, dtype=wp.spatial_vectorf, device="cpu") + + wp.launch( + shared_kernels.update_wrench_array_with_force_and_torque, + dim=(2, 2), + inputs=[ + forces, + torques, + body_link_pose_w, + wrench, + wp.array([True, False], dtype=wp.bool, device="cpu"), + wp.array([False, True], dtype=wp.bool, device="cpu"), + ], + device="cpu", + ) + + expected = initial_wrench.copy() + expected[0, 1] = [2.0, 0.0, 0.0, 20.0, 0.0, 0.0] + np.testing.assert_allclose(wrench.numpy(), expected) + + +def test_update_wrench_array_ordered_preserves_false_masks_while_scattering() -> None: + """Ordered packing must apply both masks before scattering the selected public body.""" + forces = wp.array( + np.asarray([[[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]], [[3.0, 0.0, 0.0], [4.0, 0.0, 0.0]]]), + dtype=wp.vec3f, + device="cpu", + ) + torques = wp.array( + np.asarray([[[10.0, 0.0, 0.0], [20.0, 0.0, 0.0]], [[30.0, 0.0, 0.0], [40.0, 0.0, 0.0]]]), + dtype=wp.vec3f, + device="cpu", + ) + poses = np.zeros((2, 2, 7), dtype=np.float32) + poses[..., 6] = 1.0 + body_link_pose_w = wp.array(poses, dtype=wp.transformf, device="cpu") + initial_wrench = np.full((2, 2, 6), -7.0, dtype=np.float32) + wrench = wp.array(initial_wrench, dtype=wp.spatial_vectorf, device="cpu") + + wp.launch( + articulation_kernels.update_wrench_array_with_force_and_torque_ordered, + dim=(2, 2), + inputs=[ + forces, + torques, + body_link_pose_w, + wp.array([1, 0], dtype=wp.int32, device="cpu"), + wrench, + wp.array([False, True], dtype=wp.bool, device="cpu"), + wp.array([True, False], dtype=wp.bool, device="cpu"), + ], + device="cpu", + ) + + expected = initial_wrench.copy() + expected[1, 1] = [3.0, 0.0, 0.0, 30.0, 0.0, 0.0] + np.testing.assert_allclose(wrench.numpy(), expected) diff --git a/source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py b/source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py new file mode 100644 index 000000000000..df72294389ca --- /dev/null +++ b/source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py @@ -0,0 +1,177 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Kitless controller bridges for Newton articulation dynamics data.""" + +import pytest +import torch + +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.controllers import ( + DifferentialIKController, + DifferentialIKControllerCfg, + OperationalSpaceController, + OperationalSpaceControllerCfg, +) +from isaaclab.utils.math import subtract_frame_transforms + +from source.isaaclab_newton.test.articulation_test_utils import author_fixed_spatial_chain, build_newton_context + +pytestmark = pytest.mark.integration + + +def _end_effector_pose_b(articulation) -> torch.Tensor: + """Return the last link pose in the fixed root frame.""" + root_pose_w = articulation.data.root_link_pose_w.torch + ee_pose_w = articulation.data.body_link_pose_w.torch[:, -1] + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, :3], root_pose_w[:, 3:7], ee_pose_w[:, :3], ee_pose_w[:, 3:7] + ) + return torch.cat((ee_pos_b, ee_quat_b), dim=-1) + + +def _unpowered_actuators() -> dict: + """Return passive joint configuration for effort-controlled bridges.""" + return { + "joints": ImplicitActuatorCfg( + joint_names_expr=["Joint_.*"], + stiffness=0.0, + damping=0.0, + ) + } + + +def test_differential_ik_tracks_local_newton_chain() -> None: + """A DifferentialIK command must move the real Newton end effector toward its target.""" + with build_newton_context() as sim: + articulation = author_fixed_spatial_chain() + sim.reset() + + initial_pose_b = _end_effector_pose_b(articulation) + target_pose_b = initial_pose_b.clone() + target_pose_b[:, 0] += 0.04 + jacobian = articulation.data.body_link_jacobian_w.torch[:, -1] + controller = DifferentialIKController( + DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + num_envs=1, + device="cpu", + ) + controller.set_command(target_pose_b, ee_quat=initial_pose_b[:, 3:7]) + joint_target = controller.compute( + initial_pose_b[:, :3], + initial_pose_b[:, 3:7], + jacobian, + articulation.data.joint_pos.torch, + ) + assert torch.linalg.vector_norm(joint_target - articulation.data.joint_pos.torch) > 1e-3 + initial_error = torch.linalg.vector_norm(target_pose_b[:, :3] - initial_pose_b[:, :3]) + + articulation.actuators.target_command.set_position_index(value=joint_target) + for _ in range(20): + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + + final_pose_b = _end_effector_pose_b(articulation) + final_error = torch.linalg.vector_norm(target_pose_b[:, :3] - final_pose_b[:, :3]) + assert final_error < 0.5 * initial_error + + +def test_operational_space_consumes_newton_jacobian_mass_and_gravity() -> None: + """OSC must turn live Newton dynamics into a finite, mass-dependent motion response.""" + with build_newton_context(gravity=(0.0, -9.81, 0.0)) as sim: + articulation = author_fixed_spatial_chain(actuators=_unpowered_actuators()) + sim.reset() + + ee_pose_b = _end_effector_pose_b(articulation) + target_pose_b = ee_pose_b.clone() + target_pose_b[:, 0] += 0.02 + jacobian = articulation.data.body_link_jacobian_w.torch[:, -1] + mass_matrix = articulation.data.mass_matrix.torch + gravity = articulation.data.gravity_compensation_forces.torch + assert torch.linalg.matrix_rank(jacobian).item() == 6 + assert torch.all(torch.linalg.eigvalsh(mass_matrix) > 0.0) + assert gravity[0, 1] > 1.0 + + cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + inertial_dynamics_decoupling=True, + gravity_compensation=True, + motion_stiffness_task=20.0, + motion_damping_ratio_task=1.0, + ) + controller = OperationalSpaceController(cfg, num_envs=1, device="cpu") + controller.set_command(target_pose_b) + effort = controller.compute( + jacobian, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=articulation.data.body_link_vel_w.torch[:, -1], + mass_matrix=mass_matrix, + gravity=gravity, + ) + + noninertial_cfg = cfg.replace(inertial_dynamics_decoupling=False) + noninertial_controller = OperationalSpaceController(noninertial_cfg, num_envs=1, device="cpu") + noninertial_controller.set_command(target_pose_b) + noninertial_effort = noninertial_controller.compute( + jacobian, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=articulation.data.body_link_vel_w.torch[:, -1], + gravity=gravity, + ) + assert torch.isfinite(effort).all() + assert torch.linalg.vector_norm(effort) > 1.0 + assert not torch.allclose(effort, noninertial_effort) + initial_error = torch.linalg.vector_norm(target_pose_b[:, :3] - ee_pose_b[:, :3]) + + articulation.actuators.target_command.set_effort_index(value=effort) + for _ in range(8): + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + + final_error = torch.linalg.vector_norm(target_pose_b[:, :3] - _end_effector_pose_b(articulation)[:, :3]) + assert final_error < initial_error + + +def _gravity_drift(*, compensate: bool) -> torch.Tensor: + """Measure passive joint drift in a new context, optionally applying OSC gravity effort.""" + with build_newton_context(gravity=(0.0, -9.81, 0.0)) as sim: + articulation = author_fixed_spatial_chain(actuators=_unpowered_actuators()) + sim.reset() + initial_joint_pos = articulation.data.joint_pos.torch.clone() + controller = OperationalSpaceController( + OperationalSpaceControllerCfg( + target_types=["wrench_abs"], + contact_wrench_control_axes_task=(0, 0, 0, 0, 0, 0), + gravity_compensation=True, + ), + num_envs=1, + device="cpu", + ) + controller.set_command(torch.zeros((1, 6))) + + for _ in range(20): + effort = torch.zeros_like(articulation.data.joint_pos.torch) + if compensate: + effort = controller.compute( + articulation.data.body_link_jacobian_w.torch[:, -1], + gravity=articulation.data.gravity_compensation_forces.torch, + ) + articulation.actuators.target_command.set_effort_index(value=effort) + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + + return torch.linalg.vector_norm(articulation.data.joint_pos.torch - initial_joint_pos) + + +def test_operational_space_gravity_compensation_holds_static_chain() -> None: + """Live Newton gravity effort must hold the chain materially closer to its initial state.""" + uncompensated_drift = _gravity_drift(compensate=False) + compensated_drift = _gravity_drift(compensate=True) + + assert uncompensated_drift > 1e-2 + assert compensated_drift < 0.1 * uncompensated_drift diff --git a/source/isaaclab_ov/changelog.d/asset-tests-redesign.rst b/source/isaaclab_ov/changelog.d/asset-tests-redesign.rst new file mode 100644 index 000000000000..02d998f6d68b --- /dev/null +++ b/source/isaaclab_ov/changelog.d/asset-tests-redesign.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_ov.physics.OvPhysxManager` to allow CPU and GPU + scenes to run sequentially in one process. diff --git a/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py b/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py index 03c953559883..dc89142b5653 100644 --- a/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py +++ b/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py @@ -371,8 +371,6 @@ class OvPhysxManager(PhysicsManager): _warmup_done: ClassVar[bool] = False _next_control_ordinal: ClassVar[int] = 2 _requires_full_stage: ClassVar[bool] = False - # Device mode is process-wide; later contexts must reuse the first selected device. - _locked_device: ClassVar[str | None] = None # Active clone recipes survive the consumable pending queue so a forced # re-warmup can rebuild serialized-stage or runtime-only clones. _active_clone_recipes: ClassVar[list[tuple[str, list[str], list[CloneTransform]]]] = [] @@ -481,8 +479,7 @@ def initialize(cls, sim_context: SimulationContext) -> None: ``cls._physx`` is intentionally not cleared here: if the current :class:`SimulationContext` already constructed it and has not been - closed, the manager reuses that instance. ``cls._locked_device`` carries - IsaacLab's conservative first-device policy for this process. + closed, the manager reuses that instance. """ super().initialize(sim_context) cls._ensure_physx_schemas_registered() @@ -573,10 +570,7 @@ def close(cls) -> None: def _release_physx(cls) -> None: """Release the OVPhysX runtime instance and its owned OVStage. - Safe to call multiple times. ``_locked_device`` intentionally survives - release so later IsaacLab contexts keep the process's first device - choice; this is required for CPU-first processes and conservative for - GPU-first processes. + Safe to call multiple times. """ physx = cls._physx cls._physx = None @@ -870,19 +864,14 @@ def _warmup_and_load(cls) -> None: """Serialize the USD stage and attach it to the ovphysx runtime. When no runtime is active, constructs a new :class:`ovphysx.PhysX` - instance. The first construction also records IsaacLab's process device - choice and registers process-exit cleanup. On a forced re-warm before + instance and registers process-exit cleanup. On a forced re-warm before :meth:`close`, it reuses the active instance, attaches the new USD through OVStage, rebuilds active clone recipes through full-stage materialization or runtime replay, and (on GPU) re-runs ``warmup_gpu`` so the new stage's bodies are resident. Raises: - RuntimeError: If ``SimulationContext`` is not set, or if a device - different from IsaacLab's first device choice is requested. - OVPhysX CPU-only mode is process-wide and cannot be reversed; - IsaacLab applies the same conservative policy in both - directions for a predictable lifecycle. + RuntimeError: If ``SimulationContext`` is not set. """ sim = PhysicsManager._sim if sim is None: @@ -897,13 +886,6 @@ def _warmup_and_load(cls) -> None: gpu_index = 0 ovphysx_device = "cpu" - if cls._locked_device is not None and ovphysx_device != cls._locked_device: - raise RuntimeError( - f"OvPhysxManager is locked to device {cls._locked_device!r} for the lifetime of this process; " - f"cannot switch to {ovphysx_device!r}. IsaacLab pins the first OVPhysX device choice because " - "CPU-only mode cannot be reversed; restart the process to use a different device." - ) - scene_prim = sim.stage.GetPrimAtPath(sim.cfg.physics_prim_path) if scene_prim.IsValid(): cls._configure_physx_scene_prim(scene_prim, PhysicsManager._cfg, ovphysx_device) @@ -934,7 +916,6 @@ def _warmup_and_load(cls) -> None: if cls._physx is None: cls._construct_physx(ovphysx_device, gpu_index) - cls._locked_device = ovphysx_device else: # Bindings are tied to the realized objects of one stage. Invalidate # asset/sensor handles and drain generic views before resetting the @@ -1026,7 +1007,6 @@ def _create_physx_instance(ovphysx: Any, ovphysx_device: str, gpu_index: int) -> "/physics/suppressFabricUpdate": True, } ) - ovphysx.PhysX.set_cpu_mode(ovphysx_device == "cpu") physx_kwargs = { "config": ovphysx.PhysXConfig(num_threads=8, carbonite_overrides=carbonite_overrides), } @@ -1043,10 +1023,11 @@ def _configure_physx_scene_prim(scene_prim, cfg, device: str) -> None: so we write the apiSchemas list entry and scene attributes directly via raw Sdf metadata manipulation instead of using the high-level USD API. - The schema, scene-query-support, and solver-determinism/accuracy attributes are applied - regardless of device. The GPU-specific dynamics/broadphase/capacity attributes are - applied only when ``device == "gpu"`` — without them PhysX defaults to - CPU broadphase even when OVPhysX is configured for GPU execution. + The schema, scene-query-support, solver-determinism/accuracy, and + dynamics/broadphase attributes are applied regardless of device. CPU + scenes explicitly use ``enableGPUDynamics=false`` and ``broadphaseType=MBP``; + GPU scenes use ``enableGPUDynamics=true`` and ``broadphaseType=GPU``. + GPU buffer-capacity attributes are only applied when ``device == "gpu"``. Args: scene_prim: The /World/PhysicsScene prim to configure. @@ -1078,17 +1059,18 @@ def _configure_physx_scene_prim(scene_prim, cfg, device: str) -> None: cfg.enable_external_forces_every_iteration ) - if device == "gpu": - scene_prim.CreateAttribute("physxScene:enableGPUDynamics", Sdf.ValueTypeNames.Bool).Set(True) - scene_prim.CreateAttribute("physxScene:broadphaseType", Sdf.ValueTypeNames.String).Set("GPU") - - if cfg is not None: - for attr, val in [ - ("gpuMaxRigidContactCount", cfg.gpu_max_rigid_contact_count), - ("gpuMaxRigidPatchCount", cfg.gpu_max_rigid_patch_count), - ("gpuFoundLostPairsCapacity", cfg.gpu_found_lost_pairs_capacity), - ("gpuFoundLostAggregatePairsCapacity", cfg.gpu_found_lost_aggregate_pairs_capacity), - ("gpuTotalAggregatePairsCapacity", cfg.gpu_total_aggregate_pairs_capacity), - ("gpuCollisionStackSize", cfg.gpu_collision_stack_size), - ]: - scene_prim.CreateAttribute(f"physxScene:{attr}", Sdf.ValueTypeNames.UInt).Set(val) + scene_prim.CreateAttribute("physxScene:enableGPUDynamics", Sdf.ValueTypeNames.Bool).Set(device == "gpu") + scene_prim.CreateAttribute("physxScene:broadphaseType", Sdf.ValueTypeNames.String).Set( + "GPU" if device == "gpu" else "MBP" + ) + + if device == "gpu" and cfg is not None: + for attr, val in [ + ("gpuMaxRigidContactCount", cfg.gpu_max_rigid_contact_count), + ("gpuMaxRigidPatchCount", cfg.gpu_max_rigid_patch_count), + ("gpuFoundLostPairsCapacity", cfg.gpu_found_lost_pairs_capacity), + ("gpuFoundLostAggregatePairsCapacity", cfg.gpu_found_lost_aggregate_pairs_capacity), + ("gpuTotalAggregatePairsCapacity", cfg.gpu_total_aggregate_pairs_capacity), + ("gpuCollisionStackSize", cfg.gpu_collision_stack_size), + ]: + scene_prim.CreateAttribute(f"physxScene:{attr}", Sdf.ValueTypeNames.UInt).Set(val) diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index 63a35ea00efd..d2ad71245ba4 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -3,3367 +3,375 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Real-backend tests for the OVPhysX Articulation. - -Mirrors :mod:`isaaclab_physx.test.assets.test_articulation` 1-to-1: same set -of test functions, names, parametrizations, and assertions. - -OVPhysX runs kitless under ``./scripts/run_ovphysx.sh`` so there is no -``AppLauncher`` boot — :class:`~isaaclab.sim.SimulationContext` is driven -directly via ``build_simulation_context(sim_cfg=SimulationCfg(physics=OvPhysxCfg(), ...))`` -which works because :func:`isaaclab.app.has_kit` returns False in this -environment. - -PhysX-specific ``cube_object.root_view.set_X(...)`` / ``get_X(...)`` calls are -adapted to OVPhysX by going through -:attr:`~isaaclab_ov.assets.Articulation.root_view`, an -:class:`~isaaclab_ov.sim.views.OvPhysxView` over the per-tensor-type bindings -(``root_view.get_attribute(tensor_type)`` / -:meth:`~isaaclab_ov.assets.Articulation._get_binding`), and the public setters -(:meth:`set_masses_index`, :meth:`set_coms_index`, :meth:`set_inertias_index`). -Reads use the data-class properties (``cube_object.data.body_mass``, -``body_inertia``, ``body_com_pose_b``). - -Process-global device lock --------------------------- - -The OVPhysX runtime fixes device mode (CPU vs GPU) when the process creates -its first ``ovphysx.PhysX`` instance and cannot switch it without a process -restart. :class:`~isaaclab_ov.physics.OvPhysxManager` tracks -this on ``_locked_device`` and raises :exc:`RuntimeError` if a later -:class:`SimulationContext` requests a different device. The -``_ovphysx_skip_other_device`` autouse fixture below preempts that error in -parametrized tests by ``pytest.skip``-ing on the unlocked device, so the -session finishes cleanly when only one device is exercised. - -CI note -------- -Because the lock is process-global, full coverage requires **two separate -``./scripts/run_ovphysx.sh -m pytest`` invocations** -- once with ``-k 'cpu'`` -and once with ``-k 'cuda:0'``. Until the wheel exposes a way to reset Carbonite -device state, this is the supported pattern. -""" +"""Real OVPhysX articulation coverage on one module-scoped scene.""" from __future__ import annotations -import importlib -import sys +from dataclasses import dataclass from pathlib import Path -from unittest.mock import Mock import pytest import torch import warp as wp -from pxr import Usd, UsdGeom, UsdPhysics +from pxr import Gf, Sdf, UsdPhysics -from isaaclab.test.utils import test_devices -from isaaclab.test.utils.articulation_ordering import ( - ANYMAL_C_PHYSX_JOINT_NAMES, - BRANCHING_MJWARP_BODY_NAMES, - BRANCHING_MJWARP_JOINT_NAMES, - BRANCHING_PHYSX_BODY_NAMES, - BRANCHING_PHYSX_JOINT_NAMES, - PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES, -) - -# The OVPhysX runtime wheel is optional. Skip gracefully when it is not installed; -# CI jobs that need OVPhysX coverage install it explicitly. pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") from isaaclab_ov import tensor_types as TT # noqa: E402 from isaaclab_ov.assets import Articulation # noqa: E402 -from isaaclab_ov.assets.articulation.articulation_data import ArticulationData # noqa: E402 from isaaclab_ov.physics import OvPhysxCfg # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 -import isaaclab.utils.math as math_utils # noqa: E402 -import isaaclab.utils.string as string_utils # noqa: E402 -from isaaclab.actuators import DelayedPDActuatorCfg, IdealPDActuatorCfg, ImplicitActuatorCfg # noqa: E402 -from isaaclab.assets import ArticulationCfg, get_articulation_name_ordering # noqa: E402 -from isaaclab.assets.articulation import ordering_kernels # noqa: E402 -from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit # noqa: E402 -from isaaclab.managers import SceneEntityCfg # noqa: E402 -from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 -from isaaclab.utils.version import get_isaac_sim_version, has_kit # noqa: E402 -from isaaclab.utils.warp.launch_cache import _WarpLaunchCache # noqa: E402 - -## -# Pre-defined configs -## -from isaaclab_assets import ANYMAL_C_CFG, CARTPOLE_CFG, FRANKA_PANDA_CFG, SHADOW_HAND_CFG # isort:skip - -wp.init() - -pytestmark = pytest.mark.device_split - - -_OMNI_PHYSX_SCHEMAS_GAP_REASON = ( - "Schema-level fixed-joint creation in :mod:`isaaclab.sim.schemas` imports the Kit-only " - "``omni.physx.scripts.utils`` module, which is not shipped by the ovphysx wheel." -) - -_SPATIAL_TENDON_OVSTAGE_GAP_REASON = ( - "OVPhysX 0.5.9 segfaults while attaching OVStage scenes containing spatial tendon schemas." +from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg # noqa: E402 +from isaaclab.assets import ArticulationCfg # noqa: E402 +from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context # noqa: E402 +from isaaclab.test.utils.articulation_ordering import ( # noqa: E402 + BRANCHING_MJWARP_BODY_NAMES, + BRANCHING_MJWARP_JOINT_NAMES, ) - -def test_cached_read_launches_reset_on_ordering_and_invalidation(): - """Ordering installation and simulation invalidation should discard recorded reads.""" - - class MinimalData(ArticulationData): - def __dir__(self): - return [] - - class Buffer: - timestamp = 1.0 - - data = MinimalData.__new__(MinimalData) - read_launch_cache = Mock() - data._read_launch_cache = read_launch_cache - data._configure_ordering_buffers = lambda: None - data._make_jacobian_body_user_to_backend = lambda: object() - data.joint_ordering = None - data._body_com_jacobian_w = Buffer() - data._mass_matrix = Buffer() - data._gravity_compensation_forces = Buffer() - - data._apply_ordering_maps_after_resolve() - - read_launch_cache.clear.assert_called_once_with() - assert data._body_com_jacobian_w.timestamp == -1.0 - assert data._mass_matrix.timestamp == -1.0 - assert data._gravity_compensation_forces.timestamp == -1.0 - - data._is_primed = True - data._sim_timestamp = 1.0 - data._invalidate_initialize_callback(None) - - assert read_launch_cache.clear.call_count == 2 - assert data._is_primed is False - assert data._sim_timestamp == 0.0 - - -def test_generalized_dynamics_reorder_uses_public_joint_order(): - """OVPhysX dynamics reads should gather both matrix joint axes into public order.""" - - class Buffer: - def __init__(self): - self.data = wp.zeros((1, 2, 2), dtype=wp.float32, device="cpu") - self.timestamp = -1.0 - - data = ArticulationData.__new__(ArticulationData) - data.device = "cpu" - data._sim_timestamp = 1.0 - data._read_launch_cache = _WarpLaunchCache("cpu") - data.joint_ordering = object() - data._jacobian_joint_user_to_backend = wp.array([1, 0], dtype=wp.int32, device="cpu") - data._joint_dof_signs = wp.ones(2, dtype=wp.int32, device="cpu") - data._has_reversed_joints = False - data._num_base_dofs = 0 - - backend_values = wp.array([[[1.0, 2.0], [3.0, 4.0]]], dtype=wp.float32, device="cpu") - backend_buffer = wp.zeros_like(backend_values) - buffer = Buffer() - - def read_binding(tensor_type, dst): - dst.assign(backend_values) - - data._binding_read = read_binding - data._refresh_generalized_dynamics_buffer( - buffer, - backend_buffer, - TT.MASS_MATRIX, - ordering_kernels.reorder_mass_matrix_backend_to_user, +pytestmark = pytest.mark.integration + +_FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" +_NUM_ENVS = 2 + + +@dataclass +class _ArticulationScene: + """Articulations that share one real OVPhysX lifecycle.""" + + sim: SimulationContext + ordered: Articulation + floating: Articulation + tendon: Articulation + native: Articulation | None + device: str + + +def _sim_context(device: str, *, use_newton_actuators: bool): + """Build one local OVPhysX context from an in-memory USD stage.""" + return build_simulation_context( + sim_cfg=SimulationCfg( + physics=OvPhysxCfg(), + device=device, + gravity=(0.0, 0.0, 0.0), + use_newton_actuators=use_newton_actuators, + ), + auto_add_lighting=False, + ) + + +def _apply_api_schema(prim, schema_name: str) -> None: + """Author an applied-schema token without loading the Kit PhysX schema module.""" + schemas = list(prim.GetAppliedSchemas()) + schemas.append(schema_name) + api_schemas = Sdf.TokenListOp() + api_schemas.explicitItems = schemas + prim.SetMetadata("apiSchemas", api_schemas) + + +def _spawn_articulation( + name: str, + *, + y_offset: float, + fixed_base: bool, + native_actuator: bool = False, + spatial_tendon: bool = False, +) -> Articulation: + """Spawn one cached two-environment branching-articulation island.""" + island_path = f"/World/{name}" + sim_utils.create_prim(island_path, "Xform", translation=(0.0, y_offset, 0.0)) + for env_index in range(_NUM_ENVS): + sim_utils.create_prim(f"{island_path}/Env_{env_index}", "Xform", translation=(3.0 * env_index, 0.0, 0.0)) + actuator_cfg = ( + IdealPDActuatorCfg( + joint_names_expr=[".*"], + stiffness=5.0, + damping=0.5, + actuator_effort_limit=100.0, + ) + if native_actuator + else ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=5.0, damping=0.5) ) - - torch.testing.assert_close( - wp.to_torch(buffer.data), - torch.tensor([[[4.0, 3.0], [2.0, 1.0]]]), - ) - assert buffer.timestamp == 1.0 - - -def _read_binding_to_torch(articulation: Articulation, tensor_type: int, device: str | torch.device) -> torch.Tensor: - """Read an OVPhysX attribute into a torch tensor on *device*. - - Test-side adapter for the verbatim PhysX mirror. PhysX cross-checks the - data class against the simulation via ``articulation.root_view.get_X()`` - accessors; on OVPhysX we go through the equivalent - :meth:`~isaaclab_ov.sim.views.OvPhysxView.get_attribute`, which returns a - freshly allocated ``float32`` array on the attribute's native device (CPU for - CPU-only property types), then move the result to *device*. - """ - arr = articulation.root_view.get_attribute(tensor_type) - return wp.to_torch(arr).to(device) - - -# Session-locked device. Set on the first parametrized test that runs and -# never reassigned -- ovphysx's process-global device lock means subsequent -# tests on the other device must skip. -_LOCKED_DEVICE: list[str | None] = [None] - - -@pytest.fixture(autouse=True) -def _ovphysx_skip_other_device(request): - """Skip tests whose ``device`` parameter mismatches the session-locked device. - - The OVPhysX runtime locks process-global device mode when the process - creates its first ``ovphysx.PhysX`` instance, so any test parametrized to a - different device after the first ``sim.reset()`` would hit the manager's - :exc:`RuntimeError`. We detect the locked device on the - first encounter and skip subsequent tests on the other device with a clear - message so the run finishes cleanly rather than producing spurious failures. - """ - callspec = getattr(request.node, "callspec", None) - device = callspec.params.get("device") if callspec is not None else None - if device is None: - # Test does not parametrize on device (e.g. test_warmup_attach_stage_not_called_for_cpu). - return - locked = _LOCKED_DEVICE[0] - if locked is None: - _LOCKED_DEVICE[0] = device - return - if device != locked: - pytest.skip( - f"ovphysx process-global device lock is held by '{locked}'; cannot run '{device}' " - "tests in the same session. Run pytest twice (once per device) for full coverage." + articulation = Articulation( + ArticulationCfg( + prim_path=f"{island_path}/Env_[^/]*/Robot", + spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), + actuators={"joints": actuator_cfg}, + joint_ordering="mjwarp", + body_ordering="mjwarp", ) - - -def _ovphysx_sim_context(device: str, **kwargs): - """Wrapper around :func:`build_simulation_context` that injects OVPhysX cfg. - - PhysX tests pass ``device=device`` directly and let - :func:`build_simulation_context` build a default :class:`SimulationCfg`. - OVPhysX needs ``physics=OvPhysxCfg()`` set on the cfg so the manager - dispatches to OVPhysX rather than PhysX, so we build the cfg here and - pass it through. ``gravity_enabled`` is consumed locally (it is ignored - by ``build_simulation_context`` once a ``sim_cfg`` is provided). - ``add_ground_plane``, ``auto_add_lighting``, and other kwargs continue - to flow through ``build_simulation_context`` as before. - """ - dt = kwargs.pop("dt", 1.0 / 60.0) - gravity_enabled = kwargs.pop("gravity_enabled", True) - use_newton_actuators = kwargs.pop("use_newton_actuators", False) - gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - sim_cfg = SimulationCfg( - physics=OvPhysxCfg(), - device=device, - dt=dt, - gravity=gravity, - use_newton_actuators=use_newton_actuators, ) - return build_simulation_context(device=device, sim_cfg=sim_cfg, **kwargs) - - -def generate_articulation_cfg( - articulation_type: str, - stiffness: float | None = 10.0, - damping: float | None = 2.0, - actuator_velocity_limit: float | None = None, - actuator_effort_limit: float | None = None, - joint_velocity_limit: float | None = None, - joint_effort_limit: float | None = None, -) -> ArticulationCfg: - """Generate an articulation configuration. - - Args: - articulation_type: Type of articulation to generate. - It should be one of: "humanoid", "panda", "anymal", "shadow_hand", "single_joint_implicit", - "single_joint_explicit". - stiffness: Stiffness value for the articulation's actuators. Only currently used for "humanoid". - Defaults to 10.0. - damping: Damping value for the articulation's actuators. Only currently used for "humanoid". - Defaults to 2.0. - actuator_velocity_limit: Velocity limit for the actuators. Only currently used for "single_joint_implicit" - and "single_joint_explicit". - actuator_effort_limit: Effort limit for explicit actuators. Only currently used for - "single_joint_explicit". - joint_velocity_limit: Velocity limit for the actuators (set into the simulation). - Only currently used for "single_joint_implicit" and "single_joint_explicit". - joint_effort_limit: Effort limit for the actuators (set into the simulation). - Only currently used for "single_joint_implicit" and "single_joint_explicit". - - Returns: - The articulation configuration for the requested articulation type. - - """ - if articulation_type == "humanoid": - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/Humanoid/humanoid_instanceable.usd" - ), - init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)), - actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, - ) - elif articulation_type == "panda": - articulation_cfg = FRANKA_PANDA_CFG - elif articulation_type == "anymal": - articulation_cfg = ANYMAL_C_CFG - elif articulation_type == "shadow_hand": - articulation_cfg = SHADOW_HAND_CFG - elif articulation_type == "single_joint_implicit": - articulation_cfg = ArticulationCfg( - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), - ), - actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - joint_effort_limit=joint_effort_limit, - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=actuator_velocity_limit, - stiffness=2000.0, - damping=100.0, - ), - }, - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos=({"RevoluteJoint": 1.5708}), - rot=(0.7071081, 0, 0, 0.7071055), - ), - ) - elif articulation_type == "single_joint_explicit": - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), - ), - actuators={ - "joint": IdealPDActuatorCfg( - joint_names_expr=[".*"], - joint_effort_limit=joint_effort_limit, - joint_velocity_limit=joint_velocity_limit, - actuator_effort_limit=actuator_effort_limit, - actuator_velocity_limit=actuator_velocity_limit, - stiffness=0.0, - damping=10.0, - ), - }, - ) - elif articulation_type == "spatial_tendon_test_asset": - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/IsaacLab/Tests/spatial_tendons.usd", - ), - actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=2000.0, - damping=100.0, - ), - }, - ) - else: - raise ValueError( - f"Invalid articulation type: {articulation_type}, valid options are 'humanoid', 'panda', 'anymal'," - " 'shadow_hand', 'single_joint_implicit', 'single_joint_explicit' or 'spatial_tendon_test_asset'." - ) - - return articulation_cfg - - -def generate_articulation( - articulation_cfg: ArticulationCfg, num_articulations: int, device: str -) -> tuple[Articulation, torch.tensor]: - """Generate an articulation from a configuration. - - Handles the creation of the articulation, the environment prims and the articulation's environment - translations - - Args: - articulation_cfg: Articulation configuration. - num_articulations: Number of articulations to generate. - device: Device to use for the tensors. - - Returns: - The articulation and environment translations. - - """ - # Generate translations of 2.5 m in x for each articulation - translations = torch.zeros(num_articulations, 3, device=device) - translations[:, 0] = torch.arange(num_articulations) * 2.5 - - # Create Top-level Xforms, one for each articulation - for i in range(num_articulations): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) - articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_[^/]*/Robot")) - - return articulation, translations - - -@pytest.mark.parametrize("device", ["cuda:0"]) -def test_newton_native_explicit_actuator_submits_ovphysx_effort(device): - """Run a Newton-native explicit actuator through the current OVPhysX state and effort binding.""" - stiffness, damping, actuator_effort_limit = 20.0, 1.0, 80.0 - with _ovphysx_sim_context(device=device, gravity_enabled=False, use_newton_actuators=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg("single_joint_explicit").replace( - actuators={ - "joint": IdealPDActuatorCfg( - joint_names_expr=[".*"], - stiffness=stiffness, - damping=damping, - actuator_effort_limit=actuator_effort_limit, - ) - } - ) - articulation, _ = generate_articulation(articulation_cfg, 1, device) - sim.reset() - - initial_pos = articulation.data.joint_pos.torch.clone() - target = initial_pos + 0.5 - articulation.actuators.target_command.set_position_index(value=target) - articulation.write_data_to_sim() - - assert articulation._actuator_control.native_actuator_path_active - assert articulation.newton_actuator_adapter is not None - assert torch.any(articulation.actuators.computed_effort.torch != 0.0) - assert torch.any(articulation.actuators.applied_effort.torch != 0.0) - torch.testing.assert_close( - _read_binding_to_torch(articulation, TT.DOF_ACTUATION_FORCE, device), - articulation.actuators.applied_effort.torch, + stage = sim_utils.get_current_stage() + for env_index in range(_NUM_ENVS): + prim_path = f"{island_path}/Env_{env_index}/Robot" + for joint_name in ("left_shoulder", "left_elbow", "right_shoulder", "right_elbow"): + drive = UsdPhysics.DriveAPI.Apply(stage.GetPrimAtPath(f"{prim_path}/{joint_name}"), "angular") + drive.CreateStiffnessAttr(5.0) + drive.CreateDampingAttr(0.5) + drive.CreateMaxForceAttr(100.0) + if fixed_base: + fixed_joint = UsdPhysics.FixedJoint.Define(stage, f"{prim_path}/fixed_root") + fixed_joint.GetBody1Rel().SetTargets([f"{prim_path}/base"]) + if spatial_tendon: + root_prim = stage.GetPrimAtPath(f"{prim_path}/base") + _apply_api_schema(root_prim, "PhysxTendonAttachmentRootAPI:root") + root_prim.CreateAttribute("physxTendon:root:localPos", Sdf.ValueTypeNames.Point3f).Set(Gf.Vec3f(0.0)) + root_prim.CreateAttribute("physxTendon:root:stiffness", Sdf.ValueTypeNames.Float).Set(5.0) + root_prim.CreateAttribute("physxTendon:root:damping", Sdf.ValueTypeNames.Float).Set(0.5) + root_prim.CreateAttribute("physxTendon:root:limitStiffness", Sdf.ValueTypeNames.Float).Set(1.0) + root_prim.CreateAttribute("physxTendon:root:offset", Sdf.ValueTypeNames.Float).Set(0.0) + + leaf_prim = stage.GetPrimAtPath(f"{prim_path}/left_tip") + _apply_api_schema(leaf_prim, "PhysxTendonAttachmentLeafAPI:leaf") + leaf_prim.CreateAttribute("physxTendon:leaf:localPos", Sdf.ValueTypeNames.Point3f).Set(Gf.Vec3f(0.0)) + leaf_prim.CreateAttribute("physxTendon:leaf:parentAttachment", Sdf.ValueTypeNames.Token).Set("root") + leaf_prim.CreateRelationship("physxTendon:leaf:parentLink").SetTargets([root_prim.GetPath()]) + leaf_prim.CreateAttribute("physxTendon:leaf:restLength", Sdf.ValueTypeNames.Float).Set(0.5) + leaf_prim.CreateAttribute("physxTendon:leaf:lowerLimit", Sdf.ValueTypeNames.Float).Set(0.0) + leaf_prim.CreateAttribute("physxTendon:leaf:upperLimit", Sdf.ValueTypeNames.Float).Set(2.0) + return articulation + + +@pytest.fixture(scope="module") +def articulation_scene() -> _ArticulationScene: + """Initialize every real OVPhysX articulation once for this module.""" + device = "cuda:0" if wp.is_cuda_available() else "cpu" + native_enabled = device.startswith("cuda") + with _sim_context(device, use_newton_actuators=native_enabled) as sim: + ordered = _spawn_articulation("Ordered", y_offset=0.0, fixed_base=True) + floating = _spawn_articulation("Floating", y_offset=3.0, fixed_base=False) + tendon = _spawn_articulation("Tendon", y_offset=6.0, fixed_base=True, spatial_tendon=True) + native = ( + _spawn_articulation("Native", y_offset=9.0, fixed_base=True, native_actuator=True) + if native_enabled + else None ) - - sim.step() - articulation.update(sim.cfg.dt) - # Use raw OV bindings so the observation cannot refresh the public state shadow. - current_pos = _read_binding_to_torch(articulation, TT.DOF_POSITION, device) - current_vel = _read_binding_to_torch(articulation, TT.DOF_VELOCITY, device) - assert not torch.allclose(current_pos, initial_pos) - - articulation.write_data_to_sim() - expected_effort = torch.clamp( - stiffness * (target - current_pos) - damping * current_vel, - -actuator_effort_limit, - actuator_effort_limit, - ) - torch.testing.assert_close(articulation.actuators.applied_effort.torch, expected_effort) - - -@pytest.mark.parametrize( - "module_name", - [ - "isaaclab_physx.assets.articulation.actuator_control", - "isaaclab_ov.assets.articulation.actuator_control", - ], -) -def test_host_actuator_control_import_does_not_probe_optional_newton_runtime(monkeypatch, module_name): - """Import host controls without probing an unrequested Newton optional dependency.""" - original_find_spec = importlib.util.find_spec - - def reject_newton_probe(name, *args, **kwargs): - if name.startswith("isaaclab_newton"): - raise AssertionError("host actuator-control import eagerly probed Newton") - return original_find_spec(name, *args, **kwargs) - - monkeypatch.setattr(importlib.util, "find_spec", reject_newton_probe) - importlib.reload(importlib.import_module(module_name)) - - -@pytest.mark.parametrize("device", ["cuda:0"]) -def test_newton_native_ovphysx_effort_binding_excludes_implicit_pd(device): - """Submit raw native effort so OVPhysX evaluates the implicit joint drive once.""" - with _ovphysx_sim_context(device=device, gravity_enabled=False, use_newton_actuators=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = CARTPOLE_CFG.replace( - actuators={ - "cart": ImplicitActuatorCfg( - joint_names_expr=["slider_to_cart"], joint_effort_limit=400.0, stiffness=20.0, damping=0.0 - ), - "pole": IdealPDActuatorCfg( - joint_names_expr=["cart_to_pole"], - stiffness=20.0, - damping=0.0, - actuator_effort_limit=400.0, - ), - } - ) - articulation, _ = generate_articulation(articulation_cfg, 1, device) sim.reset() - - articulation.actuators.target_command.set_position_index( - value=articulation.data.joint_pos.torch + torch.tensor([[0.25, 0.5]], device=device) - ) - articulation.write_data_to_sim() - - raw_effort = wp.to_torch(articulation._physx_actuator_wrapper.joint_f_2d) - applied_effort = articulation.actuators.applied_effort.torch - assert torch.any(applied_effort[:, 0] != raw_effort[:, 0]) - torch.testing.assert_close( - _read_binding_to_torch(articulation, TT.DOF_ACTUATION_FORCE, device), - raw_effort, + yield _ArticulationScene( + sim=sim, + ordered=ordered, + floating=floating, + tendon=tendon, + native=native, + device=device, ) -@pytest.mark.parametrize("device", ["cuda:0"]) -def test_newton_native_actuator_reset_and_gain_event_are_environment_selective(device): - """Reset and randomize only the selected OVPhysX native-controller environment.""" - from isaaclab.envs.mdp.events import randomize_actuator_gains # noqa: PLC0415 - from isaaclab.managers import EventTermCfg, SceneEntityCfg # noqa: PLC0415 - - class Env: - def __init__(self, asset): - self.scene = self - self.num_envs = asset.num_instances - self.device = asset.device - self._asset = asset - - def __getitem__(self, name): - assert name == "robot" - return self._asset - - with _ovphysx_sim_context(device=device, use_newton_actuators=True) as sim: - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg("single_joint_explicit").replace( - actuators={ - "joint": DelayedPDActuatorCfg( - joint_names_expr=[".*"], - stiffness=20.0, - damping=1.0, - actuator_effort_limit=80.0, - min_delay=1, - max_delay=1, - ) - } - ) - articulation, _ = generate_articulation(articulation_cfg, 2, device) - sim.reset() - for _ in range(3): - articulation.write_data_to_sim() - sim.step() - articulation.update(sim.cfg.dt) - - adapter = articulation.newton_actuator_adapter - stateful_pairs = [ - state - for actuator, state in zip(adapter.actuators, adapter._states_a) - if state is not None and getattr(state, "delay_state", None) is not None - ] - assert len(stateful_pairs) == 1 - articulation.reset(env_ids=torch.tensor([0], device=device, dtype=torch.long)) - assert stateful_pairs[0].delay_state.num_pushes.numpy().tolist() == [0, 1] - - env = Env(articulation) - asset_cfg = SceneEntityCfg("robot") - event_params = { - "asset_cfg": asset_cfg, - "stiffness_distribution_params": (101.0, 101.0), - "damping_distribution_params": (3.0, 3.0), - "operation": "abs", - "distribution": "uniform", - } - event = randomize_actuator_gains(EventTermCfg(func=randomize_actuator_gains, params=event_params), env) - event(env, env_ids=torch.tensor([0], device=device), **event_params) - - from isaaclab.actuators.newton import read_group_parameter - - stiffness = read_group_parameter(articulation.actuators, "joint", "controller", "kp") - damping = read_group_parameter(articulation.actuators, "joint", "controller", "kd") - torch.testing.assert_close(stiffness, torch.tensor([[101.0], [20.0]], device=device)) - torch.testing.assert_close(damping, torch.tensor([[3.0], [1.0]], device=device)) - - -@pytest.fixture -def sim(request): - """Create simulation context with the specified device.""" - device = request.getfixturevalue("device") - if "gravity_enabled" in request.fixturenames: - gravity_enabled = request.getfixturevalue("gravity_enabled") - else: - gravity_enabled = True # default to gravity enabled - if "add_ground_plane" in request.fixturenames: - add_ground_plane = request.getfixturevalue("add_ground_plane") - else: - add_ground_plane = False # default to no ground plane - with _ovphysx_sim_context( - device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane - ) as sim: - sim._app_control_on_stop_handle = None - yield sim - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_write_joint_state_accepts_int64_selector(sim, device, gravity_enabled): - """Write joint state with int64 selectors.""" - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation(articulation_cfg, 2, device=device) - sim.reset() - assert articulation.num_joints >= 2 +def test_articulation_initialization_and_partial_state(articulation_scene: _ArticulationScene) -> None: + """Prove ordering and indexed state writes against the real OVPhysX view.""" + articulation = articulation_scene.ordered + device = articulation_scene.device + assert articulation.is_initialized + assert articulation.is_fixed_base + assert tuple(articulation.joint_names) == BRANCHING_MJWARP_JOINT_NAMES + assert tuple(articulation.body_names) == BRANCHING_MJWARP_BODY_NAMES + assert articulation.joint_ordering is not None + assert articulation.body_ordering is not None + assert articulation.num_instances == 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) + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32, device=device) + target_position = torch.tensor([[0.21, -0.13]], device=device) + target_velocity = torch.tensor([[0.41, -0.23]], device=device) expected_position = articulation.data.joint_pos.torch.clone() expected_velocity = articulation.data.joint_vel.torch.clone() - + expected_position[env_ids[:, None], joint_ids] = target_position + expected_velocity[env_ids[:, None], joint_ids] = target_velocity articulation.write_joint_state_to_sim_index( - position=position, velocity=velocity, env_ids=env_ids, joint_ids=joint_ids + position=target_position, + velocity=target_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]) -def test_reversed_joint_dynamics_use_public_joint_basis(sim, device, gravity_enabled): - """Keep dynamics tensors consistent with public joint velocity.""" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg( - usd_path=str(Path(__file__).parent / "data" / "articulation_ordering_branching.usda") - ), - actuators={}, - ) - ) - UsdPhysics.FixedJoint.Define(sim.stage, "/World/Robot/fixed_root").GetBody1Rel().SetTargets(["/World/Robot/base"]) - joint = UsdPhysics.RevoluteJoint.Get(sim.stage, "/World/Robot/left_elbow") - body0, body1 = joint.GetBody0Rel().GetTargets(), joint.GetBody1Rel().GetTargets() - joint.GetBody0Rel().SetTargets(body1) - joint.GetBody1Rel().SetTargets(body0) - sim.reset() - - velocity = torch.zeros((1, articulation.num_joints), device=device) - velocity[:, articulation.find_joints("left_shoulder")[0][0]] = 0.4 - velocity[:, articulation.find_joints("left_elbow")[0][0]] = 0.7 - articulation.write_joint_velocity_to_sim_index(velocity=velocity) - sim.step() - articulation.update(sim.cfg.dt) - - joint_velocity = articulation.data.joint_vel.torch - predicted_velocity = torch.einsum("nbij,nj->nbi", articulation.data.body_com_jacobian_w.torch, joint_velocity) - torch.testing.assert_close(predicted_velocity, articulation.data.body_com_vel_w.torch[:, 1:], atol=1e-5, rtol=1e-5) - - generalized_energy = 0.5 * torch.einsum( - "ni,nij,nj->n", joint_velocity, articulation.data.mass_matrix.torch, joint_velocity - ) - body_velocity = articulation.data.body_com_vel_w.torch - body_inertia = articulation.data.body_inertia.torch.reshape(1, articulation.num_bodies, 3, 3) - body_energy = 0.5 * ( - (articulation.data.body_mass.torch.unsqueeze(-1) * body_velocity[..., :3].square()).sum((-1, -2)) - + torch.einsum("nbi,nbij,nbj->n", body_velocity[..., 3:], body_inertia, body_velocity[..., 3:]) - ) - torch.testing.assert_close(generalized_energy, body_energy, atol=1e-5, rtol=1e-5) - - -def test_joint_dof_sign_resolution_traverses_instance_proxies(): - """Resolve reversed joints inside an instanceable articulation.""" - source_stage = Usd.Stage.CreateInMemory() - UsdGeom.Xform.Define(source_stage, "/Robot") - UsdGeom.Xform.Define(source_stage, "/Robot/base") - UsdGeom.Xform.Define(source_stage, "/Robot/link") - joint = UsdPhysics.RevoluteJoint.Define(source_stage, "/Robot/joint") - joint.GetBody0Rel().SetTargets(["/Robot/link"]) - joint.GetBody1Rel().SetTargets(["/Robot/base"]) - stage = Usd.Stage.CreateInMemory() - instance = UsdGeom.Xform.Define(stage, "/World/Robot").GetPrim() - instance.GetReferences().AddReference(source_stage.GetRootLayer().identifier, "/Robot") - instance.SetInstanceable(True) - - articulation = Mock( - cfg=Mock(prim_path="/World/Robot"), - _joint_names=["joint"], - _body_names=["base", "link"], - ) - - assert Articulation._resolve_joint_dof_signs(articulation, stage) == (-1,) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_live_anymal_c_manual_joint_ordering_preserves_unselected_backend_state(sim, num_articulations, device): - """Test that a partial ordered write preserves every unselected backend joint.""" - articulation_cfg = generate_articulation_cfg("anymal").replace( - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)) - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - - joint_ordering = articulation.joint_ordering - assert joint_ordering is not None - backend_seed = torch.arange(1, articulation.num_joints + 1, dtype=torch.float32, device=device).reshape(1, -1) - backend_seed *= 0.001 - articulation.root_view.set_attribute(TT.DOF_POSITION, wp.from_torch(backend_seed)) - backend_before = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION)).clone() - torch.testing.assert_close(backend_before, backend_seed, rtol=0.0, atol=0.0) - backend_joint_id = joint_ordering.user_to_backend_indices[0] - selected_value = backend_before[0, backend_joint_id] + 0.001 - - articulation.write_joint_position_to_sim_index( - position=selected_value.reshape(1, 1), - env_ids=wp.array([0], dtype=wp.int32, device=device), - joint_ids=wp.array([0], dtype=wp.int32, device=device), - ) - - backend_after = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION)).clone() - expected = backend_before.clone() - expected[0, backend_joint_id] = selected_value - torch.testing.assert_close(backend_after, expected, rtol=0.0, atol=0.0) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_live_anymal_c_manual_joint_ordering_reorders_joint_targets(sim, device): - """Write nonidentity-ordered joint targets into their intended backend columns.""" - backend_joint_names = ANYMAL_C_PHYSX_JOINT_NAMES - joint_ordering = (*backend_joint_names[1:], backend_joint_names[0]) - articulation_cfg = generate_articulation_cfg("anymal").replace( - joint_ordering=joint_ordering, - actuators={"legs": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=2.0)}, - ) - articulation, _ = generate_articulation(articulation_cfg, 1, device=device) - sim.reset() - - ordering = articulation.joint_ordering - assert ordering is not None - user_to_backend = torch.as_tensor(ordering.user_to_backend_indices, dtype=torch.long, device=device) - backend_to_user = torch.as_tensor(ordering.backend_to_user_indices, dtype=torch.long, device=device) - assert not torch.equal(user_to_backend, backend_to_user) - - joint_index = torch.arange(articulation.num_joints, dtype=torch.float32, device=device).unsqueeze(0) - position_target = -0.25 + 0.031 * joint_index - velocity_target = 0.07 + 0.017 * joint_index - articulation.set_joint_position_target_index(target=position_target) - articulation.set_joint_velocity_target_index(target=velocity_target) - articulation.write_data_to_sim() - - backend_position_target = _read_binding_to_torch(articulation, TT.DOF_POSITION_TARGET, device) - backend_velocity_target = _read_binding_to_torch(articulation, TT.DOF_VELOCITY_TARGET, device) - torch.testing.assert_close(backend_position_target, position_target[:, backend_to_user]) - torch.testing.assert_close(backend_velocity_target, velocity_target[:, backend_to_user]) - - -@pytest.mark.parametrize("device", ["cpu"]) -def test_live_anymal_c_manual_joint_ordering_reorders_joint_friction_properties(sim, device): - """Read every friction component from backend order into public joint order.""" - backend_joint_names = ANYMAL_C_PHYSX_JOINT_NAMES - joint_ordering = (*backend_joint_names[1:], backend_joint_names[0]) - articulation_cfg = generate_articulation_cfg("anymal").replace(joint_ordering=joint_ordering) - articulation, _ = generate_articulation(articulation_cfg, 1, device=device) - sim.reset() - - ordering = articulation.joint_ordering - assert ordering is not None - user_to_backend = torch.as_tensor(ordering.user_to_backend_indices, dtype=torch.long, device=device) - joint_index = torch.arange(articulation.num_joints, dtype=torch.float32, device=device).unsqueeze(0) - backend_friction = torch.stack( - (20.0 + joint_index, 10.0 + 0.5 * joint_index, 1.0 + 0.25 * joint_index), - dim=-1, - ) - articulation.root_view.set_attribute( - TT.DOF_FRICTION_PROPERTIES, - wp.from_torch(backend_friction.contiguous()), - ) - articulation.data._joint_friction_props_buf.timestamp = -1.0 - articulation.data._joint_friction_props_backend.timestamp = -1.0 - - expected = backend_friction[:, user_to_backend] - torch.testing.assert_close(articulation.data.joint_friction_coeff.torch, expected[..., 0]) - torch.testing.assert_close(articulation.data.joint_dynamic_friction_coeff.torch, expected[..., 1]) - torch.testing.assert_close(articulation.data.joint_viscous_friction_coeff.torch, expected[..., 2]) - - -@pytest.mark.parametrize("selection", ["full", "partial"]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_reversed_joint_ordering_joint_state_index_writes_backend_order(sim, selection, device): - """Write full and partial indexed joint state through a nonidentity public joint axis.""" - articulation_cfg = generate_articulation_cfg("anymal").replace( - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)) + backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION)), + expected_position[:, backend_to_user], ) - articulation, _ = generate_articulation(articulation_cfg, 2, device=device) - sim.reset() - - ordering = articulation.joint_ordering - assert ordering is not None - num_joints = articulation.num_joints - user_to_backend = torch.as_tensor(ordering.user_to_backend_indices, dtype=torch.long, device=device) - backend_to_user = torch.as_tensor(ordering.backend_to_user_indices, dtype=torch.long, device=device) - - backend_pos_before = torch.arange(2 * num_joints, dtype=torch.float32, device=device).reshape(2, num_joints) - backend_vel_before = backend_pos_before + 100.0 - articulation.root_view.set_attribute(TT.DOF_POSITION, wp.from_torch(backend_pos_before.contiguous())) - articulation.root_view.set_attribute(TT.DOF_VELOCITY, wp.from_torch(backend_vel_before.contiguous())) - for buffer in ( - articulation.data._joint_pos_buf, - articulation.data._joint_vel_buf, - articulation.data._joint_pos_backend, - articulation.data._joint_vel_backend, - ): - if buffer is not None: - buffer.timestamp = -1.0 - public_pos_before = articulation.data.joint_pos.torch.clone() - public_vel_before = articulation.data.joint_vel.torch.clone() - torch.testing.assert_close(public_pos_before, backend_pos_before[:, user_to_backend]) - torch.testing.assert_close(public_vel_before, backend_vel_before[:, user_to_backend]) - if selection == "full": - position = torch.arange(2 * num_joints, dtype=torch.float32, device=device).reshape(2, num_joints) + 200.0 - velocity = position + 100.0 - env_ids = None - joint_ids = None - expected_public_pos = position - expected_public_vel = velocity - expected_backend_pos = position[:, backend_to_user] - expected_backend_vel = velocity[:, backend_to_user] - else: - position = torch.tensor([[201.0, 203.0]], device=device) - velocity = torch.tensor([[301.0, 303.0]], device=device) - env_ids = [1] - joint_ids = [0, 2] - expected_public_pos = public_pos_before.clone() - expected_public_vel = public_vel_before.clone() - expected_public_pos[1, joint_ids] = position[0] - expected_public_vel[1, joint_ids] = velocity[0] - expected_backend_pos = backend_pos_before.clone() - expected_backend_vel = backend_vel_before.clone() - backend_joint_ids = user_to_backend[joint_ids] - expected_backend_pos[1, backend_joint_ids] = position[0] - expected_backend_vel[1, backend_joint_ids] = velocity[0] - - articulation.write_joint_state_to_sim_index( - position=position, - velocity=velocity, +def test_articulation_joint_and_body_properties_round_trip(articulation_scene: _ArticulationScene) -> None: + """Prove selected joint and body properties reach real OVPhysX bindings.""" + articulation = articulation_scene.ordered + device = articulation_scene.device + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32, device=device) + backend_friction_before = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)).clone() + static_friction = torch.tensor([[0.9, 0.7]], device=device) + dynamic_friction = torch.tensor([[0.4, 0.3]], device=device) + viscous_friction = torch.tensor([[0.11, 0.22]], device=device) + articulation.write_joint_friction_coefficient_to_sim_index( + joint_friction_coeff=static_friction, + joint_dynamic_friction_coeff=dynamic_friction, + joint_viscous_friction_coeff=viscous_friction, env_ids=env_ids, joint_ids=joint_ids, ) - - torch.testing.assert_close(articulation.data.joint_pos.torch, expected_public_pos) - torch.testing.assert_close(articulation.data.joint_vel.torch, expected_public_vel) - torch.testing.assert_close( - _read_binding_to_torch(articulation, TT.DOF_POSITION, device), - expected_backend_pos, - ) - torch.testing.assert_close( - _read_binding_to_torch(articulation, TT.DOF_VELOCITY, device), - expected_backend_vel, - ) - - -@pytest.mark.parametrize("num_articulations", [1]) -# COM pose is a CPU-resident OVPhysX binding (``_CPU_ONLY_TYPES``) even on a GPU sim, and this test -# restores it via the low-level ``root_view.set_attribute`` which forbids cross-device staging, so it -# is inherently CPU-only (aligning it to ``cuda:0`` would fail on the CPU-native COM binding). -@pytest.mark.parametrize("device", ["cpu"]) -def test_live_panda_manual_body_ordering_preserves_unselected_coms(sim, num_articulations, device): - """Test that a partial ordered COM write preserves every unselected backend body.""" - articulation_cfg = FRANKA_PANDA_CFG.replace(body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - - body_ordering = articulation.body_ordering - assert body_ordering is not None - backend_before = _read_binding_to_torch(articulation, TT.BODY_COM_POSE, device).clone() - assert torch.unique(backend_before[0], dim=0).shape[0] > 1 - - articulation.data._body_com_pose_b.timestamp = -1.0 - backend_staging = articulation.data._body_com_pose_b_backend - if backend_staging is not None: - backend_staging.timestamp = -1.0 - - public_body_id = 1 - backend_body_id = body_ordering.user_to_backend_indices[public_body_id] - assert backend_body_id != public_body_id - selected_com = backend_before[0, backend_body_id].clone() - selected_com[0] += 0.001 - - articulation.set_coms_index( - coms=wp.from_torch(selected_com.reshape(1, 1, 7).contiguous(), dtype=wp.transformf), - env_ids=wp.array([0], dtype=wp.int32, device=device), - body_ids=wp.array([public_body_id], dtype=wp.int32, device=device), - ) - - backend_after = _read_binding_to_torch(articulation, TT.BODY_COM_POSE, device).clone() - - articulation.root_view.set_attribute(TT.BODY_COM_POSE, wp.from_torch(backend_before.contiguous())) - noop_after = _read_binding_to_torch(articulation, TT.BODY_COM_POSE, device).clone() - unselected_body_mask = torch.ones(backend_before.shape[1], dtype=torch.bool, device=device) - unselected_body_mask[backend_body_id] = False - - assert torch.equal(noop_after[..., :3], backend_before[..., :3]) - assert torch.equal(backend_after[0, backend_body_id, :3], selected_com[:3]) - assert torch.equal(backend_after[0, unselected_body_mask, :3], backend_before[0, unselected_body_mask, :3]) - - # Bound semantic orientation equality by the native setter's float32 no-op normalization. - native_orientation_atol = torch.max( - torch.abs(noop_after[0, unselected_body_mask, 3:7] - backend_before[0, unselected_body_mask, 3:7]) - ).item() - assert native_orientation_atol <= torch.finfo(backend_before.dtype).eps + backend_joint_ids = torch.as_tensor(articulation.joint_ordering.user_to_backend_indices)[joint_ids.cpu()] + expected_backend_friction = backend_friction_before.clone() + expected_backend_friction[1, backend_joint_ids, 0] = static_friction.cpu() + expected_backend_friction[1, backend_joint_ids, 1] = dynamic_friction.cpu() + expected_backend_friction[1, backend_joint_ids, 2] = viscous_friction.cpu() torch.testing.assert_close( - backend_after[0, unselected_body_mask, 3:7], - backend_before[0, unselected_body_mask, 3:7], - rtol=0.0, - atol=native_orientation_atol, + wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)), + expected_backend_friction, + ) + + body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32, device=device) + backend_body_ids = torch.as_tensor(articulation.body_ordering.user_to_backend_indices)[body_ids.cpu()] + raw_mass_before = wp.to_torch(articulation.root_view.get_attribute(TT.BODY_MASS)).clone() + masses = torch.tensor([[2.5, 3.5]], device=device) + articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + expected_raw_mass = raw_mass_before.clone() + expected_raw_mass[1, backend_body_ids] = masses.cpu() + torch.testing.assert_close(wp.to_torch(articulation.root_view.get_attribute(TT.BODY_MASS)), expected_raw_mass) + + raw_com_before = wp.to_torch(articulation.root_view.get_attribute(TT.BODY_COM_POSE)).clone() + coms = articulation.data.body_com_pose_b.torch[env_ids][:, body_ids].clone() + coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03], device=device) + coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02], device=device) + articulation.set_coms_index(coms=wp.from_torch(coms, dtype=wp.transformf), env_ids=env_ids, body_ids=body_ids) + expected_raw_com = raw_com_before.clone() + expected_raw_com[1, backend_body_ids] = coms.cpu() + torch.testing.assert_close(wp.to_torch(articulation.root_view.get_attribute(TT.BODY_COM_POSE)), expected_raw_com) + + raw_inertia_before = wp.to_torch(articulation.root_view.get_attribute(TT.BODY_INERTIA)).clone() + inertias = articulation.data.body_inertia.torch[env_ids][:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[0, 1, 4] *= 1.3 + articulation.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) + expected_raw_inertia = raw_inertia_before.clone() + expected_raw_inertia[1, backend_body_ids] = inertias.cpu() + torch.testing.assert_close(wp.to_torch(articulation.root_view.get_attribute(TT.BODY_INERTIA)), expected_raw_inertia) + torch.testing.assert_close(articulation.data.body_mass.torch[env_ids][:, body_ids], masses) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[env_ids][:, body_ids], coms) + torch.testing.assert_close(articulation.data.body_inertia.torch[env_ids][:, body_ids], inertias) + + +def test_articulation_drive_and_dynamics(articulation_scene: _ArticulationScene) -> None: + """Prove implicit drive delivery and live OVPhysX dynamics access.""" + articulation = articulation_scene.ordered + device = articulation_scene.device + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([0], dtype=torch.int32, device=device) + articulation.write_joint_velocity_to_sim_index(velocity=torch.zeros_like(articulation.data.joint_vel.torch)) + initial_drive_position = articulation.data.joint_pos.torch[:, 0].clone() + raw_target_before = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)).clone() + drive_target = articulation.data.joint_pos.torch[env_ids][:, joint_ids].clone() + 0.4 + articulation.actuators.target_command.set_position_index( + value=drive_target, + env_ids=env_ids, + joint_ids=joint_ids, ) - assert torch.equal(backend_after[..., 3:7], noop_after[..., 3:7]) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_reversed_body_ordering_wrench_composes_from_backend_pose_without_shadow_refresh( - sim, num_articulations, device -): - """Reversed body ordering: an external wrench composes from the backend-order link pose and - ``write_data_to_sim`` no longer refreshes the public ``body_link_pose_w`` shadow. - """ - articulation_cfg = FRANKA_PANDA_CFG.replace(body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - - body_ordering = articulation.body_ordering - assert body_ordering is not None - - # Apply a body-frame wrench to a single named body (public order). - body_ids, _ = articulation.find_bodies("panda_hand") - public_body_id = body_ids[0] - backend_body_id = int(body_ordering.user_to_backend_indices[public_body_id]) - assert backend_body_id != public_body_id # exercises the reorder - - force_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=device) - torque_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=device) - force_b[..., 0], force_b[..., 1], force_b[..., 2] = 3.0, -5.0, 7.0 - torque_b[..., 0], torque_b[..., 1], torque_b[..., 2] = 0.5, -1.5, 2.5 + articulation.write_data_to_sim() + backend_target = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)) + backend_joint_id = articulation.joint_ordering.user_to_backend_indices[0] + torch.testing.assert_close(backend_target[1, backend_joint_id], drive_target[0, 0]) + torch.testing.assert_close(backend_target[0], raw_target_before[0]) + + for _ in range(8): + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + articulation.write_data_to_sim() + assert torch.abs(articulation.data.joint_pos.torch[1, 0] - initial_drive_position[1]) > 1e-6 + jacobian = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobian.shape == (_NUM_ENVS, articulation.num_bodies - 1, 6, articulation.num_joints) + assert mass_matrix.shape == (_NUM_ENVS, articulation.num_joints, articulation.num_joints) + assert jacobian.device.type == torch.device(articulation_scene.device).type + assert mass_matrix.device.type == torch.device(articulation_scene.device).type + assert torch.isfinite(jacobian).all() + assert torch.isfinite(mass_matrix).all() + torch.testing.assert_close(mass_matrix, mass_matrix.transpose(-1, -2), atol=1e-5, rtol=1e-5) + + +def test_floating_articulation_root_and_wrench_response(articulation_scene: _ArticulationScene) -> None: + """Prove floating-root state and a real OVPhysX external-wrench response.""" + articulation = articulation_scene.floating + device = articulation_scene.device + assert articulation.is_initialized + assert not articulation.is_fixed_base + assert articulation.data.root_link_pose_w.torch.shape == (_NUM_ENVS, 7) + assert articulation.data.root_com_pose_w.torch.shape == (_NUM_ENVS, 7) + assert articulation.data.body_link_pose_w.torch.shape == (_NUM_ENVS, articulation.num_bodies, 7) + assert articulation.data.body_com_pose_w.torch.shape == (_NUM_ENVS, articulation.num_bodies, 7) + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + initial_pose = articulation.data.root_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids].clone() + target_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3], device=device) + articulation.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[env_ids], target_pose) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_pose[:1]) + initial_velocity = articulation.data.root_com_lin_vel_w.torch.clone() articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=force_b, torques=torque_b, body_ids=body_ids + forces=torch.tensor([[[8.0, 0.0, 0.0]]], device=device), + torques=torch.zeros((1, 1, 3), device=device), + env_ids=env_ids, + body_ids=torch.tensor([0], dtype=torch.int32, device=device), ) - - # Step once so the link poses are non-trivial (rotated), giving the quaternion rotation teeth. - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - sim.step() - articulation.update(sim.cfg.dt) - - # write_data_to_sim must NOT advance the public body_link_pose_w shadow timestamp: the wrench - # path now reads the backend-order pose buffer instead of refreshing the public shadow. - shadow_ts_before = articulation.data._body_link_pose_w.timestamp articulation.write_data_to_sim() - assert articulation.data._body_link_pose_w.timestamp == shadow_ts_before - - # The wrench buffer is in backend order; the world-frame wrench must match the one composed - # from the SAME physical body's pose read via the public (user-order) shadow. - wrench_buf = wp.to_torch(articulation._wrench_buf).to(device) - pose = articulation.data.body_link_pose_w.torch[0, public_body_id] # user order, [pos(3), quat_xyzw(4)] - quat_xyzw = pose[3:7] - expected_force_w = math_utils.quat_apply(quat_xyzw, force_b[0, 0]) - expected_torque_w = math_utils.quat_apply(quat_xyzw, torque_b[0, 0]) - torch.testing.assert_close(wrench_buf[0, backend_body_id, 0:3], expected_force_w, rtol=1e-4, atol=1e-4) - torch.testing.assert_close(wrench_buf[0, backend_body_id, 3:6], expected_torque_w, rtol=1e-4, atol=1e-4) - torch.testing.assert_close(wrench_buf[0, backend_body_id, 6:9], pose[0:3], rtol=1e-4, atol=1e-4) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_reversed_joint_ordering_joint_acc_matches_canonicalized_finite_difference(sim, num_articulations, device): - """Reversed joint ordering: the fused ``joint_acc`` finite difference reads the backend-order - velocity source and equals the identity-order acceleration permuted into public order. - """ - articulation_cfg = generate_articulation_cfg("anymal").replace( - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)) - ) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - data = articulation.data - - joint_ordering = articulation.joint_ordering - assert joint_ordering is not None - num_joints = articulation.num_joints - - user_to_backend = torch.as_tensor( - [int(joint_ordering.user_to_backend_indices[u]) for u in range(num_joints)], - dtype=torch.long, - device=device, - ) - - # Controlled, non-uniform finite-difference scenario in backend order (so a wrong permutation - # in the kernel would produce a different result -- i.e. the test has teeth). - cur_vel_backend = (torch.arange(1, num_joints + 1, dtype=torch.float32, device=device) * 0.1).reshape(1, -1) - prev_vel_backend = (torch.arange(1, num_joints + 1, dtype=torch.float32, device=device) * -0.03).reshape(1, -1) - - # Push the current velocity into the backend DOF_VELOCITY binding (backend order). - articulation.root_view.set_attribute(TT.DOF_VELOCITY, wp.from_torch(cur_vel_backend.contiguous())) - - # ``_previous_joint_vel`` is stored in PUBLIC order: prev_user[u] = prev_backend[map[u]]. - prev_vel_user = prev_vel_backend[:, user_to_backend].contiguous() - data._previous_joint_vel.assign(wp.from_torch(prev_vel_user)) - - # Force a stale finite-difference state with a known dt so the ordered branch recomputes, and a - # stale backend velocity staging so it re-reads the value we just set. - dt = 0.02 - data._joint_acc.timestamp = data._sim_timestamp - dt - data._joint_vel_backend.timestamp = -1.0 - - joint_acc_user = data.joint_acc.torch.clone() + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + assert articulation.data.root_com_lin_vel_w.torch[1, 0] > initial_velocity[1, 0] + torch.testing.assert_close(articulation.data.root_com_lin_vel_w.torch[0], initial_velocity[0], atol=1e-6, rtol=0) - # Expected identity-order acceleration, then permuted into public order. - acc_backend = (cur_vel_backend - prev_vel_backend) / dt - expected_user = acc_backend[:, user_to_backend] - torch.testing.assert_close(joint_acc_user, expected_user, rtol=1e-5, atol=1e-6) - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_branching_fixture_physx_ordering_is_identity_on_ovphysx(sim, device): - """Take the same-backend identity fast path for ``joint_ordering="physx"`` on OVPhysX. - - Live coverage for the same-backend symbolic-convention path: OVPhysX is a - PhysX-family backend whose articulation view is already in PhysX (breadth-first) order, so - requesting ``physx`` must expose the public joint/body axes verbatim in backend order with no - reorder map. - """ - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg( - usd_path=str(Path(__file__).parent / "data" / "articulation_ordering_branching.usda") - ), - actuators={}, - joint_ordering="physx", - body_ordering="physx", - ) - ) - sim.reset() - assert articulation.is_initialized - - # OVPhysX exposes the native breadth-first PhysX order on the backend axis. - assert tuple(articulation.backend_joint_names) == BRANCHING_PHYSX_JOINT_NAMES - assert tuple(articulation.backend_body_names) == BRANCHING_PHYSX_BODY_NAMES - - # Same-backend preset: the public axis equals the backend axis and no reorder map is created. - assert tuple(articulation.joint_names) == tuple(articulation.backend_joint_names) - assert tuple(articulation.body_names) == tuple(articulation.backend_body_names) - assert tuple(articulation.joint_names) == BRANCHING_PHYSX_JOINT_NAMES - assert tuple(articulation.body_names) == BRANCHING_PHYSX_BODY_NAMES - assert articulation.joint_ordering is None - assert articulation.body_ordering is None - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_branching_fixture_mjwarp_ordering_reorders_ovphysx_to_dfs(sim, device): - """Resolve depth-first MJWarp order cross-backend for ``joint_ordering="mjwarp"`` on OVPhysX. - - Live coverage for the cross-backend symbolic-convention path: OVPhysX is a - PhysX-family backend (native breadth-first order), so requesting ``mjwarp`` triggers a temporary - Newton USD discovery of the depth-first order and reorders the public joint/body axes to it. The - MJWarp/DFS ground truth is the same tuple isaaclab_newton's - ``test_mjwarp_ordering_resolver_matches_newton_backend_names`` pins for its live Newton backend. - """ - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg( - usd_path=str(Path(__file__).parent / "data" / "articulation_ordering_branching.usda") - ), - actuators={}, - joint_ordering="mjwarp", - body_ordering="mjwarp", - ) - ) - sim.reset() +def test_spatial_tendon_properties_round_trip(articulation_scene: _ArticulationScene) -> None: + """Prove a locally authored spatial tendon is discovered and writable.""" + articulation = articulation_scene.tendon + device = articulation_scene.device assert articulation.is_initialized + assert articulation.is_fixed_base + assert articulation.num_instances == _NUM_ENVS + assert articulation.num_spatial_tendons == 1 + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + initial_stiffness = articulation.data.spatial_tendon_stiffness.torch.clone() + stiffness = torch.tensor([[12.0]], device=device) + damping = torch.tensor([[1.5]], device=device) + limit_stiffness = torch.tensor([[3.0]], device=device) + offset = torch.tensor([[0.1]], device=device) + articulation.set_spatial_tendon_stiffness_index(stiffness=stiffness, env_ids=env_ids) + articulation.set_spatial_tendon_damping_index(damping=damping, env_ids=env_ids) + articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=limit_stiffness, env_ids=env_ids) + articulation.set_spatial_tendon_offset_index(offset=offset, env_ids=env_ids) + torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch[env_ids], stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch[:1], initial_stiffness[:1]) + torch.testing.assert_close(articulation.data.spatial_tendon_damping.torch[env_ids], damping) + torch.testing.assert_close(articulation.data.spatial_tendon_limit_stiffness.torch[env_ids], limit_stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_offset.torch[env_ids], offset) + + +def test_native_actuator_submits_real_effort(articulation_scene: _ArticulationScene) -> None: + """Prove the native controller reaches the real OVPhysX effort binding.""" + articulation = articulation_scene.native + if articulation is None: + pytest.skip("Native actuator wheel probe requires CUDA") + assert articulation.num_instances == _NUM_ENVS + assert articulation._actuator_control.native_actuator_path_active + assert articulation.newton_actuator_adapter is not None + target = articulation.data.joint_pos.torch.clone() + 0.2 + initial_position = articulation.data.joint_pos.torch.clone() + articulation.actuators.target_command.set_position_index(value=target) + articulation.write_data_to_sim() - # OVPhysX exposes the native breadth-first PhysX order on the backend axis. - assert tuple(articulation.backend_joint_names) == BRANCHING_PHYSX_JOINT_NAMES - assert tuple(articulation.backend_body_names) == BRANCHING_PHYSX_BODY_NAMES - - # Cross-backend Newton discovery resolves the depth-first MJWarp order and reorders the public axis. - assert get_articulation_name_ordering(articulation, "mjwarp", kind="joint") == BRANCHING_MJWARP_JOINT_NAMES - assert get_articulation_name_ordering(articulation, "mjwarp", kind="body") == BRANCHING_MJWARP_BODY_NAMES - assert tuple(articulation.joint_names) == BRANCHING_MJWARP_JOINT_NAMES - assert tuple(articulation.body_names) == BRANCHING_MJWARP_BODY_NAMES - assert articulation.joint_ordering is not None - assert articulation.body_ordering is not None - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_articulation_dynamics_fixed_base_match_raw_ovphysx_bindings(sim, device): - """Expose fixed-base computed dynamics through the backend-agnostic data API.""" - articulation, _ = generate_articulation(generate_articulation_cfg("panda"), 1, device=device) - sim.reset() - - num_generalized_dofs = articulation.num_joints - num_jacobian_bodies = articulation.num_bodies - 1 - raw_jacobian = _read_binding_to_torch(articulation, TT.JACOBIAN, device).reshape( - 1, num_jacobian_bodies, 6, num_generalized_dofs - ) - raw_mass_matrix = _read_binding_to_torch(articulation, TT.MASS_MATRIX, device) - raw_gravity = _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device) - - body_com_jacobian = articulation.data.body_com_jacobian_w - mass_matrix = articulation.data.mass_matrix - gravity = articulation.data.gravity_compensation_forces - assert body_com_jacobian is articulation.data.body_com_jacobian_w - assert mass_matrix is articulation.data.mass_matrix - assert gravity is articulation.data.gravity_compensation_forces - - torch.testing.assert_close(body_com_jacobian.torch, raw_jacobian) - torch.testing.assert_close(mass_matrix.torch, raw_mass_matrix) - torch.testing.assert_close(gravity.torch, raw_gravity) - assert articulation.data.body_link_jacobian_w.torch.shape == raw_jacobian.shape - assert torch.isfinite(articulation.data.body_link_jacobian_w.torch).all() - torch.testing.assert_close(mass_matrix.torch, mass_matrix.torch.transpose(-1, -2), rtol=1e-5, atol=1e-5) - - joint_position = articulation.data.joint_pos.torch.clone() - joint_position[:, 1] += 0.2 - articulation.write_joint_position_to_sim_index(position=joint_position) - updated_raw_jacobian = _read_binding_to_torch(articulation, TT.JACOBIAN, device).reshape( - 1, num_jacobian_bodies, 6, num_generalized_dofs - ) - updated_raw_mass_matrix = _read_binding_to_torch(articulation, TT.MASS_MATRIX, device) - updated_raw_gravity = _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device) - assert not torch.allclose(updated_raw_jacobian, raw_jacobian) - assert not torch.allclose(updated_raw_mass_matrix, raw_mass_matrix) - torch.testing.assert_close(articulation.data.body_com_jacobian_w.torch, updated_raw_jacobian) - torch.testing.assert_close(articulation.data.mass_matrix.torch, updated_raw_mass_matrix) - torch.testing.assert_close(articulation.data.gravity_compensation_forces.torch, updated_raw_gravity) - - joint_velocity = torch.linspace(-0.2, 0.2, articulation.num_joints, dtype=torch.float32, device=device).unsqueeze(0) - articulation.write_joint_velocity_to_sim_index(velocity=joint_velocity) - expected_com_velocity = torch.einsum("nbij,nj->nbi", articulation.data.body_com_jacobian_w.torch, joint_velocity) - expected_link_velocity = torch.einsum("nbij,nj->nbi", articulation.data.body_link_jacobian_w.torch, joint_velocity) - torch.testing.assert_close( - expected_com_velocity, articulation.data.body_com_vel_w.torch[:, 1:], atol=1e-5, rtol=1e-4 - ) - torch.testing.assert_close( - expected_link_velocity, articulation.data.body_link_vel_w.torch[:, 1:], atol=1e-5, rtol=1e-4 - ) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_articulation_dynamics_refresh_after_same_timestamp_model_writes(sim, device): - """Refresh computed dynamics after model-property writes without advancing simulation time.""" - articulation, _ = generate_articulation(generate_articulation_cfg("panda"), 1, device=device) - sim.reset() - - data = articulation.data - num_jacobian_bodies = articulation.num_bodies - 1 - num_generalized_dofs = articulation.num_joints - - # COM offsets affect COM Jacobians, mass matrices, and gravity forces. - data.body_com_jacobian_w - data.mass_matrix - data.gravity_compensation_forces - coms = _read_binding_to_torch(articulation, TT.BODY_COM_POSE, device) - coms[:, -1, 0] += 0.01 - articulation.set_coms_index(coms=wp.from_torch(coms.contiguous(), dtype=wp.transformf)) - assert data._body_com_jacobian_w.timestamp < data._sim_timestamp - assert data._mass_matrix.timestamp < data._sim_timestamp - assert data._gravity_compensation_forces.timestamp < data._sim_timestamp - raw_jacobian = _read_binding_to_torch(articulation, TT.JACOBIAN, device).reshape( - 1, num_jacobian_bodies, 6, num_generalized_dofs - ) - torch.testing.assert_close(data.body_com_jacobian_w.torch, raw_jacobian) - torch.testing.assert_close(data.mass_matrix.torch, _read_binding_to_torch(articulation, TT.MASS_MATRIX, device)) - torch.testing.assert_close( - data.gravity_compensation_forces.torch, - _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device), - ) + raw_effort = wp.to_torch(articulation._physx_actuator_wrapper.joint_f_2d).clone() + backend_effort = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_ACTUATION_FORCE)) + backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) + assert torch.any(raw_effort != 0.0) + torch.testing.assert_close(backend_effort, raw_effort[:, backend_to_user]) - # Mass affects the mass matrix and gravity forces, but not the kinematic Jacobian. - masses = data.body_mass.torch.clone() - masses[:, -1] *= 1.1 - articulation.set_masses_index(masses=masses) - assert data._mass_matrix.timestamp < data._sim_timestamp - assert data._gravity_compensation_forces.timestamp < data._sim_timestamp - torch.testing.assert_close(data.mass_matrix.torch, _read_binding_to_torch(articulation, TT.MASS_MATRIX, device)) + for _ in range(8): + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + articulation.write_data_to_sim() + assert torch.any(articulation.data.joint_pos.torch != initial_position) + recomputed_effort = wp.to_torch(articulation._physx_actuator_wrapper.joint_f_2d) + assert torch.any(recomputed_effort != raw_effort) torch.testing.assert_close( - data.gravity_compensation_forces.torch, - _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device), + wp.to_torch(articulation.root_view.get_attribute(TT.DOF_ACTUATION_FORCE)), + recomputed_effort[:, backend_to_user], ) - - # Inertia and armature each affect only the generalized mass matrix. - inertias = data.body_inertia.torch.clone() - inertias[:, -1, [0, 4, 8]] *= 1.1 - articulation.set_inertias_index(inertias=inertias) - assert data._mass_matrix.timestamp < data._sim_timestamp - torch.testing.assert_close(data.mass_matrix.torch, _read_binding_to_torch(articulation, TT.MASS_MATRIX, device)) - - armature = data.joint_armature.torch.clone() - armature[:, -1] += 0.01 - articulation.write_joint_armature_to_sim_index(armature=armature) - assert data._mass_matrix.timestamp < data._sim_timestamp - torch.testing.assert_close(data.mass_matrix.torch, _read_binding_to_torch(articulation, TT.MASS_MATRIX, device)) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_articulation_dynamics_reorder_body_rows_and_joint_axes(sim, device): - """Gather computed dynamics into MJWarp body and joint order.""" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg( - usd_path=str(Path(__file__).parent / "data" / "articulation_ordering_branching.usda") - ), - actuators={}, - joint_ordering="mjwarp", - body_ordering="mjwarp", - ) - ) - sim.reset() - - joint_ordering = articulation.joint_ordering - body_ordering = articulation.body_ordering - assert joint_ordering is not None - assert body_ordering is not None - joint_user_to_backend = torch.as_tensor(joint_ordering.user_to_backend_indices, dtype=torch.long, device=device) - body_offset = 1 if articulation.is_fixed_base else 0 - body_user_to_backend = torch.as_tensor( - [ - backend_body_id - body_offset - for backend_body_id in body_ordering.user_to_backend_indices - if not body_offset or backend_body_id != 0 - ], - dtype=torch.long, - device=device, - ) - generalized_user_to_backend = torch.cat( - ( - torch.arange(articulation.num_base_dofs, device=device), - articulation.num_base_dofs + joint_user_to_backend, - ) - ) - - raw_jacobian = _read_binding_to_torch(articulation, TT.JACOBIAN, device).reshape( - 1, - articulation.num_bodies - body_offset, - 6, - articulation.num_joints + articulation.num_base_dofs, - ) - raw_mass_matrix = _read_binding_to_torch(articulation, TT.MASS_MATRIX, device) - raw_gravity = _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device) - - expected_jacobian = raw_jacobian[:, body_user_to_backend, :, :][:, :, :, generalized_user_to_backend] - expected_mass_matrix = raw_mass_matrix[:, generalized_user_to_backend, :][:, :, generalized_user_to_backend] - expected_gravity = raw_gravity[:, generalized_user_to_backend] - torch.testing.assert_close(articulation.data.body_com_jacobian_w.torch, expected_jacobian) - torch.testing.assert_close(articulation.data.mass_matrix.torch, expected_mass_matrix) - torch.testing.assert_close(articulation.data.gravity_compensation_forces.torch, expected_gravity) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_articulation_dynamics_preserve_floating_base_columns_during_joint_reordering(sim, device): - """Keep floating-base columns leading while gathering actuated-joint axes.""" - articulation_cfg = generate_articulation_cfg("anymal").replace( - joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES)) - ) - articulation, _ = generate_articulation(articulation_cfg, 1, device=device) - sim.reset() - - joint_ordering = articulation.joint_ordering - assert joint_ordering is not None - joint_user_to_backend = torch.as_tensor(joint_ordering.user_to_backend_indices, dtype=torch.long, device=device) - generalized_user_to_backend = torch.cat( - ( - torch.arange(6, device=device), - 6 + joint_user_to_backend, - ) - ) - raw_jacobian = _read_binding_to_torch(articulation, TT.JACOBIAN, device).reshape( - 1, articulation.num_bodies, 6, articulation.num_joints + 6 - ) - raw_mass_matrix = _read_binding_to_torch(articulation, TT.MASS_MATRIX, device) - raw_gravity = _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device) - - torch.testing.assert_close( - articulation.data.body_com_jacobian_w.torch, - raw_jacobian[:, :, :, generalized_user_to_backend], - ) - torch.testing.assert_close( - articulation.data.mass_matrix.torch, - raw_mass_matrix[:, generalized_user_to_backend, :][:, :, generalized_user_to_backend], - ) - torch.testing.assert_close( - articulation.data.gravity_compensation_forces.torch, - raw_gravity[:, generalized_user_to_backend], - ) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_floating_base_non_root(sim, num_articulations, device, add_ground_plane): - """Test initialization for a floating-base with articulation root on a rigid body. - - 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="humanoid", stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - - # Check if articulation is initialized - assert articulation.is_initialized - # Check that is fixed 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, 21) - - # Cross-check binding shapes against cached counts. PhysX does this via - # ``root_view.max_dofs == shared_metatype.dof_count``; on OVPhysX - # ``root_view`` is an ``OvPhysxView`` over the per-tensor-type bindings, so the equivalent - # invariant is that each per-DOF / per-link binding's shape agrees with - # the count cached on the asset. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # Body-name ordering check is degenerate on OVPhysX: ``body_names`` is - # sourced from binding metadata (``sample.body_names``), so the PhysX - # ``link_paths[0]`` round-trip is a no-op here and is omitted. - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == is_implicit_model_cfg - assert actuator.joint_indices == slice(None) - - # 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("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_floating_base(sim, num_articulations, device, add_ground_plane): - """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="anymal", stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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) - - # Cross-check binding shapes against cached counts. PhysX does this via - # ``root_view.max_dofs == shared_metatype.dof_count``; on OVPhysX - # ``root_view`` is an ``OvPhysxView`` over the per-tensor-type bindings, so the equivalent - # invariant is that each per-DOF / per-link binding's shape agrees with - # the count cached on the asset. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # Body-name ordering check is degenerate on OVPhysX: ``body_names`` is - # sourced from binding metadata (``sample.body_names``), so the PhysX - # ``link_paths[0]`` round-trip is a no-op here and is omitted. - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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("device", test_devices()) -def test_initialization_fixed_base(sim, num_articulations, device): - """Test initialization for fixed base. - - 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="panda") - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 9) - 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) - - # Cross-check binding shapes against cached counts. PhysX does this via - # ``root_view.max_dofs == shared_metatype.dof_count``; on OVPhysX - # ``root_view`` is an ``OvPhysxView`` over the per-tensor-type bindings, so the equivalent - # invariant is that each per-DOF / per-link binding's shape agrees with - # the count cached on the asset. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # Body-name ordering check is degenerate on OVPhysX: ``body_names`` is - # sourced from binding metadata (``sample.body_names``), so the PhysX - # ``link_paths[0]`` round-trip is a no-op here and is omitted. - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == is_implicit_model_cfg - assert isinstance(actuator.joint_indices, torch.Tensor) - assert actuator.joint_indices.dtype == torch.int32 - assert actuator.joint_indices.device == torch.device(device) - - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_fixed_base_single_joint(sim, num_articulations, device, add_ground_plane): - """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="single_joint_implicit") - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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) - - # Cross-check binding shapes against cached counts. PhysX does this via - # ``root_view.max_dofs == shared_metatype.dof_count``; on OVPhysX - # ``root_view`` is an ``OvPhysxView`` over the per-tensor-type bindings, so the equivalent - # invariant is that each per-DOF / per-link binding's shape agrees with - # the count cached on the asset. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # Body-name ordering check is degenerate on OVPhysX: ``body_names`` is - # sourced from binding metadata (``sample.body_names``), so the PhysX - # ``link_paths[0]`` round-trip is a no-op here and is omitted. - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_hand_with_tendons(sim, num_articulations, device): - """Test initialization for fixed base articulated hand with tendons. - - 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 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="shadow_hand") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 24) - 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) - - # Cross-check binding shapes against cached counts. See the equivalent - # block in test_initialization_fixed_base_single_joint for why the verbatim - # PhysX ``root_view.max_dofs == shared_metatype.dof_count`` identity is - # replaced with binding-shape checks on OVPhysX. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.xfail(reason=_OMNI_PHYSX_SCHEMAS_GAP_REASON, strict=False) -def test_initialization_floating_base_made_fixed_base(sim, num_articulations, device, add_ground_plane): - """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="anymal").copy() - # Fix root link by making it kinematic - articulation_cfg.spawn.articulation_props.fix_root_link = True - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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) - - # Cross-check binding shapes against cached counts. PhysX does this via - # ``root_view.max_dofs == shared_metatype.dof_count``; on OVPhysX - # ``root_view`` is an ``OvPhysxView`` over the per-tensor-type bindings, so the equivalent - # invariant is that each per-DOF / per-link binding's shape agrees with - # the count cached on the asset. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # Body-name ordering check is degenerate on OVPhysX: ``body_names`` is - # sourced from binding metadata (``sample.body_names``), so the PhysX - # ``link_paths[0]`` round-trip is a no-op here and is omitted. - - # 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("device", ["cpu"]) -def test_fragment_fix_root_reenables_existing_joint(sim, device): - """The fragment path must normalize OVPhysX topology even when a disabled fixed joint exists.""" - articulation_cfg = generate_articulation_cfg(articulation_type="anymal").copy() - articulation_cfg.spawn.articulation_props = [] - articulation_cfg.spawn.fix_root_link = None - articulation, _ = generate_articulation(articulation_cfg, num_articulations=1, device=device) - - stage = sim.stage - asset_path = "/World/Env_0/Robot" - root = sim_utils.get_first_matching_child_prim( - asset_path, lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), stage=stage - ) - assert root is not None and root.HasAPI(UsdPhysics.RigidBodyAPI) - old_root_path = root.GetPath().pathString - final_root = root.GetParent() - - joint = UsdPhysics.FixedJoint.Define(stage, f"{old_root_path}/PreAuthoredFixedJoint") - joint.CreateBody1Rel().SetTargets([root.GetPath()]) - joint.CreateJointEnabledAttr().Set(False) - - assert sim_utils.apply_articulation_root_properties(asset_path, [], stage, fix_root_link=True) - assert joint.GetJointEnabledAttr().Get() is True - world_joints = [] - for prim in sim_utils.get_all_matching_child_prims( - asset_path, lambda prim: prim.IsA(UsdPhysics.FixedJoint), stage=stage - ): - usd_joint = UsdPhysics.Joint(prim) - has_body_0 = bool(usd_joint.GetBody0Rel().GetTargets()) - has_body_1 = bool(usd_joint.GetBody1Rel().GetTargets()) - if has_body_0 != has_body_1: - world_joints.append(prim) - assert world_joints == [joint.GetPrim()] - assert final_root.HasAPI(UsdPhysics.ArticulationRootAPI) - assert not root.HasAPI(UsdPhysics.ArticulationRootAPI) - - sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_fixed_base_made_floating_base(sim, num_articulations, device, add_ground_plane): - """Test initialization for fixed base made floating-base using schema properties. - - This test verifies that: - 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 - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - # Unfix root link by making it non-kinematic - articulation_cfg.spawn.articulation_props.fix_root_link = False - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # 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) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 9) - - # Cross-check binding shapes against cached counts. PhysX does this via - # ``root_view.max_dofs == shared_metatype.dof_count``; on OVPhysX - # ``root_view`` is an ``OvPhysxView`` over the per-tensor-type bindings, so the equivalent - # invariant is that each per-DOF / per-link binding's shape agrees with - # the count cached on the asset. - for tt in (TT.DOF_POSITION, TT.DOF_VELOCITY, TT.DOF_STIFFNESS): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_joints - for tt in (TT.BODY_MASS, TT.BODY_COM_POSE): - binding = articulation.root_view.try_binding_for(tt) - if binding is not None: - assert binding.shape[1] == articulation.num_bodies - # Body-name ordering check is degenerate on OVPhysX: ``body_names`` is - # sourced from binding metadata (``sample.body_names``), so the PhysX - # ``link_paths[0]`` round-trip is a no-op here and is omitted. - - # 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("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_ground_plane): - """Test that the default joint position from configuration is out of range. - - This test verifies that: - 1. The articulation fails to initialize when joint positions are out of range - 2. The error is properly handled - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="panda").copy() - articulation_cfg.init_state.joint_pos = { - "panda_joint1": 10.0, - "panda_joint[2, 4]": -20.0, - } - - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(ValueError): - sim.reset() - - -@pytest.mark.parametrize("device", test_devices()) -def test_out_of_range_default_joint_vel(sim, device): - """Test that the default joint velocity from configuration is out of range. - - This test verifies that: - 1. The articulation fails to initialize when joint velocities are out of range - 2. The error is properly handled - """ - articulation_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot") - articulation_cfg.init_state.joint_vel = { - "panda_joint1": 100.0, - "panda_joint[2, 4]": -60.0, - } - articulation = Articulation(articulation_cfg) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(ValueError): - sim.reset() - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): - """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="panda") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Play sim - 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_joint_effort_limits(sim, num_articulations, device, add_ground_plane): - """Validate joint effort limits via joint_effort_out_of_limit().""" - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Minimal env wrapper exposing scene["robot"] - class _Env: - def __init__(self, art): - self.scene = {"robot": art} - - env = _Env(articulation) - robot_all = SceneEntityCfg(name="robot") - - 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(sim, num_articulations, device): - """Test if external force buffer correctly updates in the force value is zero case. - - This test verifies that: - 1. External forces can be applied correctly - 2. Force buffers are updated properly - 3. Zero forces are handled correctly - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # play the simulator - 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) - - # reset articulation - articulation.reset() - - # 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.composed_force.torch[i, 0, 0].item() == force - assert articulation.permanent_wrench_composer.composed_torque.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, - ) - - # 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) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body(sim, num_articulations, device): - """Test application of external force on the base of the articulation. - - 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 - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # Play the simulator - 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[..., 1] = 1000.0 - - # Now we are ready! - for _ in range(5): - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(sim, num_articulations, device): - """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="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # Play the simulator - 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] = 500.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] = 1000.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 1000.0 - - # Now we are ready! - for i in range(5): - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_multiple_bodies(sim, num_articulations, device): - """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="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - 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] = 100.0 - - # Now we are ready! - for _ in range(5): - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device): - """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="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - 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] = 500.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] = 1000.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 1000.0 - - # Now we are ready! - for i in range(5): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - 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() - - 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 - - # apply force - 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 - for i in range(num_articulations): - # since there is a moment applied on the articulation, the articulation should rotate - assert torch.abs(articulation.data.root_ang_vel_w.torch[i, 2]).item() > 0.1 - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_loading_gains_from_usd(sim, num_articulations, device): - """Test that gains are loaded from USD file if actuator model has them as None. - - This test verifies that: - 1. Gains are loaded correctly from USD file - 2. Default gains are applied when not specified - 3. The gains match the expected values - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid", stiffness=None, damping=None) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play sim - sim.reset() - - # Expected gains - # -- Stiffness values - expected_stiffness = { - ".*_waist.*": 20.0, - ".*_upper_arm.*": 10.0, - "pelvis": 10.0, - ".*_lower_arm": 2.0, - ".*_thigh:0": 10.0, - ".*_thigh:1": 20.0, - ".*_thigh:2": 10.0, - ".*_shin": 5.0, - ".*_foot.*": 2.0, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_stiffness, articulation.joint_names - ) - expected_stiffness = torch.zeros(articulation.num_instances, articulation.num_joints, device=articulation.device) - expected_stiffness[:, indices_list] = torch.tensor(values_list, device=articulation.device) - # -- Damping values - expected_damping = { - ".*_waist.*": 5.0, - ".*_upper_arm.*": 5.0, - "pelvis": 5.0, - ".*_lower_arm": 1.0, - ".*_thigh:0": 5.0, - ".*_thigh:1": 5.0, - ".*_thigh:2": 5.0, - ".*_shin": 0.1, - ".*_foot.*": 1.0, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_damping, articulation.joint_names - ) - expected_damping = torch.zeros_like(expected_stiffness) - expected_damping[:, indices_list] = torch.tensor(values_list, device=articulation.device) - - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane): - """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="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device - ) - - # Play sim - 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_setting_gains_from_cfg_dict(sim, num_articulations, device): - """Test that gains are loaded from the configuration dictionary correctly. - - This test verifies that: - 1. Gains are loaded correctly from configuration dictionary - 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="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device - ) - # Play sim - 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("device", test_devices()) -@pytest.mark.parametrize("joint_velocity_limit", [1e5, None]) -def test_setting_velocity_limit_writes_to_solver(sim, device, joint_velocity_limit): - """Test that the resolved joint velocity limit reaches the PhysX tensor-API solver. - - The full limit-resolution matrix (config override vs. USD default, implicit and explicit - actuators, actuator-limit soft fallback) is covered on the Newton backend and at unit - level. This smoke test only verifies the PhysX tensor-API write path: the configured limit (or the - USD-authored default when unset) lands in the native solver buffers and matches - ``data.joint_vel_limits``. - """ - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - joint_velocity_limit=joint_velocity_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=1, - device=device, - ) - # Play sim - sim.reset() - - # read the values set into the simulation - physx_vel_limit = _read_binding_to_torch(articulation, TT.DOF_MAX_VELOCITY, device) - # check data buffer - torch.testing.assert_close(articulation.data.joint_vel_limits.torch, physx_vel_limit) - # the solver clamp comes from joint_velocity_limit when set, otherwise the USD-authored value - if joint_velocity_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_joint_velocity - else: - limit = joint_velocity_limit - expected_velocity_limit = torch.full_like(physx_vel_limit, limit) - torch.testing.assert_close(physx_vel_limit, expected_velocity_limit) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("joint_effort_limit", [1e5, None]) -def test_setting_effort_limit_writes_to_solver(sim, device, joint_effort_limit): - """Test that the resolved joint effort limit reaches the PhysX tensor-API solver. - - The full limit-resolution matrix (config override vs. USD default, implicit and explicit - actuators, actuator-limit soft fallback) is covered on the Newton backend and at unit - level. This smoke test only verifies the PhysX tensor-API write path: the configured limit (or the - USD-authored default when unset) lands in the native solver buffers and matches - ``data.joint_effort_limits``. - """ - 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=1, - device=device, - ) - # Play sim - sim.reset() - - # obtain the physx effort limits - physx_effort_limit = _read_binding_to_torch(articulation, TT.DOF_MAX_FORCE, device) - # check data buffer - torch.testing.assert_close(articulation.data.joint_effort_limits.torch, physx_effort_limit) - # the solver keeps the USD-authored limit unless the user overrides it explicitly - if joint_effort_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_force - else: - limit = joint_effort_limit - expected_effort_limit = torch.full_like(physx_effort_limit, limit) - torch.testing.assert_close(physx_effort_limit, expected_effort_limit) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset(sim, num_articulations, device): - """Test that reset method works properly.""" - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Now we are ready! - # reset articulation - articulation.reset() - - # 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.composed_force.torch) == 0 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.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.composed_force.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == num_bodies * 3 - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_write_root_velocity_invalidates_body_frame_cache(sim, num_articulations, device): - """Writing root velocity refreshes cached body-frame root velocities before a step.""" - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - sim.reset() - for _ in range(3): - sim.step() - articulation.update(sim.cfg.dt) - ang_before = articulation.data.root_ang_vel_b.torch.clone() - - new_vel = torch.zeros(num_articulations, 6, device=device) - new_vel[:, :3] = torch.tensor([3.0, 0.0, 0.0], device=device) - new_vel[:, 3:] = torch.tensor([0.0, 0.0, 5.0], device=device) - articulation.write_root_velocity_to_sim_index( - root_velocity=wp.from_torch(new_vel.contiguous(), dtype=wp.spatial_vectorf) - ) - - ang_after = articulation.data.root_ang_vel_b.torch - torch.testing.assert_close( - ang_after.norm(dim=-1), - torch.full((num_articulations,), 5.0, device=device), - atol=1e-3, - rtol=1e-3, - ) - assert not torch.allclose(ang_after, ang_before) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): - """Test applying of joint position target functions correctly for a robotic arm.""" - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - 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) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -def test_body_root_state(sim, num_articulations, device, with_offset): - """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 - - 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="single_joint_implicit") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - env_idx = torch.tensor([x for x in range(num_articulations)], device=device, dtype=torch.int32) - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10, "Possible reference leak for articulation" - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized, "Articulation is not initialized" - # Check that fixed base - assert articulation.is_fixed_base, "Articulation is not a fixed base" - - # 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] - - # create com offsets — apply offset to the Arm body - num_bodies = articulation.num_bodies - com = _read_binding_to_torch(articulation, TT.BODY_COM_POSE, device) - link_offset = [1.0, 0.0, 0.0] # the offset from CenterPivot to Arm frames - new_com = torch.tensor(offset, device=device).repeat(num_articulations, 1, 1) - com[:, arm_idx, :3] = new_com.squeeze(-2) - # PhysX uses ``root_view.set_coms``; OVPhysX wraps the wheel - # ``BODY_COM_POSE`` write in :meth:`set_coms_index` (wp.transformf contract). - articulation.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), - ) - - # check they are set - torch.testing.assert_close(_read_binding_to_torch(articulation, TT.BODY_COM_POSE, device), com) - - for i in range(50): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # 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 - body_link_pose_w = articulation.data.body_link_pose_w.torch - body_link_vel_w = articulation.data.body_link_vel_w.torch - 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 - # OVStage determines the runtime link pose from the joint frames, which may differ - # from the authored USD Xform. Verify the COM offset relative to that runtime pose. - pos_gt = body_link_pose_w[..., :3].clone() - px = offset[0] * torch.cos(joint_pos) - py = torch.zeros(num_articulations, 1, 1, device=device) - pz = offset[0] * torch.sin(joint_pos) - pos_gt[:, arm_idx, :] += torch.cat([px, py, pz], dim=-1).squeeze(-2) - 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", [1, 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]) -def test_write_root_state(sim, num_articulations, device, with_offset, state_location, gravity_enabled): - """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="anymal") - 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 - sim.reset() - - # 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 = _read_binding_to_torch(articulation, TT.BODY_COM_POSE, device) - new_com = offset.to(device) - com[:, 0, :3] = new_com.squeeze(-2) - # See test_body_root_state for the PhysX → OVPhysX setter substitution. - articulation.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), - ) - - # check they are set - torch.testing.assert_close(_read_binding_to_torch(articulation, TT.BODY_COM_POSE, device), 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) - - -@pytest.mark.parametrize("device", ["cpu"]) -def test_body_com_pose_b_cache_and_set_coms_invalidation(sim, device): - """Body-frame COM offsets stay cached and invalidate derived buffers after writes.""" - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") - articulation, _ = generate_articulation(articulation_cfg, 2, device=device) - - sim.reset() - articulation.update(sim.cfg.dt) - - articulation.data.body_com_pose_b - first_timestamp = articulation.data._body_com_pose_b.timestamp - articulation.update(sim.cfg.dt) - articulation.data.body_com_pose_b - assert articulation.data._body_com_pose_b.timestamp == first_timestamp - - dependent_buffers = [ - ("root_com_pose_w", articulation.data._root_com_pose_w), - ("root_com_vel_w", articulation.data._root_com_vel_w), - ("root_link_vel_w", articulation.data._root_link_vel_w), - ("body_com_pose_w", articulation.data._body_com_pose_w), - ("body_com_vel_w", articulation.data._body_com_vel_w), - ("body_link_vel_w", articulation.data._body_link_vel_w), - ("root_link_lin_vel_b", articulation.data._root_link_lin_vel_b), - ("root_link_ang_vel_b", articulation.data._root_link_ang_vel_b), - ("root_com_lin_vel_b", articulation.data._root_com_lin_vel_b), - ("root_com_ang_vel_b", articulation.data._root_com_ang_vel_b), - ("root_state_w", articulation.data._root_state_w_buf), - ("root_link_state_w", articulation.data._root_link_state_w_buf), - ("root_com_state_w", articulation.data._root_com_state_w_buf), - ("body_state_w", articulation.data._body_state_w_buf), - ("body_link_state_w", articulation.data._body_link_state_w_buf), - ("body_com_state_w", articulation.data._body_com_state_w_buf), - ("body_com_jacobian_w", articulation.data._body_com_jacobian_w), - ("mass_matrix", articulation.data._mass_matrix), - ("gravity_compensation_forces", articulation.data._gravity_compensation_forces), - ] - for _, buffer in dependent_buffers: - buffer.timestamp = articulation.data._sim_timestamp - - coms = wp.zeros((articulation.num_instances, articulation.num_bodies), dtype=wp.transformf, device=device) - articulation.set_coms_index(coms=coms) - - assert articulation.data._body_com_pose_b.timestamp >= 0.0 - for name, buffer in dependent_buffers: - assert buffer.timestamp < articulation.data._sim_timestamp, name - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_root_link_vel_w_refreshes_fk_before_body_com_vel_w_read(sim, device): - """Reading ``root_link_vel_w`` must run FK before ``body_com_vel_w`` sees a "fresh" buffer. - - Regression test for a bug where ``root_link_vel_w`` read the ``LINK_VELOCITY`` binding without - first calling ``_ensure_fk_fresh()``, unlike the sibling ``body_com_vel_w`` / ``body_link_pose_w`` - getters. ``_read_binding_into_buf`` stamps a buffer's timestamp as fresh unconditionally, so a - ``root_link_vel_w`` read performed right after ``write_joint_velocity_to_sim_index`` (which sets - ``_fk_timestamp = -1.0`` to force a refresh) would mark the shared velocity buffer fresh *before* - FK actually ran. A subsequent ``body_com_vel_w`` read then sees the buffer already fresh and skips - its own re-read, silently returning pre-FK data. - - The OVPhysX kitless backend recomputes ``LINK_VELOCITY`` eagerly on every attribute read - regardless of whether ``update_articulations_kinematic`` was called, so comparing the numeric - value of ``body_com_vel_w`` before and after the fix would pass either way here. The invariant - that actually catches the bug is that ``_fk_timestamp`` must be current by the time - ``root_link_vel_w`` finishes reading, so every dependent buffer it marks fresh is trustworthy. - """ - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") - articulation, _ = generate_articulation(articulation_cfg, 2, device=device) - - sim.reset() - articulation.update(sim.cfg.dt) - - # Prime the derived buffers before the write so their TimestampedBuffers are populated; otherwise - # the reads below would trivially be "first reads" regardless of the cache-invalidation bug. - articulation.data.root_link_vel_w - articulation.data.body_com_vel_w - - joint_vel = torch.full((2, articulation.num_joints), 3.0, device=device) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - - # The velocity write forces a kinematic refresh on the next FK-dependent read. - assert articulation.data._fk_timestamp < 0.0 - - articulation.data.root_link_vel_w - # `root_link_vel_w` must have triggered the FK refresh itself -- it cannot rely on a later - # `body_com_vel_w` read to do so, because it already marks the shared velocity buffer fresh. - assert articulation.data._fk_timestamp == articulation.data._sim_timestamp - - body_com_vel_w = articulation.data.body_com_vel_w.torch - assert torch.linalg.norm(body_com_vel_w[:, 1, :]) > 1e-3 - - -@pytest.mark.parametrize("device", test_devices()) -def test_setting_articulation_root_prim_path(sim, device): - """Test that the articulation root prim path can be set explicitly.""" - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation_cfg.articulation_root_prim_path = "/torso" - articulation, _ = generate_articulation(articulation_cfg, 1, device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation._is_initialized - - -@pytest.mark.parametrize("device", test_devices()) -def test_setting_invalid_articulation_root_prim_path(sim, device): - """Test that the articulation root prim path can be set explicitly.""" - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation_cfg.articulation_root_prim_path = "/non_existing_prim_path" - articulation, _ = generate_articulation(articulation_cfg, 1, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.parametrize("device", ["cpu"]) -def test_deprecated_joint_state_writer_delegates_to_index_writers(sim, device, mocker): - """Keep the deprecated combined API as a thin composition of public writers.""" - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, 1, device) - sim.reset() - - position = torch.tensor([[0.1]], device=device) - velocity = torch.tensor([[0.2]], device=device) - position_writer = mocker.patch.object(articulation, "write_joint_position_to_sim_index") - velocity_writer = mocker.patch.object(articulation, "write_joint_velocity_to_sim_index") - - with pytest.warns(DeprecationWarning): - articulation.write_joint_state_to_sim( - position=position, - velocity=velocity, - joint_ids=[0], - env_ids=[0], - ) - - position_writer.assert_called_once_with(position=position, joint_ids=[0], env_ids=[0]) - velocity_writer.assert_called_once_with(velocity=velocity, joint_ids=[0], env_ids=[0]) - - -@pytest.mark.parametrize("device", test_devices()) -def test_write_joint_state_to_sim_index_partial(sim, device): - """Test fused joint-state writes with partial environment and joint indices.""" - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, 2, device) - sim.reset() - - original_joint_pos = articulation.data.joint_pos.torch.clone() - original_joint_vel = articulation.data.joint_vel.torch.clone() - _ = articulation.data.body_link_pose_w - _ = articulation.data.body_com_vel_w - pose_timestamp = articulation.data._body_link_pose_w.timestamp - velocity_timestamp = articulation.data._body_com_vel_w.timestamp - - previous_joint_vel = wp.to_torch(articulation.data._previous_joint_vel) - joint_acc = wp.to_torch(articulation.data._joint_acc.data) - previous_joint_vel.fill_(3.0) - joint_acc.fill_(4.0) - articulation.data._joint_acc.timestamp = -1.0 - - position = torch.tensor([[0.1, -0.1]], device=device) - velocity = torch.tensor([[0.2, -0.2]], device=device) - articulation.write_joint_state_to_sim_index( - position=position, velocity=velocity, env_ids=[1], joint_ids=[0, 2], skip_forward=True - ) - - expected_joint_pos = original_joint_pos.clone() - expected_joint_vel = original_joint_vel.clone() - expected_previous_joint_vel = torch.full_like(previous_joint_vel, 3.0) - expected_joint_acc = torch.full_like(joint_acc, 4.0) - expected_joint_pos[1, [0, 2]] = position[0] - expected_joint_vel[1, [0, 2]] = velocity[0] - expected_previous_joint_vel[1, [0, 2]] = velocity[0] - expected_joint_acc[1, [0, 2]] = 0.0 - - torch.testing.assert_close(articulation.data.joint_pos.torch, expected_joint_pos) - torch.testing.assert_close(articulation.data.joint_vel.torch, expected_joint_vel) - torch.testing.assert_close(previous_joint_vel, expected_previous_joint_vel) - torch.testing.assert_close(joint_acc, expected_joint_acc) - assert articulation.data._joint_acc.timestamp == articulation.data._sim_timestamp - assert articulation.data._body_link_pose_w.timestamp == pose_timestamp - assert articulation.data._body_com_vel_w.timestamp == velocity_timestamp - torch.testing.assert_close(_read_binding_to_torch(articulation, TT.DOF_POSITION, device), expected_joint_pos) - torch.testing.assert_close(_read_binding_to_torch(articulation, TT.DOF_VELOCITY, device), expected_joint_vel) - - articulation.write_joint_state_to_sim_index(position=position, velocity=velocity, env_ids=[1], joint_ids=[0, 2]) - assert articulation.data._body_link_pose_w.timestamp < articulation.data._sim_timestamp - assert articulation.data._body_com_vel_w.timestamp < articulation.data._sim_timestamp - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_write_joint_state_data_consistency(sim, num_articulations, device, gravity_enabled): - """Test the setters for root_state using both the link frame and center of mass as reference frame. - - 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 - device: The device to run the simulation on - """ - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) - env_idx = torch.tensor([x for x in range(num_articulations)]) - - # Play sim - sim.reset() - - 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) - - from torch.distributions import Uniform - - joint_pos_limits = articulation.data.joint_pos_limits.torch - joint_vel_limits = articulation.data.joint_vel_limits.torch - pos_dist = Uniform(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) - vel_dist = Uniform(-joint_vel_limits, joint_vel_limits) - - original_body_link_pose_w = articulation.data.body_link_pose_w.torch.clone() - original_body_com_vel_w = articulation.data.body_com_vel_w.torch.clone() - - rand_joint_pos = pos_dist.sample() - rand_joint_vel = vel_dist.sample() - - articulation.write_joint_state_to_sim_index(position=rand_joint_pos, velocity=rand_joint_vel) - # make sure valued updated - body_link_pose_w = articulation.data.body_link_pose_w.torch - body_com_vel_w = articulation.data.body_com_vel_w.torch - original_body_states = torch.cat([original_body_link_pose_w, original_body_com_vel_w], dim=-1) - body_state_w = torch.cat([body_link_pose_w, body_com_vel_w], dim=-1) - 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 - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # validate link - com conistency - body_com_pos_b = articulation.data.body_com_pos_b.torch - body_com_quat_b = articulation.data.body_com_quat_b.torch - expected_com_pos, expected_com_quat = math_utils.combine_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:].view(-1, 4), - 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("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.skip(reason=_SPATIAL_TENDON_OVSTAGE_GAP_REASON) -def test_spatial_tendons(sim, num_articulations, device): - """Test spatial tendons apis. - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation has spatial tendons - 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 - """ - # skip test if Isaac Sim version is less than 5.0 - if has_kit() and get_isaac_sim_version().major < 5: - pytest.skip("Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later.") - return - articulation_cfg = generate_articulation_cfg(articulation_type="spatial_tendon_test_asset") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 3) - 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) - assert articulation.num_spatial_tendons == 1 - - articulation.set_spatial_tendon_stiffness_index(stiffness=10.0) - articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=10.0) - articulation.set_spatial_tendon_damping_index(damping=10.0) - articulation.set_spatial_tendon_offset_index(offset=10.0) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground_plane): - """Test applying of joint position target functions correctly for a robotic arm.""" - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # apply action to the articulation - dynamic_friction = torch.rand(num_articulations, articulation.num_joints, device=device) - viscous_friction = torch.rand(num_articulations, articulation.num_joints, device=device) - friction = torch.rand(num_articulations, articulation.num_joints, device=device) - - # Guarantee that the dynamic friction is not greater than the static friction - dynamic_friction = torch.min(dynamic_friction, friction) - - # The static friction must be set first to be sure the dynamic friction is not greater than static - # when both are set. - articulation.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=friction, - joint_dynamic_friction_coeff=dynamic_friction, - joint_viscous_friction_coeff=viscous_friction, - ) - articulation.write_data_to_sim() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - friction_props_from_sim = _read_binding_to_torch(articulation, TT.DOF_FRICTION_PROPERTIES, "cpu") - joint_friction_coeff_sim = friction_props_from_sim[:, :, 0] - joint_dynamic_friction_coeff_sim = friction_props_from_sim[:, :, 1] - joint_viscous_friction_coeff_sim = friction_props_from_sim[:, :, 2] - assert torch.allclose(joint_dynamic_friction_coeff_sim, dynamic_friction.cpu()) - assert torch.allclose(joint_viscous_friction_coeff_sim, viscous_friction.cpu()) - assert torch.allclose(joint_friction_coeff_sim, friction.cpu()) - - # For Isaac Sim >= 5.0: also test the combined API that can set dynamic and viscous via - # write_joint_friction_coefficient_to_sim; reset the sim to isolate this path. - if has_kit() and get_isaac_sim_version().major >= 5: - # Reset simulator to ensure a clean state for the alternative API path - sim.reset() - - # Warm up a few steps to populate buffers - for _ in range(100): - sim.step() - articulation.update(sim.cfg.dt) - - # New random coefficients - dynamic_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) - viscous_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) - friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) - - # Guarantee that the dynamic friction is not greater than the static friction - dynamic_friction_2 = torch.min(dynamic_friction_2, friction_2) - - # Use the combined setter to write all three at once - articulation.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=friction_2, - joint_dynamic_friction_coeff=dynamic_friction_2, - joint_viscous_friction_coeff=viscous_friction_2, - ) - articulation.write_data_to_sim() - - # Step to let sim ingest new params and refresh data buffers - for _ in range(100): - sim.step() - articulation.update(sim.cfg.dt) - - friction_props_from_sim_2 = _read_binding_to_torch(articulation, TT.DOF_FRICTION_PROPERTIES, "cpu") - joint_friction_coeff_sim_2 = friction_props_from_sim_2[:, :, 0] - friction_dynamic_coef_sim_2 = friction_props_from_sim_2[:, :, 1] - friction_viscous_coeff_sim_2 = friction_props_from_sim_2[:, :, 2] - - # Validate values propagated - assert torch.allclose(friction_viscous_coeff_sim_2, viscous_friction_2.cpu()) - assert torch.allclose(friction_dynamic_coef_sim_2, dynamic_friction_2.cpu()) - assert torch.allclose(joint_friction_coeff_sim_2, friction_2.cpu()) - - -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_set_material_properties(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test getting and setting per-shape material properties (friction/restitution). - - OVPhysX exposes per-collision-shape material as the - ``articulation_shape_friction_and_restitution`` tensor binding (shape ``[N, S, 3]`` = - static friction, dynamic friction, restitution), addressed through the - :class:`~isaaclab_ov.sim.views.OvPhysxView`. The binding is CPU-native, so the - buffer lives in host memory. (The PhysX backend instead uses a dedicated - ``root_view.get_material_properties`` / ``set_material_properties`` view API.) - """ - if not hasattr(TT, "SHAPE_FRICTION_AND_RESTITUTION"): - pytest.skip("ovphysx wheel does not expose the shape material tensor type") - - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - view = articulation.root_view - # Number of collision shapes per articulation, from the material binding's shape [N, S, 3]. - num_shapes = view.binding_for(TT.SHAPE_FRICTION_AND_RESTITUTION).shape[1] - - # Random material per shape: (static_friction, dynamic_friction, restitution), on the CPU. - materials = torch.empty(num_articulations, num_shapes, 3, device="cpu").uniform_(0.0, 1.0) - materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) # dynamic <= static - - # Set material properties through the view, then simulate. - view.set_attribute(TT.SHAPE_FRICTION_AND_RESTITUTION, wp.from_torch(materials, dtype=wp.float32)) - sim.step() - articulation.update(sim.cfg.dt) - - # Read back from the simulation and verify the round-trip. - materials_check = wp.to_torch(view.get_attribute(TT.SHAPE_FRICTION_AND_RESTITUTION)) - torch.testing.assert_close(materials_check, materials) - - -if __name__ == "__main__": - pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab_ov/test/assets/test_deformable_object.py b/source/isaaclab_ov/test/assets/test_deformable_object.py index 0bc028a1ee37..7d6fe394650e 100644 --- a/source/isaaclab_ov/test/assets/test_deformable_object.py +++ b/source/isaaclab_ov/test/assets/test_deformable_object.py @@ -3,535 +3,93 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - -"""Real-backend tests for the OVPhysX deformable object.""" +"""Minimal real-OVPhysX integration coverage for deformable objects.""" from __future__ import annotations -import multiprocessing -import queue -import sys -import traceback -from typing import Any - -import ovphysx.types # noqa: F401 import pytest import torch import warp as wp -from flaky import flaky + +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + from isaaclab_ov import tensor_types as TT # noqa: E402 from isaaclab_ov.physics import OvPhysxCfg, OvPhysxManager # noqa: E402 -from isaaclab_physx.sim.schemas import PhysxCollisionPropertiesCfg, PhysxRigidBodyPropertiesCfg # noqa: E402 from isaaclab_physx.sim.spawners.materials import PhysxDeformableBodyMaterialCfg # noqa: E402 -from pxr import Gf, Sdf, Usd, UsdGeom # noqa: E402 - import isaaclab.sim as sim_utils # noqa: E402 -import isaaclab.utils.math as math_utils # noqa: E402 -from isaaclab.assets import DeformableObject, DeformableObjectCfg, RigidObjectCfg # noqa: E402 -from isaaclab.scene import InteractiveScene, InteractiveSceneCfg # noqa: E402 +from isaaclab.assets import DeformableObject, DeformableObjectCfg # noqa: E402 from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 -from isaaclab.utils.configclass import configclass # noqa: E402 from ..deformable_utils import ( # noqa: E402 pre_tetrahedralized_deformable_spawn_cfg, pretriangulated_surface_deformable_spawn_cfg, ) -wp.init() - - -@configclass -class DeformableSceneCfg(InteractiveSceneCfg): - """Interactive scene configuration for cloned volume deformables.""" - - deformable: DeformableObjectCfg = DeformableObjectCfg( - prim_path="{ENV_REGEX_NS}/Object", - spawn=pre_tetrahedralized_deformable_spawn_cfg(), - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), - ) - - -@configclass -class MixedDeformableRigidSceneCfg(InteractiveSceneCfg): - """Interactive scene configuration for cloned deformable and rigid assets.""" - - deformable: DeformableObjectCfg = DeformableObjectCfg( - prim_path="{ENV_REGEX_NS}/Object", - spawn=pre_tetrahedralized_deformable_spawn_cfg(), - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), - ) - cube: RigidObjectCfg = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), - rigid_props=PhysxRigidBodyPropertiesCfg(disable_gravity=True), - collision_props=PhysxCollisionPropertiesCfg(collision_enabled=True), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.35, 0.0, 1.0)), - ) - +pytestmark = pytest.mark.integration -@configclass -class HeterogeneousMixedDeformableRigidSceneCfg(InteractiveSceneCfg): - """Interactive scene configuration with two rigid variants and a deformable.""" - deformable: DeformableObjectCfg = DeformableObjectCfg( - prim_path="{ENV_REGEX_NS}/Object", - spawn=pre_tetrahedralized_deformable_spawn_cfg(), - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), +def _sim_context(): + """Build the CUDA OVPhysX context required by deformable bindings.""" + return build_simulation_context( + sim_cfg=SimulationCfg(physics=OvPhysxCfg(), device="cuda:0", dt=0.01), + auto_add_lighting=False, ) - shape: RigidObjectCfg = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Shape", - spawn=sim_utils.MultiAssetSpawnerCfg( - assets_cfg=[ - sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)), - sim_utils.SphereCfg(radius=0.05), - ], - rigid_props=PhysxRigidBodyPropertiesCfg(disable_gravity=True), - collision_props=PhysxCollisionPropertiesCfg(collision_enabled=True), - random_choice=False, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.35, 0.0, 1.0)), - ) - -def _ovphysx_sim_context(device: str, *, gravity_enabled: bool = True): - """Build a kitless OVPhysX simulation context.""" - gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - sim_cfg = SimulationCfg(physics=OvPhysxCfg(), device=device, dt=0.01, gravity=gravity) - return build_simulation_context(device=device, sim_cfg=sim_cfg, auto_add_lighting=True) - -def _generate_deformable_scene( - spawn: sim_utils.SpawnerCfg, - num_objects: int = 2, - height: float = 1.0, - initial_rot: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), -) -> DeformableObject: - """Create independently authored deformables beneath matching parent prims.""" - for index in range(num_objects): - sim_utils.create_prim(f"/World/Table_{index}", "Xform", translation=(index * 1.0, 0.0, height)) - cfg = DeformableObjectCfg( - prim_path="/World/Table_[^/]*/Object", - spawn=spawn, - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height), rot=initial_rot), +def _spawn_deformables(spawn: sim_utils.SpawnerCfg) -> DeformableObject: + """Author two local deformables in an in-memory USD stage.""" + for index in range(2): + sim_utils.create_prim(f"/World/Env_{index}", "Xform", translation=(index * 1.0, 0.0, 1.0)) + return DeformableObject( + DeformableObjectCfg( + prim_path="/World/Env_[^/]*/Object", + spawn=spawn, + init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) ) - return DeformableObject(cfg=cfg) - - -def _assert_finite_deformable_state(deformable: DeformableObject) -> None: - """Assert finite nodal and derived root state.""" - assert torch.isfinite(deformable.data.nodal_state_w.torch).all() - assert torch.isfinite(deformable.data.root_pos_w.torch).all() - assert torch.isfinite(deformable.data.root_vel_w.torch).all() def _canonical_connectivity(connectivity: torch.Tensor) -> list[set[tuple[int, ...]]]: - """Return unordered elements with each element's vertex indices sorted.""" + """Convert each body's topology into order-independent literal elements.""" return [{tuple(sorted(element)) for element in body} for body in connectivity.cpu().tolist()] -def _assert_rest_positions_match_authored( - rest_positions: torch.Tensor, authored_points: torch.Tensor, prim_paths: list[str] -) -> None: - """Assert each body contains the authored local rest points transformed into world space.""" - assert rest_positions.shape == (rest_positions.shape[0], authored_points.shape[0], 3) - assert torch.is_floating_point(rest_positions) - assert torch.isfinite(rest_positions).all() - assert len(prim_paths) == rest_positions.shape[0] - stage = sim_utils.get_current_stage() - xform_cache = UsdGeom.XformCache() - for body_points, prim_path in zip(rest_positions, prim_paths): - local_to_world = xform_cache.GetLocalToWorldTransform(stage.GetPrimAtPath(prim_path)) - expected = torch.tensor( - [tuple(local_to_world.Transform(Gf.Vec3d(*point.tolist()))) for point in authored_points.cpu()], - dtype=rest_positions.dtype, - device=rest_positions.device, - ) - distances = torch.cdist(body_points, expected) - torch.testing.assert_close( - distances.min(dim=0).values, torch.zeros(expected.shape[0], device=expected.device), atol=1e-6, rtol=0.0 - ) - torch.testing.assert_close( - distances.min(dim=1).values, torch.zeros(expected.shape[0], device=expected.device), atol=1e-6, rtol=0.0 - ) - - -def _run_cpu_deformable_initialization(result_queue: Any) -> None: - """Run the CPU initialization contract in a fresh spawned process.""" - try: - with _ovphysx_sim_context(device="cpu") as sim: - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg(), num_objects=5) - assert sys.getrefcount(deformable) < 10 - try: - sim.reset() - except RuntimeError as error: - result = ("runtime_error", str(error), deformable.is_initialized) - else: - result = ("no_error", "", deformable.is_initialized) - except BaseException: - result = ("child_error", traceback.format_exc(), None) - result_queue.put(result) - - -@pytest.mark.parametrize( - "num_objects, material_path", - [ - (1, "material"), - (2, None), - (2, "/World/SoftMaterial"), - (2, "material"), - ], -) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_initialization(num_objects: int, material_path: str | None): - """Test volume deformable initialization and public buffer shapes.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - deformable = _generate_deformable_scene( - pre_tetrahedralized_deformable_spawn_cfg(material_path=material_path), num_objects=num_objects - ) - - assert sys.getrefcount(deformable) < 10 - sim.reset() - - assert deformable.is_initialized - assert deformable.num_instances == num_objects - assert deformable.num_bodies == 1 - assert deformable.root_view.count == num_objects - if material_path is None: - assert deformable.material_physx_view is None - elif material_path.startswith("/"): - assert deformable.material_physx_view is not None - assert deformable.material_physx_view.count == 1 - else: - assert deformable.material_physx_view is not None - assert deformable.material_physx_view.count == num_objects - assert deformable.data.nodal_state_w.torch.shape == ( - num_objects, - deformable.max_sim_vertices_per_body, - 6, - ) - assert deformable.data.nodal_kinematic_target is not None - assert deformable.data.nodal_kinematic_target.torch.shape == ( - num_objects, - deformable.max_sim_vertices_per_body, - 4, - ) - assert deformable.data.root_pos_w.torch.shape == (num_objects, 3) - assert deformable.data.root_vel_w.torch.shape == (num_objects, 3) - - deformable._invalidate_initialize_callback(None) - assert deformable._root_physx_view is None - assert deformable._material_physx_view is None - - @pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") @pytest.mark.isaacsim_ci -def test_absolute_material_sibling_prefix_is_not_expanded(): - """Keep a shared absolute material exact when its path only textually prefixes the asset path.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - material_path = "/World/Table_0/ObjectSiblingMaterial" - deformable = _generate_deformable_scene( - pre_tetrahedralized_deformable_spawn_cfg(material_path=material_path), num_objects=2 - ) +def test_volume_deformable_real_ovphysx_seams() -> None: + """Prove volume topology, partial state/target/material writes, and stepping.""" + with _sim_context() as sim: + material_path = "/World/Env_0/ObjectSiblingMaterial" + deformable = _spawn_deformables(pre_tetrahedralized_deformable_spawn_cfg(material_path=material_path)) distractor_cfg = PhysxDeformableBodyMaterialCfg() - distractor_cfg.func("/World/Table_1/ObjectSiblingMaterial", distractor_cfg) - - sim.reset() - - material_view = deformable.material_physx_view - assert material_view is not None - assert material_view.count == 1 - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_initialization_surface_deformable(): - """Test surface deformable initialization and unsupported target writes.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - num_objects = 2 - deformable = _generate_deformable_scene(pretriangulated_surface_deformable_spawn_cfg(), num_objects=num_objects) - - sim.reset() - - assert deformable.is_initialized - assert deformable._deformable_type == "surface" - assert deformable.num_instances == num_objects - assert deformable.root_view.count == num_objects - assert deformable.material_physx_view is not None - assert deformable.material_physx_view.count == num_objects - assert deformable.data.nodal_state_w.torch.shape == ( - num_objects, - deformable.max_sim_vertices_per_body, - 6, - ) - assert deformable.data.root_pos_w.torch.shape == (num_objects, 3) - assert deformable.data.root_vel_w.torch.shape == (num_objects, 3) - assert deformable.data.nodal_kinematic_target is None - - dummy_targets = torch.zeros(num_objects, deformable.max_sim_vertices_per_body, 4, device=sim.device) - with pytest.raises(ValueError, match="Kinematic targets can only be set for volume deformable bodies"): - deformable.write_nodal_kinematic_target_to_sim_index(dummy_targets) - - -@pytest.mark.isaacsim_ci -def test_initialization_on_device_cpu(): - """Test that OVPhysX deformable initialization rejects a CPU simulation.""" - context = multiprocessing.get_context("spawn") - result_queue = context.Queue() - process = context.Process(target=_run_cpu_deformable_initialization, args=(result_queue,)) - process.start() - process.join(timeout=30.0) - - if process.is_alive(): - process.terminate() - process.join() - pytest.fail("CPU deformable initialization child process timed out.") - assert process.exitcode == 0 - - try: - result_kind, message, is_initialized = result_queue.get(timeout=5.0) - except queue.Empty: - pytest.fail("CPU deformable initialization child process returned no result.") - finally: - result_queue.close() - result_queue.join_thread() - - assert result_kind == "runtime_error", message - assert message == "OVPhysX deformable tensors require a CUDA simulation device; received 'cpu'." - assert is_initialized is False - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_set_nodal_state(): - """Test combined nodal state writes while independently randomizing position and velocity.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - num_objects = 2 - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg(), num_objects=num_objects) - sim.reset() - - for state_type_to_randomize in ["nodal_pos_w", "nodal_vel_w"]: - state_dict = { - "nodal_pos_w": torch.zeros_like(deformable.data.nodal_pos_w.torch), - "nodal_vel_w": torch.zeros_like(deformable.data.nodal_vel_w.torch), - } - - for _ in range(5): - deformable.reset() - state_dict[state_type_to_randomize] = torch.randn( - num_objects, deformable.max_sim_vertices_per_body, 3, device=sim.device - ) - - for _ in range(5): - nodal_state = torch.cat([state_dict["nodal_pos_w"], state_dict["nodal_vel_w"]], dim=-1) - deformable.write_nodal_state_to_sim_index(nodal_state) - torch.testing.assert_close(deformable.data.nodal_state_w.torch, nodal_state, rtol=1e-5, atol=1e-5) - - sim.step() - deformable.update(sim.cfg.dt) - - -@pytest.mark.parametrize( - ("property_name", "write_method_name", "tensor_type", "command_value"), - [ - ("nodal_pos_w", "write_nodal_pos_to_sim_index", "deformable_sim_nodal_position", 100.0), - ("nodal_vel_w", "write_nodal_velocity_to_sim_index", "deformable_sim_nodal_velocity", -100.0), - ], -) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_indexed_partial_write_preserves_retained_aliased_slice_in_simulator( - property_name: str, write_method_name: str, tensor_type: str, command_value: float -) -> None: - """Preserve a retained selected command while hydrating stale rows from OVPhysX.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg(), num_objects=2) - sim.reset() - - retained = getattr(deformable.data, property_name).torch - sim.step() - deformable.update(sim.cfg.dt) - latest = wp.to_torch(deformable.root_view.get_attribute(tensor_type)).clone() - selected = retained[1:2] - selected.fill_(command_value) - - getattr(deformable, write_method_name)(selected, env_ids=torch.tensor([1], device=sim.device)) - readback = wp.to_torch(deformable.root_view.get_attribute(tensor_type)) - - torch.testing.assert_close(readback[0], latest[0], rtol=1e-5, atol=1e-5) - torch.testing.assert_close(readback[1], selected[0], rtol=1e-5, atol=1e-5) - - -@pytest.mark.parametrize( - "num_objects, randomize_pos, randomize_rot", - [ - (1, False, False), - (1, True, False), - (1, False, True), - (2, True, True), - ], -) -@flaky(max_runs=3, min_passes=1) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_set_nodal_state_with_applied_transform(num_objects: int, randomize_pos: bool, randomize_rot: bool): - """Test combined nodal state writes after applying rigid transforms.""" - with _ovphysx_sim_context(device="cuda:0", gravity_enabled=False) as sim: - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg(), num_objects=num_objects) - sim.reset() - - for _ in range(5): - nodal_state = deformable.data.default_nodal_state_w.torch.clone() - mean_nodal_pos_default = nodal_state[..., :3].mean(dim=1) - - if randomize_pos: - pos_w = 0.5 * torch.rand(deformable.num_instances, 3, device=sim.device) - pos_w[:, 2] += 0.5 - else: - pos_w = None - if randomize_rot: - quat_w = math_utils.random_orientation(deformable.num_instances, device=sim.device) - else: - quat_w = None - - nodal_state[..., :3] = deformable.transform_nodal_pos(nodal_state[..., :3], pos_w, quat_w) - mean_nodal_pos_init = nodal_state[..., :3].mean(dim=1) - - if pos_w is None: - torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default, rtol=1e-5, atol=1e-5) - else: - torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default + pos_w, rtol=1e-5, atol=1e-5) - - deformable.write_nodal_state_to_sim_index(nodal_state) - deformable.reset() - - for _ in range(50): - sim.step() - deformable.update(sim.cfg.dt) - - torch.testing.assert_close(deformable.data.root_pos_w.torch, mean_nodal_pos_init, rtol=1e-4, atol=1e-4) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_set_kinematic_targets(): - """Test pinning one volume deformable while another falls under gravity.""" - with _ovphysx_sim_context(device="cuda:0", gravity_enabled=True) as sim: - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg(), num_objects=2, height=1.0) - sim.reset() - - nodal_kinematic_targets = wp.to_torch( - deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_KINEMATIC_TARGET) - ).clone() - - for _ in range(5): - deformable.write_nodal_state_to_sim_index(deformable.data.default_nodal_state_w.torch) - default_root_pos = deformable.data.default_nodal_state_w.torch[..., :3].mean(dim=1) - deformable.reset() - - nodal_kinematic_targets[1:, :, 3] = 1.0 - nodal_kinematic_targets[0, :, 3] = 0.0 - nodal_kinematic_targets[0, :, :3] = deformable.data.default_nodal_state_w.torch[0, :, :3] - deformable.write_nodal_kinematic_target_to_sim_index( - nodal_kinematic_targets[0:1], env_ids=torch.tensor([0], device=sim.device) - ) - - for _ in range(20): - sim.step() - deformable.update(sim.cfg.dt) - - torch.testing.assert_close( - deformable.data.nodal_pos_w.torch[0], - nodal_kinematic_targets[0, :, :3], - rtol=1e-5, - atol=1e-5, - ) - assert torch.all(deformable.data.root_pos_w.torch[1:, 2] < default_root_pos[1:, 2]) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_volume_deformable_reads_writes_targets_materials_and_steps(): - """Exercise authored volume state, topology, targets, materials, and stepping.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg()) - + distractor_cfg.func("/World/Env_1/ObjectSiblingMaterial", distractor_cfg) sim.reset() assert deformable.is_initialized assert deformable.num_instances == 2 - assert deformable.num_bodies == 1 - assert deformable.root_view.count == 2 - assert deformable.max_sim_vertices_per_body == 5 - assert deformable.max_sim_elements_per_body == 2 - assert deformable.max_collision_elements_per_body == 2 - assert deformable.max_collision_vertices_per_body == 5 - - nodal_state = deformable.data.nodal_state_w.torch - nodal_pos = deformable.data.nodal_pos_w.torch - nodal_vel = deformable.data.nodal_vel_w.torch - assert nodal_state.shape == (2, 5, 6) - assert deformable.data.default_nodal_state_w.torch.shape == (2, 5, 6) - assert deformable.data.root_pos_w.torch.shape == (2, 3) - assert deformable.data.root_vel_w.torch.shape == (2, 3) - rest_positions = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_REST_NODAL_POSITION)) - _assert_rest_positions_match_authored( - rest_positions, - torch.tensor( - [ - [0.0, 0.0, 0.0], - [0.2, 0.0, 0.0], - [0.0, 0.2, 0.0], - [0.0, 0.0, 0.2], - [0.2, 0.2, 0.2], - ], - device=rest_positions.device, - ), - deformable.root_view.prim_paths, - ) - torch.testing.assert_close(deformable.data.root_pos_w.torch, nodal_pos.mean(dim=1)) - torch.testing.assert_close(deformable.data.root_vel_w.torch, nodal_vel.mean(dim=1)) - - element_indices = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_ELEMENT_INDICES)) - collision_indices = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_COLLISION_ELEMENT_INDICES)) - assert element_indices.shape == (2, 2, 4) - assert collision_indices.shape == (2, 2, 4) - assert element_indices.dtype == torch.int32 - assert collision_indices.dtype == torch.int32 - assert torch.all((element_indices >= 0) & (element_indices < 5)) - assert torch.all((collision_indices >= 0) & (collision_indices < 5)) - expected_tetrahedra = {(0, 1, 2, 3), (1, 2, 3, 4)} - assert all(elements == expected_tetrahedra for elements in _canonical_connectivity(element_indices)) - assert all(elements == expected_tetrahedra for elements in _canonical_connectivity(collision_indices)) - - updated_pos = nodal_pos[1:2].clone() + assert deformable.data.nodal_state_w.torch.shape == (2, 5, 6) + topology = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_ELEMENT_INDICES)) + assert _canonical_connectivity(topology) == [ + {(0, 1, 2, 3), (1, 2, 3, 4)}, + {(0, 1, 2, 3), (1, 2, 3, 4)}, + ] + + initial_pos = deformable.data.nodal_pos_w.torch.clone() + updated_pos = initial_pos[1:2].clone() updated_pos[..., 0] += 0.025 - deformable.write_nodal_pos_to_sim_index(updated_pos, env_ids=torch.tensor([1], device=sim.device)) + deformable.write_nodal_pos_to_sim_index(updated_pos, env_ids=torch.tensor([1], device="cuda:0")) readback_pos = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_NODAL_POSITION)) - torch.testing.assert_close(readback_pos[0], nodal_pos[0], rtol=1e-5, atol=1e-5) + torch.testing.assert_close(readback_pos[0], initial_pos[0], rtol=1e-5, atol=1e-5) torch.testing.assert_close(readback_pos[1], updated_pos[0], rtol=1e-5, atol=1e-5) - updated_vel = nodal_vel[0:1].clone() - updated_vel[..., 1] = 0.1 - deformable.write_nodal_velocity_to_sim_index(updated_vel, env_ids=torch.tensor([0])) - readback_vel = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_NODAL_VELOCITY)) - torch.testing.assert_close(readback_vel[0], updated_vel[0], rtol=1e-5, atol=1e-5) - torch.testing.assert_close(readback_vel[1], nodal_vel[1], rtol=1e-5, atol=1e-5) - targets = deformable.data.nodal_kinematic_target assert targets is not None - assert targets.torch.shape == (2, 5, 4) - torch.testing.assert_close(targets.torch[..., 3], torch.ones_like(targets.torch[..., 3])) updated_targets = targets.torch[1:2].clone() - updated_targets[..., :3] = readback_pos[1:2] + torch.tensor([0.0, 0.0, 0.03], device=sim.device) + updated_targets[..., :3] = readback_pos[1:2] + torch.tensor([0.0, 0.0, 0.03], device="cuda:0") updated_targets[..., 3] = 0.0 deformable.write_nodal_kinematic_target_to_sim_index( - updated_targets, env_ids=torch.tensor([1], device=sim.device) + updated_targets, env_ids=torch.tensor([1], device="cuda:0") ) readback_targets = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_KINEMATIC_TARGET)) torch.testing.assert_close(readback_targets[0, :, 3], torch.ones_like(readback_targets[0, :, 3])) @@ -539,270 +97,67 @@ def test_volume_deformable_reads_writes_targets_materials_and_steps(): material_view = deformable.material_physx_view assert material_view is not None - assert material_view.count == 2 - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_DYNAMIC_FRICTION)), torch.full((2,), 0.5) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_YOUNGS_MODULUS)), torch.full((2,), 1000.0) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_POISSONS_RATIO)), torch.full((2,), 0.3) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_ELASTICITY_DAMPING)), torch.full((2,), 0.005) - ) - - updated_youngs = torch.tensor([1000.0, 1500.0]) + assert material_view.count == 1 + youngs = torch.tensor([1500.0]) material_view.set_attribute( TT.DEFORMABLE_MATERIAL_YOUNGS_MODULUS, - wp.from_torch(updated_youngs), - indices=wp.array([1], dtype=wp.int32), + wp.from_torch(youngs), + indices=wp.array([0], dtype=wp.int32), ) torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_YOUNGS_MODULUS)), updated_youngs.cpu() + wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_YOUNGS_MODULUS)), youngs ) - for _ in range(5): - sim.step() - deformable.update(sim.cfg.dt) - _assert_finite_deformable_state(deformable) + original_view = deformable.root_view + original_position_binding = original_view.binding_for(TT.DEFORMABLE_SIM_NODAL_POSITION) + OvPhysxManager._warmup_done = False + sim.reset() + assert deformable.is_initialized + assert deformable.root_view is not original_view + assert deformable.root_view.binding_for(TT.DEFORMABLE_SIM_NODAL_POSITION) is not original_position_binding + assert torch.isfinite(deformable.data.nodal_state_w.torch).all() + + sim.step() + deformable.update(sim.cfg.dt) + assert torch.isfinite(deformable.data.nodal_state_w.torch).all() @pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") @pytest.mark.isaacsim_ci -def test_surface_deformable_reads_writes_materials_and_steps(): - """Exercise authored surface state, topology, materials, and stepping.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - deformable = _generate_deformable_scene(pretriangulated_surface_deformable_spawn_cfg()) - +def test_surface_deformable_real_ovphysx_seams() -> None: + """Prove surface topology/state/material, rejected targets, and stepping.""" + with _sim_context() as sim: + deformable = _spawn_deformables(pretriangulated_surface_deformable_spawn_cfg()) sim.reset() assert deformable.is_initialized - assert deformable.num_instances == 2 - assert deformable.root_view.count == 2 - assert deformable.max_sim_vertices_per_body == 4 - assert deformable.max_sim_elements_per_body == 2 - assert deformable.max_collision_elements_per_body == 0 assert deformable.data.nodal_state_w.torch.shape == (2, 4, 6) - assert deformable.data.root_pos_w.torch.shape == (2, 3) - assert deformable.data.root_vel_w.torch.shape == (2, 3) - rest_positions = wp.to_torch(deformable.root_view.get_attribute(TT.SURFACE_DEFORMABLE_REST_POSITION)) - _assert_rest_positions_match_authored( - rest_positions, - torch.tensor( - [ - [0.0, 0.0, 0.0], - [0.2, 0.0, 0.0], - [0.2, 0.2, 0.0], - [0.0, 0.2, 0.0], - ], - device=rest_positions.device, - ), - deformable.root_view.prim_paths, - ) - - element_indices = wp.to_torch(deformable.root_view.get_attribute(TT.SURFACE_DEFORMABLE_SIM_ELEMENT_INDICES)) - assert element_indices.shape == (2, 2, 3) - assert element_indices.dtype == torch.int32 - assert torch.all((element_indices >= 0) & (element_indices < 4)) - expected_triangles = {(0, 1, 2), (0, 2, 3)} - assert all(elements == expected_triangles for elements in _canonical_connectivity(element_indices)) - + topology = wp.to_torch(deformable.root_view.get_attribute(TT.SURFACE_DEFORMABLE_SIM_ELEMENT_INDICES)) + assert _canonical_connectivity(topology) == [{(0, 1, 2), (0, 2, 3)}, {(0, 1, 2), (0, 2, 3)}] assert deformable.data.nodal_kinematic_target is None - dummy_targets = torch.zeros((2, 4, 4), device=sim.device) with pytest.raises(ValueError, match="Kinematic targets can only be set for volume deformable bodies"): - deformable.write_nodal_kinematic_target_to_sim_index(dummy_targets) + deformable.write_nodal_kinematic_target_to_sim_index(torch.zeros((2, 4, 4), device="cuda:0")) - nodal_pos = deformable.data.nodal_pos_w.torch - updated_pos = nodal_pos[1:2].clone() - updated_pos[..., 0] += 0.025 - deformable.write_nodal_pos_to_sim_index(updated_pos, env_ids=torch.tensor([1], device=sim.device)) + initial_pos = deformable.data.nodal_pos_w.torch.clone() + updated_pos = initial_pos[1:2].clone() + updated_pos[..., 1] += 0.025 + deformable.write_nodal_pos_to_sim_index(updated_pos, env_ids=torch.tensor([1], device="cuda:0")) readback_pos = wp.to_torch(deformable.root_view.get_attribute(TT.SURFACE_DEFORMABLE_SIM_POSITION)) - torch.testing.assert_close(readback_pos[0], nodal_pos[0], rtol=1e-5, atol=1e-5) + torch.testing.assert_close(readback_pos[0], initial_pos[0], rtol=1e-5, atol=1e-5) torch.testing.assert_close(readback_pos[1], updated_pos[0], rtol=1e-5, atol=1e-5) - nodal_vel = deformable.data.nodal_vel_w.torch - updated_vel = nodal_vel[0:1].clone() - updated_vel[..., 1] = 0.1 - deformable.write_nodal_velocity_to_sim_index(updated_vel, env_ids=torch.tensor([0])) - readback_vel = wp.to_torch(deformable.root_view.get_attribute(TT.SURFACE_DEFORMABLE_SIM_VELOCITY)) - torch.testing.assert_close(readback_vel[0], updated_vel[0], rtol=1e-5, atol=1e-5) - torch.testing.assert_close(readback_vel[1], nodal_vel[1], rtol=1e-5, atol=1e-5) - material_view = deformable.material_physx_view assert material_view is not None - assert material_view.count == 2 - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_DYNAMIC_FRICTION)), torch.full((2,), 0.4) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_YOUNGS_MODULUS)), torch.full((2,), 2000.0) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_POISSONS_RATIO)), torch.full((2,), 0.25) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_ELASTICITY_DAMPING)), torch.full((2,), 0.03) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_BENDING_STIFFNESS)), torch.full((2,), 0.6) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_THICKNESS)), torch.full((2,), 0.02) - ) - torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_BENDING_DAMPING)), torch.full((2,), 0.04) - ) - - updated_bending_damping = torch.tensor([0.08, 0.04]) + damping = torch.tensor([0.08, 0.04]) material_view.set_attribute( TT.DEFORMABLE_MATERIAL_BENDING_DAMPING, - wp.from_torch(updated_bending_damping), + wp.from_torch(damping), indices=wp.array([0], dtype=wp.int32), ) torch.testing.assert_close( - wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_BENDING_DAMPING)), - updated_bending_damping.cpu(), + wp.to_torch(material_view.get_attribute(TT.DEFORMABLE_MATERIAL_BENDING_DAMPING)), damping ) - for _ in range(5): - sim.step() - deformable.update(sim.cfg.dt) - _assert_finite_deformable_state(deformable) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_deformable_interactive_scene_uses_full_authored_stage(): - """Initialize cloned deformable bodies and materials from the full authored stage.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - scene = InteractiveScene(DeformableSceneCfg(num_envs=3, env_spacing=0.75, lazy_sensor_update=False)) - - sim.reset() - - deformable = scene["deformable"] - assert deformable.num_instances == 3 - assert deformable.root_view.count == 3 - assert deformable.material_physx_view is not None - assert deformable.material_physx_view.count == 3 - - sim.step() - scene.update(sim.cfg.dt) - _assert_finite_deformable_state(deformable) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_forced_rewarm_rebuilds_deformable_bindings(): - """Replace deformable bindings when a forced re-warm replaces the attached stage.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - deformable = _generate_deformable_scene(pre_tetrahedralized_deformable_spawn_cfg(), num_objects=2) - sim.reset() - - original_view = deformable.root_view - original_binding = original_view.binding_for(TT.DEFORMABLE_SIM_NODAL_POSITION) - - OvPhysxManager._warmup_done = False - sim.reset() - - assert deformable.is_initialized - assert deformable.root_view is not original_view - assert deformable.root_view.binding_for(TT.DEFORMABLE_SIM_NODAL_POSITION) is not original_binding - _assert_finite_deformable_state(deformable) sim.step() deformable.update(sim.cfg.dt) - _assert_finite_deformable_state(deformable) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_mixed_deformable_rigid_scene_does_not_duplicate_runtime_clones(): - """Keep deformable, material, and rigid clone counts aligned in a mixed scene.""" - with _ovphysx_sim_context(device="cuda:0") as sim: - scene = InteractiveScene(MixedDeformableRigidSceneCfg(num_envs=3, env_spacing=0.75, lazy_sensor_update=False)) - - sim.reset() - - deformable = scene["deformable"] - cube = scene["cube"] - assert deformable.num_instances == 3 - assert deformable.root_view.count == 3 - assert deformable.material_physx_view is not None - assert deformable.material_physx_view.count == 3 - assert cube.num_instances == 3 - assert cube.root_view.count == 3 - - sim.step() - scene.update(sim.cfg.dt) - _assert_finite_deformable_state(deformable) - assert torch.isfinite(cube.data.root_pos_w.torch).all() - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_heterogeneous_mixed_deformable_rigid_scene_materializes_missing_targets( - monkeypatch: pytest.MonkeyPatch, -): - """Materialize missing rigid targets beside full-stage deformable clones without duplicates.""" - from isaaclab import cloner - - clone_cfg_type = cloner.CloneCfg - monkeypatch.setattr( - cloner, - "CloneCfg", - lambda device: clone_cfg_type(device=device, clone_strategy=cloner.sequential), - ) - - with _ovphysx_sim_context(device="cuda:0") as sim: - num_envs = 4 - scene = InteractiveScene( - HeterogeneousMixedDeformableRigidSceneCfg( - num_envs=num_envs, - env_spacing=1.0, - lazy_sensor_update=False, - ) - ) - plan = scene.clone_plan - assert plan is not None - shape_rows = plan.cfg_rows[id(scene.cfg.shape)] - shape_mask = plan.clone_mask[list(shape_rows)] - assert shape_mask.sum(dim=1).tolist() == [2, 2] - assert shape_mask.sum(dim=0).tolist() == [1, 1, 1, 1] - - expected_paths = {f"/World/envs/env_{index}/Shape" for index in range(num_envs)} - source_paths = {plan.sources[row] for row in shape_rows} - assert source_paths == {"/World/envs/env_0/Shape", "/World/envs/env_1/Shape"} - stage = sim_utils.get_current_stage() - ancestor_path = "/World/envs/env_2/Shape" - camera_path = f"{ancestor_path}/Camera" - UsdGeom.Xform.Define(stage, camera_path) - authored_paths = {path for path in expected_paths if stage.GetPrimAtPath(path).IsValid()} - assert authored_paths == source_paths | {ancestor_path} - authored_deformable_paths = {f"/World/envs/env_{index}/Object/simulation" for index in range(num_envs)} - deformable_rows = plan.cfg_rows[id(scene.cfg.deformable)] - deformable_source_paths = {f"{plan.sources[row]}/simulation" for row in deformable_rows} - assert { - path for path in authored_deformable_paths if stage.GetPrimAtPath(path).IsValid() - } == deformable_source_paths - - sim.reset() - - deformable = scene["deformable"] - shape = scene["shape"] - assert deformable.root_view.count == num_envs, deformable.root_view.prim_paths - assert shape.root_view.count == num_envs, shape.root_view.prim_paths - runtime_paths = shape.root_view.prim_paths - assert set(runtime_paths) == expected_paths - assert len(runtime_paths) == len(set(runtime_paths)) == num_envs - assert OvPhysxManager._stage_usda is not None - layer = Sdf.Layer.CreateAnonymous("materialized.usda") - assert layer.ImportFromString(OvPhysxManager._stage_usda) - materialized_stage = Usd.Stage.Open(layer) - assert materialized_stage.GetPrimAtPath(camera_path).IsValid() - assert all(materialized_stage.GetPrimAtPath(path).IsValid() for path in authored_deformable_paths) - - sim.step() - scene.update(sim.cfg.dt) - _assert_finite_deformable_state(deformable) - assert torch.isfinite(shape.data.root_pos_w.torch).all() + assert torch.isfinite(deformable.data.nodal_state_w.torch).all() diff --git a/source/isaaclab_ov/test/assets/test_rigid_object.py b/source/isaaclab_ov/test/assets/test_rigid_object.py index d8d330b60256..7a55aeaa80ff 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object.py @@ -3,1306 +3,110 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Real-backend tests for the OVPhysX RigidObject. - -Run via ``./scripts/run_ovphysx.sh -m pytest`` (kitless, no ``AppLauncher``). - -The OVPhysX runtime fixes device mode (CPU vs GPU) when the process creates -its first ``ovphysx.PhysX`` instance and cannot switch it without a process -restart. Full coverage therefore requires two separate pytest -invocations -- once with ``-k 'cpu'`` and once with ``-k 'cuda:0'``. The -``_ovphysx_skip_other_device`` autouse fixture below preempts the manager's -:exc:`RuntimeError` by ``pytest.skip``-ing on the unlocked device so -single-device runs finish cleanly. -""" +"""Minimal real-OVPhysX integration coverage for rigid objects.""" from __future__ import annotations -import logging -import sys -from typing import Literal -from unittest.mock import MagicMock - import pytest import torch import warp as wp -from flaky import flaky - -from isaaclab.test.utils import test_devices -# The OVPhysX runtime wheel is optional. Skip gracefully when it is not installed; -# CI jobs that need OVPhysX coverage install it explicitly. pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") from isaaclab_ov import tensor_types as TT # noqa: E402 from isaaclab_ov.assets import RigidObject # noqa: E402 -from isaaclab_ov.physics import OvPhysxCfg, OvPhysxManager # noqa: E402 +from isaaclab_ov.physics import OvPhysxCfg # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.assets import RigidObjectCfg # noqa: E402 from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 -from isaaclab.sim.spawners import materials # noqa: E402 -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR # noqa: E402 -from isaaclab.utils.math import ( # noqa: E402 - combine_frame_transforms, - default_orientation, - quat_apply_inverse, - quat_inv, - quat_mul, - quat_rotate, - random_orientation, -) - -wp.init() - -pytestmark = pytest.mark.device_split - -_logger = logging.getLogger(__name__) - - -_LOCKED_DEVICE: list[str | None] = [None] -"""Device the session pins to on the first parametrized test that runs.""" - - -@pytest.fixture(autouse=True) -def _ovphysx_skip_other_device(request): - """Skip parametrized tests on the device the session is not pinned to. - - See the module docstring for the wheel's process-global device-mode lock. - """ - callspec = getattr(request.node, "callspec", None) - device = callspec.params.get("device") if callspec is not None else None - if device is None: - # Test does not parametrize on device (e.g. test_warmup_attach_stage_not_called_for_cpu). - return - locked = _LOCKED_DEVICE[0] - if locked is None: - _LOCKED_DEVICE[0] = device - return - if device != locked: - pytest.skip( - f"ovphysx process-global device lock is held by '{locked}'; cannot run '{device}' " - "tests in the same session. Run pytest twice (once per device) for full coverage." - ) - - -def _ovphysx_sim_context(device: str, **kwargs): - """Wrapper around :func:`build_simulation_context` that injects OVPhysX cfg. - - PhysX tests pass ``device=device`` directly and let - :func:`build_simulation_context` build a default :class:`SimulationCfg`. - OVPhysX needs ``physics=OvPhysxCfg()`` set on the cfg so the manager - dispatches to OVPhysX rather than PhysX, so we build the cfg here and - pass it through. ``gravity_enabled`` is consumed locally (it is ignored - by ``build_simulation_context`` once a ``sim_cfg`` is provided). - ``add_ground_plane``, ``auto_add_lighting``, and other kwargs continue - to flow through ``build_simulation_context`` as before. - """ - dt = kwargs.pop("dt", 1.0 / 60.0) - gravity_enabled = kwargs.pop("gravity_enabled", True) - gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - sim_cfg = SimulationCfg(physics=OvPhysxCfg(), device=device, dt=dt, gravity=gravity) - return build_simulation_context(device=device, sim_cfg=sim_cfg, **kwargs) - - -def generate_cubes_scene( - num_cubes: int = 1, - height=1.0, - api: Literal["none", "rigid_body", "articulation_root"] = "rigid_body", - kinematic_enabled: bool = False, - device: str = "cuda:0", -) -> tuple[RigidObject, torch.Tensor]: - """Generate a scene with the provided number of cubes. - - Args: - num_cubes: Number of cubes to generate. - height: Height of the cubes. - api: The type of API that the cubes should have. - kinematic_enabled: Whether the cubes are kinematic. - device: Device to use for the simulation. - - Returns: - A tuple containing the rigid object representing the cubes and the origins of the cubes. - - """ - origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) - - # Resolve spawn configuration - if api == "none": - # since no rigid body properties defined, this is just a static collider - spawn_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - elif api == "rigid_body": - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - elif api == "articulation_root": - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/RigidObject/Cube/dex_cube_instanceable_with_articulation_root.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - else: - raise ValueError(f"Unknown api: {api}") - - # Create rigid object. OVPhysX matches prim paths via fnmatch globs (not regex), - # so use ``Table_*`` rather than the PhysX ``Table_.*`` form. - cube_object_cfg = RigidObjectCfg( - prim_path="/World/Table_[^/]+/Object", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height)), - ) - cube_object = RigidObject(cfg=cube_object_cfg) - - return cube_object, origins - - -# --------------------------------------------------------------------------- -# Per-shape material helpers (friction / restitution) -# -# OVPhysX exposes per-collision-shape material as the -# ``rigid_body_shape_friction_and_restitution`` tensor binding (shape ``[N, S, 3]`` = -# static friction, dynamic friction, restitution), addressed through the OvPhysxView. -# (The PhysX backend instead uses ``root_view.get/set_material_properties`` / ``max_shapes``.) -# --------------------------------------------------------------------------- +pytestmark = pytest.mark.integration -def _num_shapes(cube_object) -> int: - """Per-body collision-shape count, from the material binding's ``[N, S, 3]`` shape.""" - return cube_object.root_view.binding_for(TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION).shape[1] - -def _write_shape_material(cube_object, materials_nS3: torch.Tensor) -> None: - """Write a full ``[N, S, 3]`` (static friction, dynamic friction, restitution) tensor via the view.""" - cube_object.root_view.set_attribute( - TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION, wp.from_torch(materials_nS3.contiguous(), dtype=wp.float32) +def _sim_context(): + """Build a local CPU OVPhysX context from an in-memory USD stage.""" + return build_simulation_context( + sim_cfg=SimulationCfg(physics=OvPhysxCfg(), device="cpu", gravity=(0.0, 0.0, 0.0)), + auto_add_lighting=False, ) -def _read_shape_material(cube_object) -> torch.Tensor: - """Read the per-shape material as a torch tensor ``[N, S, 3]``.""" - return wp.to_torch(cube_object.root_view.get_attribute(TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION)) - - -@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 _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - # 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 - 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.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_kinematic_enabled(num_cubes, device): - """Test that initialization for prim with kinematic flag enabled.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - # Generate cubes scene - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - 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) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - # check that the object is kinematic - default_root_pose = cube_object.data.default_root_pose.torch.clone() - default_root_vel = cube_object.data.default_root_vel.torch.clone() - default_root_pose[:, :3] += origins - torch.testing.assert_close(cube_object.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, default_root_vel) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_no_rigid_body(num_cubes, device): - """Test that initialization fails when no rigid body is found at the provided prim path.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="none", device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_articulation_root(num_cubes, device): - """Test that initialization fails when an articulation root is found at the provided prim path.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="articulation_root", device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - with pytest.raises(RuntimeError): - 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. - - 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 _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - cube_object, origins = generate_cubes_scene(num_cubes=1, device=device) - - # play the simulator - 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.composed_force.torch[i, 0, 0].item() == force - assert cube_object._permanent_wrench_composer.composed_torque.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.parametrize("num_cubes", [2, 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. - - In this test, 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 validate that this works when we apply the force in the global frame and in the local frame. - """ - # Generate cubes scene - with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - 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. PhysX reads the mass - # from ``root_view.get_masses()``; OVPhysX exposes the same value via - # ``cube_object.data.body_mass`` (shape ``(N, 1)``). - 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() - - is_global = False - if i % 2 == 0: - is_global = True - positions = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] - else: - positions = None - - # apply force - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=positions, - 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) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - cube_object.data.root_pos_w.torch[0::2, 2], torch.ones(num_cubes // 2, device=sim.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", [2, 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 _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - 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 - 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] = 500.0 - external_wrench_positions_b[0::2, :, 1] = 1.0 - - # Desired force and torque - desired_force = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) - desired_force[0::2, :, 2] = 1000.0 - desired_torque = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[0::2, :, 0] = 1000.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() - - is_global = False - if i % 2 == 0: - is_global = True - body_com_pos_w = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] - 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 - - # apply force - cube_object.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, - ) - cube_object.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, - ) - torch.testing.assert_close( - cube_object._permanent_wrench_composer.composed_force.torch[:, 0, :], - desired_force[:, 0, :], - rtol=1e-6, - atol=1e-7, - ) - torch.testing.assert_close( - cube_object._permanent_wrench_composer.composed_torque.torch[:, 0, :], - desired_torque[:, 0, :], - rtol=1e-6, - atol=1e-7, - ) - # 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) - - # 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) - # 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", [1, 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 _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - 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), - } - - # 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 - torch.testing.assert_close(value, expected_value, rtol=1e-3, atol=1e-3) - - cube_object.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset_rigid_object(num_cubes, device): - """Test resetting the state of the rigid object.""" - with _ovphysx_sim_context(device=device, gravity_enabled=True, auto_add_lighting=True) as sim: - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - 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) - - if i % 2 == 0: - # reset object - 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.composed_force.torch) == 0 - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_torque.torch) == 0 - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_set_material_properties(num_cubes, device): - """Test getting and setting per-shape material properties of a rigid object.""" - with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play sim - sim.reset() - - # Random material per shape: (static_friction, dynamic_friction, restitution), on the CPU. - num_shapes = _num_shapes(cube_object) - static_friction = torch.empty(num_cubes, num_shapes, 1, device="cpu").uniform_(0.4, 0.8) - dynamic_friction = torch.empty(num_cubes, num_shapes, 1, device="cpu").uniform_(0.4, 0.8) - restitution = torch.empty(num_cubes, num_shapes, 1, device="cpu").uniform_(0.0, 0.2) - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - # Add friction/restitution to the cubes through the view. - _write_shape_material(cube_object, materials) - - # Simulate physics - sim.step() - cube_object.update(sim.cfg.dt) - - # Read back and verify the round-trip. - torch.testing.assert_close(_read_shape_material(cube_object), materials) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_set_material_properties_via_view(num_cubes, device): - """Test setting per-shape material via the OvPhysxView binding API.""" - with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play sim - sim.reset() - - # Generate random material properties: (static_friction, dynamic_friction, restitution). - num_shapes = _num_shapes(cube_object) - materials = torch.empty(num_cubes, num_shapes, 3, device="cpu").uniform_(0.0, 1.0) - materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) # dynamic <= static - - # Set material properties through the view, simulate, then read back. - _write_shape_material(cube_object, materials) - sim.step() - cube_object.update(sim.cfg.dt) - - torch.testing.assert_close(_read_shape_material(cube_object), materials) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_no_friction(num_cubes, device): - """Test that a rigid object with no friction will maintain its velocity when sliding across a plane.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - - # Create a ground plane with no friction - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialBaseCfg( - static_friction=0.0, dynamic_friction=0.0, restitution=0.0 - ) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set the cubes' friction (and restitution) to zero. This test isolates friction, so a zero - # restitution keeps the resting contact clean instead of letting the cube bounce vertically. - num_shapes = _num_shapes(cube_object) - cube_materials = torch.zeros(num_cubes, num_shapes, 3, device="cpu") - _write_shape_material(cube_object, cube_materials) - - # Let the cube settle onto the plane so the initial ground-penetration transient (a small - # vertical velocity) dies out before we measure that the horizontal sliding velocity holds. - for _ in range(30): - sim.step() - cube_object.update(sim.cfg.dt) - - # Initial velocity in X to get the block moving. - initial_velocity = torch.zeros((num_cubes, 6), device=device) - initial_velocity[:, 0] = 0.1 - cube_object.write_root_velocity_to_sim_index(root_velocity=initial_velocity) - - # Non-deterministic on GPU, so use a looser tolerance there. - tolerance = 1e-2 if device == "cuda:0" else 1e-5 - for _ in range(5): - sim.step() - cube_object.update(sim.cfg.dt) - torch.testing.assert_close( - cube_object.data.root_lin_vel_w.torch, initial_velocity[:, :3], rtol=1e-5, atol=tolerance - ) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_with_static_friction(num_cubes, device): - """Test that static friction applied to rigid object works as expected. - - This test works by applying a force to the object and checking if the object moves or not based on the - mu (coefficient of static friction) value set for the object. We set the static friction to be non-zero and - apply a force to the object. When the force applied is below mu, the object should not move. When the force - applied is above mu, the object should move. - """ - with _ovphysx_sim_context(device=device, dt=0.01, auto_add_lighting=True) as sim: - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.03125, device=device) - - # Create ground plane. Dynamic friction is set equal to static friction to work around a PhysX bug. - mu = 0.5 - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialBaseCfg(static_friction=mu, dynamic_friction=mu) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set the cubes' static (and, per the PhysX bug, dynamic) friction to mu, restitution zero. - num_shapes = _num_shapes(cube_object) - cube_materials = torch.zeros(num_cubes, num_shapes, 3, device="cpu") - cube_materials[..., 0] = mu - cube_materials[..., 1] = mu - _write_shape_material(cube_object, cube_materials) - - # Let everything settle. - for _ in range(100): - sim.step() - cube_object.update(sim.cfg.dt) - cube_object.write_root_velocity_to_sim_index(root_velocity=torch.zeros((num_cubes, 6), device=device)) - - cube_mass = cube_object.data.body_mass.torch[:, 0] # [N], PhysX reads this via root_view.get_masses() - gravity_magnitude = abs(sim.cfg.gravity[2]) - # below mu: block should not move (applied force <= mu); above mu: block should move. - for force in "below_mu", "above_mu": - cube_object.write_root_velocity_to_sim_index(root_velocity=torch.zeros((num_cubes, 6), device=device)) - - external_wrench_b = torch.zeros((num_cubes, 1, 6), device=device) - factor = 0.99 if force == "below_mu" else 1.01 - external_wrench_b[:, 0, 0] = mu * cube_mass * gravity_magnitude * factor - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - ) - - initial_root_pos = cube_object.data.root_pos_w.torch.clone() - for _ in range(200): - cube_object.write_data_to_sim() - sim.step() - cube_object.update(sim.cfg.dt) - if force == "below_mu": - torch.testing.assert_close( - cube_object.data.root_pos_w.torch, initial_root_pos, rtol=2e-3, atol=2e-3 - ) - if force == "above_mu": - assert (cube_object.data.root_pos_w.torch[..., 0] - initial_root_pos[..., 0] > 0.02).all() - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_with_restitution(num_cubes, device): - """Test that restitution when applied to rigid object works as expected. - - This test works by dropping a block from a height and checking if the block bounces or not based on the - restitution value set for the object. We set the restitution to be non-zero and drop the block from a height. - When the restitution is 0, the block should not bounce. When the restitution is between 0 and 1, the block - should bounce with less energy. - """ - for expected_collision_type in "partially_elastic", "inelastic": - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - restitution_coefficient = 0.0 if expected_collision_type == "inelastic" else 0.5 - - # Create ground plane with the matching restitution. - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialBaseCfg(restitution=restitution_coefficient) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Drop the cubes from a height with an initial downward velocity. - root_pose = torch.zeros(num_cubes, 7, device=device) - root_pose[:, 3] = 1.0 # unit quaternion - for i in range(num_cubes): - root_pose[i, 1] = 1.0 * i - root_pose[:, 2] = 1.0 - root_vel = torch.zeros(num_cubes, 6, device=device) - root_vel[:, 2] = -1.0 - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # Frictionless cubes with the matching restitution. - num_shapes = _num_shapes(cube_object) - cube_materials = torch.zeros(num_cubes, num_shapes, 3, device="cpu") - cube_materials[..., 2] = restitution_coefficient - _write_shape_material(cube_object, cube_materials) - - curr_z_velocity = cube_object.data.root_lin_vel_w.torch[:, 2].clone() - prev_z_velocity = curr_z_velocity - for _ in range(100): - sim.step() - cube_object.update(sim.cfg.dt) - curr_z_velocity = cube_object.data.root_lin_vel_w.torch[:, 2].clone() - - if expected_collision_type == "inelastic": - # The block must not bounce: its z velocity stays <= 0. - assert (curr_z_velocity <= 0.0).all() - - if torch.all(curr_z_velocity <= 0.0): - prev_z_velocity = curr_z_velocity # still falling - else: - break # collision happened (now moving up) - - if expected_collision_type == "partially_elastic": - # The block bounced but lost energy. - assert torch.all(torch.le(abs(curr_z_velocity), abs(prev_z_velocity))) - assert (curr_z_velocity > 0.0).all() - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_rigid_body_set_mass(num_cubes, device): - """Test getting and setting mass of rigid object.""" - with _ovphysx_sim_context( - device=device, gravity_enabled=False, add_ground_plane=True, auto_add_lighting=True - ) as sim: - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - # Play sim - sim.reset() - - # Get masses before increasing - original_masses = cube_object.data.body_mass.torch.clone() - - assert original_masses.shape == (num_cubes, 1) - - # Randomize mass of the object - masses = original_masses + torch.FloatTensor(num_cubes, 1).uniform_(4, 8).to(sim.device) - - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - # Set the new masses via the OVPhysX writer (matches PhysX/Newton). - cube_object.set_masses_index( - masses=wp.from_torch(masses.contiguous(), dtype=wp.float32), - env_ids=wp.from_torch(indices, dtype=wp.int32), +def _spawn_rigid_objects() -> RigidObject: + """Author two local cuboids for partial-write and wrench proofs.""" + for index in range(2): + sim_utils.create_prim(f"/World/Env_{index}", "Xform", translation=(2.0 * index, 0.0, 0.0)) + return RigidObject( + RigidObjectCfg( + prim_path="/World/Env_[^/]*/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), ) + ) - torch.testing.assert_close(cube_object.data.body_mass.torch, masses) - - # Simulate physics - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - masses_to_check = cube_object.data.body_mass.torch - - # Check if mass is set correctly - torch.testing.assert_close(masses, masses_to_check) - - -@pytest.mark.parametrize("num_cubes", [1, 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 _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled) as sim: - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Obtain gravity direction - if gravity_enabled: - gravity_dir = (0.0, 0.0, -1.0) - else: - gravity_dir = (0.0, 0.0, 0.0) - - # Play sim - sim.reset() - - # Check that gravity is set correctly - assert cube_object.data.GRAVITY_VEC_W.torch[0, 0] == gravity_dir[0] - assert cube_object.data.GRAVITY_VEC_W.torch[0, 1] == gravity_dir[1] - assert cube_object.data.GRAVITY_VEC_W.torch[0, 2] == gravity_dir[2] - - # 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.parametrize("num_cubes", [1, 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): - """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" - with _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - # Play sim +def test_rigid_object_real_ovphysx_seams() -> None: + """Prove partial state, inertial properties, and one real wrench delivery.""" + with _sim_context() as sim: + rigid_object = _spawn_rigid_objects() sim.reset() - # Check if cube_object is initialized - assert cube_object.is_initialized + assert rigid_object.is_initialized + assert rigid_object.num_instances == 2 + assert rigid_object.data.body_mass.torch.shape == (2, 1) + assert rigid_object.data.body_com_pose_b.torch.shape == (2, 1, 7) + assert rigid_object.data.body_inertia.torch.shape == (2, 1, 9) - # 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) + initial_pose = rigid_object.data.root_link_pose_w.torch.clone() + target_pose = initial_pose[1:2].clone() + target_pose[0, :3] += torch.tensor([0.25, -0.1, 0.2]) + rigid_object.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=[1]) + torch.testing.assert_close(rigid_object.data.root_link_pose_w.torch[1:2], target_pose) + torch.testing.assert_close(rigid_object.data.root_link_pose_w.torch[0:1], initial_pose[0:1]) - # Read current COMs, mutate the translation, write back via the OVPhysX - # ``set_coms_index`` setter (PhysX uses ``root_view.set_coms`` for the same - # operation; OVPhysX wraps the wheel ``RIGID_BODY_COM_POSE`` write in - # :meth:`set_coms_index`, which follows the PhysX ``wp.transformf`` contract). - com = cube_object.data.body_com_pose_b.torch.clone() # shape (N, 1, 7) - com[..., :3] = offset.to(com.device).unsqueeze(1) - cube_object.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), + raw_mass_before = wp.to_torch(rigid_object.root_view.get_attribute(TT.RIGID_BODY_MASS)).clone() + rigid_object.set_masses_index(masses=wp.array([[3.0]], dtype=wp.float32, device="cpu"), env_ids=[1]) + expected_raw_mass = raw_mass_before.clone() + expected_raw_mass[1] = 3.0 + torch.testing.assert_close( + wp.to_torch(rigid_object.root_view.get_attribute(TT.RIGID_BODY_MASS)), expected_raw_mass ) - # check ceter of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # Simulate physics - for _ in range(100): - # spin the object around Z axis (com) - cube_object.write_root_velocity_to_sim_index(root_velocity=spin_twist.repeat(num_cubes, 1)) - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # get state properties - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - 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_link_pose_w = cube_object.data.body_link_pose_w.torch - body_link_vel_w = cube_object.data.body_link_vel_w.torch - 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, root_link_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) - torch.testing.assert_close(body_link_pose_w, body_link_pose_w) - 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) - torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3]) - torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2)) - # 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) - 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)) - - # 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:]) - torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:]) - - # orientation of link will match root state will always match - torch.testing.assert_close(root_link_pose_w[..., 3:], root_link_pose_w[..., 3:]) - torch.testing.assert_close(body_link_pose_w[..., 3:], body_link_pose_w[..., 3:]) - - # 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]) - torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3]) - # link frame will be moving, and should account for the reported COM velocity and 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]) - com_lin_vel_rel_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_com_vel_w[..., :3]) - com_ang_vel_rel_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_com_vel_w[..., 3:]) - lin_vel_rel_gt = com_lin_vel_rel_gt + torch.linalg.cross(com_ang_vel_rel_gt, -offset) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, atol=1e-4, rtol=1e-4) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), atol=1e-4, rtol=1e-4) - - # ang_vel will always match - torch.testing.assert_close(root_com_vel_w[..., 3:], root_com_vel_w[..., 3:]) - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_com_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@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.""" - with _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - # Play sim - 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) - - com = cube_object.data.body_com_pose_b.torch.clone() # shape (N, 1, 7) - com[..., :3] = offset.to(com.device).unsqueeze(1) - cube_object.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), + raw_com_before = wp.to_torch(rigid_object.root_view.get_attribute(TT.RIGID_BODY_COM_POSE)).clone() + coms = rigid_object.data.body_com_pose_b.torch[1:2].clone() + coms[0, 0, :3] = torch.tensor([0.01, -0.02, 0.03]) + rigid_object.set_coms_index(coms=wp.from_torch(coms, dtype=wp.transformf), env_ids=[1]) + expected_raw_com = raw_com_before.clone() + expected_raw_com[1] = coms[0, 0] + torch.testing.assert_close( + wp.to_torch(rigid_object.root_view.get_attribute(TT.RIGID_BODY_COM_POSE)), expected_raw_com ) - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) - - 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) - - env_idx = env_idx.to(device) - for i in range(10): - # perform step - sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - 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) - 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 - ) - - 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) - - -@pytest.mark.parametrize("num_cubes", [1, 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 _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - # Play sim - 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) - - com = cube_object.data.body_com_pose_b.torch.clone() # shape (N, 1, 7) - com[..., :3] = offset.to(com.device).unsqueeze(1) - cube_object.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), + raw_inertia_before = wp.to_torch(rigid_object.root_view.get_attribute(TT.RIGID_BODY_INERTIA)).clone() + inertias = rigid_object.data.body_inertia.torch[1:2].clone() + inertias[0, 0, 0] *= 1.5 + rigid_object.set_inertias_index(inertias=wp.from_torch(inertias, dtype=wp.float32), env_ids=[1]) + expected_raw_inertia = raw_inertia_before.clone() + expected_raw_inertia[1] = inertias[0, 0] + torch.testing.assert_close( + wp.to_torch(rigid_object.root_view.get_attribute(TT.RIGID_BODY_INERTIA)), expected_raw_inertia ) - - # check ceter of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) - - 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) - - env_idx = env_idx.to(device) - - # perform step + torch.testing.assert_close(rigid_object.data.body_mass.torch[:, 0], torch.tensor([1.0, 3.0])) + torch.testing.assert_close(rigid_object.data.body_com_pose_b.torch[1:2], coms) + torch.testing.assert_close(rigid_object.data.body_inertia.torch[1:2], inertias) + + initial_velocity = rigid_object.data.root_com_vel_w.torch.clone() + forces = torch.zeros((2, 1, 3)) + forces[1, 0, 0] = 20.0 + rigid_object.instantaneous_wrench_composer.set_forces_and_torques_index(forces=forces, is_global=True) + rigid_object.write_data_to_sim() sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - 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:]) - - -def test_warmup_attach_stage_not_called_for_cpu(): - """Regression test: ``physx.warmup_gpu()`` must not be called for CPU. - - OVPhysX-equivalent of PhysX's ``test_warmup_attach_stage_not_called_for_cpu``: - PhysX guards :meth:`attach_stage` with ``if is_gpu:`` so the CPU MBP - broadphase is not double-initialised. The OVPhysX manager has the same - structural guard around :meth:`OvPhysxManager._physx.warmup_gpu`: it is - only invoked when ``ovphysx_device == "gpu"``. + rigid_object.update(sim.cfg.dt) - We monkey-patch ``OvPhysxManager._physx`` with a :class:`MagicMock` - wrapping the live PhysX object so that ``warmup_gpu`` becomes a spy while - other calls continue to forward, then assert ``warmup_gpu.call_count == 0`` - after a CPU-mode :meth:`sim.reset`. - - The test always runs CPU regardless of session parametrization, so it is - skipped when the session-locked device is anything other than CPU. The - skip is enforced inline (rather than in the autouse fixture) so the rest - of the suite can still pin to GPU when invoked together. - """ - if _LOCKED_DEVICE[0] not in (None, "cpu"): - pytest.skip( - f"ovphysx process-global device lock is held by '{_LOCKED_DEVICE[0]}'; cannot run " - "CPU-only regression test in the same session." - ) - _LOCKED_DEVICE[0] = "cpu" - - with _ovphysx_sim_context(device="cpu", add_ground_plane=True, dt=0.01, auto_add_lighting=True) as sim: - # Allocate a single rigid body so the manager has something to load. - generate_cubes_scene(num_cubes=1, height=1.0, device="cpu") - - # First reset constructs (or reuses) the real ovphysx.PhysX so we have - # a live instance to wrap. The PhysX object is a C++ binding, so we - # cannot patch attributes directly — replace the class-level reference - # with a MagicMock(wraps=...) that forwards every call. - sim.reset() - original_physx = OvPhysxManager._physx - assert original_physx is not None, "PhysX should be constructed after sim.reset()" - spy = MagicMock(wraps=original_physx) - OvPhysxManager._physx = spy - # Force _warmup_and_load to run again on the next reset so the spy - # observes the warmup_gpu (or non-call) decision; close() resets - # _warmup_done back to False but we just called sim.reset() above. - OvPhysxManager._warmup_done = False - try: - sim.reset() - finally: - OvPhysxManager._physx = original_physx - - assert spy.warmup_gpu.call_count == 0, ( - f"warmup_gpu() was called {spy.warmup_gpu.call_count} time(s) during CPU warmup. " - "OvPhysxManager._warmup_and_load() must guard warmup_gpu() with " - "ovphysx_device == 'gpu' so the CPU pipeline is not mis-initialised." - ) + assert rigid_object.data.root_com_vel_w.torch[1, 0] > initial_velocity[1, 0] + torch.testing.assert_close(rigid_object.data.root_com_vel_w.torch[0], initial_velocity[0], atol=1e-6, rtol=0.0) diff --git a/source/isaaclab_ov/test/assets/test_rigid_object_collection.py b/source/isaaclab_ov/test/assets/test_rigid_object_collection.py index 846636abb855..b7e319cef170 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object_collection.py @@ -3,35 +3,14 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Real-backend tests for the OVPhysX RigidObjectCollection. - -Run via ``./scripts/run_ovphysx.sh -m pytest`` (kitless, no ``AppLauncher``). - -The OVPhysX runtime fixes device mode (CPU vs GPU) when the process creates -its first ``ovphysx.PhysX`` instance and cannot switch it without a process -restart. Full coverage therefore requires two separate pytest -invocations -- once with ``-k 'cpu'`` and once with ``-k 'cuda:0'``. The -``_ovphysx_skip_other_device`` autouse fixture below preempts the manager's -:exc:`RuntimeError` by ``pytest.skip``-ing on the unlocked device so -single-device runs finish cleanly. -""" +"""Minimal real-OVPhysX integration coverage for rigid-object collections.""" from __future__ import annotations -import sys - import pytest import torch import warp as wp -from isaaclab.test.utils import test_devices - -# The OVPhysX runtime wheel is optional. Skip gracefully when it is not installed; -# CI jobs that need OVPhysX coverage install it explicitly. pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") from isaaclab_ov import tensor_types as TT # noqa: E402 @@ -41,932 +20,115 @@ import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.assets import RigidObjectCfg, RigidObjectCollectionCfg # noqa: E402 from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 -from isaaclab.utils.math import ( # noqa: E402 - combine_frame_transforms, - default_orientation, - quat_apply_inverse, - quat_inv, - quat_mul, - quat_rotate, - random_orientation, - subtract_frame_transforms, -) - -wp.init() - -pytestmark = pytest.mark.device_split - - -_LOCKED_DEVICE: list[str | None] = [None] -"""Device the session pins to on the first parametrized test that runs.""" - -@pytest.fixture(autouse=True) -def _ovphysx_skip_other_device(request): - """Skip parametrized tests on the device the session is not pinned to. +pytestmark = pytest.mark.integration - See the module docstring for the wheel's process-global device-mode lock. - """ - callspec = getattr(request.node, "callspec", None) - device = callspec.params.get("device") if callspec is not None else None - if device is None: - # Test does not parametrize on device. - return - locked = _LOCKED_DEVICE[0] - if locked is None: - _LOCKED_DEVICE[0] = device - return - if device != locked: - pytest.skip( - f"ovphysx process-global device lock is held by '{locked}'; cannot run '{device}' " - "tests in the same session. Run pytest twice (once per device) for full coverage." - ) - - -def _ovphysx_sim_context(device: str, **kwargs): - """Wrapper around :func:`build_simulation_context` that injects OVPhysX cfg. - - PhysX tests pass ``device=device`` directly and let - :func:`build_simulation_context` build a default :class:`SimulationCfg`. - OVPhysX needs ``physics=OvPhysxCfg()`` set on the cfg so the manager - dispatches to OVPhysX rather than PhysX, so we build the cfg here and - pass it through. ``gravity_enabled`` is consumed locally (it is ignored - by ``build_simulation_context`` once a ``sim_cfg`` is provided). - ``add_ground_plane``, ``auto_add_lighting``, and other kwargs continue - to flow through ``build_simulation_context`` as before. - """ - dt = kwargs.pop("dt", 1.0 / 60.0) - gravity_enabled = kwargs.pop("gravity_enabled", True) - gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - sim_cfg = SimulationCfg(physics=OvPhysxCfg(), device=device, dt=dt, gravity=gravity) - return build_simulation_context(device=device, sim_cfg=sim_cfg, **kwargs) - - -def generate_cubes_scene( - num_envs: int = 1, - num_cubes: int = 1, - height=1.0, - has_api: bool = True, - kinematic_enabled: bool = False, - device: str = "cuda:0", -) -> tuple[RigidObjectCollection, torch.Tensor]: - """Generate a scene with the provided number of cubes. - Args: - num_envs: Number of envs to generate. - num_cubes: Number of cubes to generate. - height: Height of the cubes. - has_api: Whether the cubes have a rigid body API on them. - kinematic_enabled: Whether the cubes are kinematic. - device: Device to use for the simulation. +def _sim_context(): + """Build a local CPU OVPhysX context from an in-memory USD stage.""" + return build_simulation_context( + sim_cfg=SimulationCfg(physics=OvPhysxCfg(), device="cpu", gravity=(0.0, 0.0, 0.0)), + auto_add_lighting=False, + ) - Returns: - A tuple containing the rigid object representing the cubes and the origins of the cubes. - """ - origins = torch.tensor([(i * 3.0, 0, height) for i in range(num_envs)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) - - # Resolve spawn configuration - if has_api: - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - else: - # since no rigid body properties defined, this is just a static collider - spawn_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), +def _cube_cfg(prim_path: str, y: float) -> RigidObjectCfg: + """Create one local collection body.""" + return RigidObjectCfg( + prim_path=prim_path, + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), collision_props=sim_utils.CollisionPropertiesCfg(), - ) - - # create the rigid object configs. OVPhysX matches prim paths via fnmatch globs (not regex), - # so use ``Table_*`` rather than the PhysX ``Table_.*`` form. - cube_config_dict = {} - for i in range(num_cubes): - cube_object_cfg = RigidObjectCfg( - prim_path=f"/World/Table_[^/]+/Object_{i}", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 3 * i, height)), - ) - cube_config_dict[f"cube_{i}"] = cube_object_cfg - # create the rigid object collection - cube_object_collection_cfg = RigidObjectCollectionCfg(rigid_objects=cube_config_dict) - cube_object_colection = RigidObjectCollection(cfg=cube_object_collection_cfg) - - return cube_object_colection, origins - - -@pytest.mark.parametrize("num_envs", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 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 _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - 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 - - # Play sim - sim.reset() - - # Check if object is initialized - assert object_collection.is_initialized - assert len(object_collection.body_names) == num_cubes - - # Check buffers that exist and have correct shapes - 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_id_conversion(device): - """Test environment and object index conversion to physics view indices.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, _ = generate_cubes_scene(num_envs=2, num_cubes=3, device=device) - - # Play sim - sim.reset() - - expected = [ - torch.tensor([4, 5], device=device, dtype=torch.int32), - torch.tensor([4], device=device, dtype=torch.int32), - torch.tensor([0, 2, 4], device=device, dtype=torch.int32), - torch.tensor([1, 3, 5], device=device, dtype=torch.int32), - ] - - torch_all_env_indices = wp.to_torch(object_collection._ALL_ENV_INDICES) - torch_all_body_indices = wp.to_torch(object_collection._ALL_BODY_INDICES) - - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices, torch_all_body_indices[None, 2], device=device - ) - assert (wp.to_torch(view_ids) == expected[0]).all() - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices[None, 0], torch_all_body_indices[None, 2], device=device - ) - assert (wp.to_torch(view_ids) == expected[1]).all() - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices[None, 0], torch_all_body_indices, device=device - ) - assert (wp.to_torch(view_ids) == expected[2]).all() - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices[None, 1], torch_all_body_indices, device=device - ) - assert (wp.to_torch(view_ids) == expected[3]).all() - - -@pytest.mark.parametrize("num_envs", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 3]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_kinematic_enabled(num_envs, num_cubes, device): - """Test that initialization for prim with kinematic flag enabled.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, origins = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, kinematic_enabled=True, device=device - ) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(object_collection) < 10 - - # Play sim - sim.reset() - - # Check if object is initialized - assert object_collection.is_initialized - assert len(object_collection.body_names) == num_cubes - - # Check buffers that exist and have correct shapes - 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) - - # Simulate physics - for _ in range(2): - sim.step() - object_collection.update(sim.cfg.dt) - # check that the object is kinematic - default_body_pose = object_collection.data.default_body_pose.torch.clone() - default_body_vel = object_collection.data.default_body_vel.torch.clone() - default_body_pose[..., :3] += origins.unsqueeze(1) - torch.testing.assert_close(object_collection.data.body_link_pose_w.torch, default_body_pose) - torch.testing.assert_close(object_collection.data.body_link_vel_w.torch, default_body_vel) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_no_rigid_body(num_cubes, device): - """Test that initialization fails when no rigid body is found at the provided prim path.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, _ = generate_cubes_scene(num_cubes=num_cubes, has_api=False, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(object_collection) < 10 - - # Play sim - with pytest.raises(RuntimeError): - 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.""" - num_envs = 2 - num_cubes = 1 - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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.composed_force.torch[i, 0, 0].item() == force - assert object_collection._permanent_wrench_composer.composed_torque.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", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 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.""" - with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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 - 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 - 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() - - is_global = False - if i % 2 == 0: - positions = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] - is_global = True - else: - positions = None - - # apply force - object_collection.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=positions, - body_ids=object_ids, - 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) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - object_collection.data.body_link_pos_w.torch[:, 0::2, 2], - torch.ones_like(object_collection.data.body_link_pos_w.torch[:, 0::2, 2]), - ) - # 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", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 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 _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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 - 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] = 500.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() - - is_global = False - if i % 2 == 0: - body_com_pos_w = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] - 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 - is_global = True - else: - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - - # apply force - object_collection.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=object_ids, - env_ids=None, - is_global=is_global, - ) - object_collection.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=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) - - # 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) - # 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", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_set_object_state(num_envs, num_cubes, device, gravity_enabled): - """Test setting the state of the object. - - .. note:: - Turn off gravity for this test as we don't want any external forces acting on the object - to ensure state remains static - """ - with _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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 - ), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, y, 1.0)), + ) + + +def _spawn_collection() -> RigidObjectCollection: + """Author the canonical N=2, B=3 local collection.""" + for env_index in range(2): + sim_utils.create_prim(f"/World/Env_{env_index}", "Xform", translation=(3.0 * env_index, 0.0, 0.0)) + return RigidObjectCollection( + RigidObjectCollectionCfg( + rigid_objects={ + "left": _cube_cfg("/World/Env_[^/]*/Object_0", 0.0), + "middle": _cube_cfg("/World/Env_[^/]*/Object_1", 1.0), + "right": _cube_cfg("/World/Env_[^/]*/Object_2", 2.0), } - - 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 - torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) - - object_collection.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_envs", [1, 4]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_object_state_properties(num_envs, num_cubes, device, with_offset, gravity_enabled): - """Test the object_com_state_w and object_link_state_w properties.""" - with _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - 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) - - 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) ) + ) - # Read current COMs, mutate the translation, write back via the OVPhysX - # ``set_coms_index`` setter (PhysX uses ``root_view.set_coms`` + reshape helpers - # for the same operation; OVPhysX wraps the wheel RIGID_BODY_COM_POSE write in - # :meth:`set_coms_index`). - com = cube_object.data.body_com_pose_b.torch.clone() # shape (num_envs, num_cubes, 7) - com[..., :3] = offset.to(com.device) - cube_object.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_ids, dtype=wp.int32), - ) - - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # initial spawn point - init_com = cube_object.data.body_com_pose_w.torch[..., :3] - - for i in range(10): - # spin the object around Z axis (com) - cube_object.write_body_com_velocity_to_sim_index(body_velocities=spin_twist.repeat(num_envs, num_cubes, 1)) - sim.step() - cube_object.update(sim.cfg.dt) - - # get state properties - object_link_pose_w = cube_object.data.body_link_pose_w.torch - object_link_vel_w = cube_object.data.body_link_vel_w.torch - 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: - # 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]) - - # 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], - ) - - torch.testing.assert_close(-offset, object_link_state_pos_rel_com) - - # 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:]) - - # orientation of link will match object state will always match - torch.testing.assert_close(object_link_pose_w[..., 3:], object_link_pose_w[..., 3:]) - - # 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], - ) - - # 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, atol=1e-4, rtol=1e-3) - - # ang_vel will always match - torch.testing.assert_close(object_com_vel_w[..., 3:], object_com_vel_w[..., 3:]) - torch.testing.assert_close(object_com_vel_w[..., 3:], object_link_vel_w[..., 3:]) - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 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]) -def test_write_object_state(num_envs, num_cubes, device, with_offset, state_location, gravity_enabled): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - with _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - # 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) +def test_rigid_object_collection_real_ovphysx_seams() -> None: + """Prove fused remapping through partial state, inertial, and material writes.""" + with _sim_context() as sim: + collection = _spawn_collection() 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) - ) - - com = cube_object.data.body_com_pose_b.torch.clone() # shape (num_envs, num_cubes, 7) - com[..., :3] = offset.to(com.device) - cube_object.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_ids, dtype=wp.int32), - ) - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) - - 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): - 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 - ) - - 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) - 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) - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 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 _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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.composed_force.torch) == 0 - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_torque.torch) == 0 - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_set_material_properties(num_envs, num_cubes, device): - """Test getting and setting per-shape material properties of a rigid object collection. - - OVPhysX exposes per-collision-shape material as the - ``rigid_body_shape_friction_and_restitution`` tensor binding, addressed through the - fused :class:`~isaaclab_ov.sim.views.OvPhysxView`. The binding is body-major flat, - so data ``[num_envs, num_cubes, 3]`` is mapped to/from the view layout with - :meth:`~isaaclab_ov.assets.RigidObjectCollection.reshape_data_to_view_3d` and its - inverse. (The PhysX backend uses ``root_view.get/set_material_properties``.) - """ - with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - - # Play sim - sim.reset() - - # Random material per object: (static_friction, dynamic_friction, restitution), on the CPU. - static_friction = torch.empty(num_envs, num_cubes, 1, device="cpu").uniform_(0.4, 0.8) - dynamic_friction = torch.empty(num_envs, num_cubes, 1, device="cpu").uniform_(0.4, 0.8) - restitution = torch.empty(num_envs, num_cubes, 1, device="cpu").uniform_(0.0, 0.2) - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) # [num_envs, num_cubes, 3] - - # Map data -> body-major view layout, write through the view. - view_materials = object_collection.reshape_data_to_view_3d(wp.from_torch(materials, dtype=wp.float32), 3) - object_collection.root_view.set_attribute(TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION, view_materials) - - # Simulate physics - sim.step() - object_collection.update(sim.cfg.dt) - - # Read back and map view -> data layout (inverse of reshape_data_to_view_3d), then verify. - view_check = wp.to_torch( - object_collection.root_view.get_attribute(TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION) - ) - materials_check = view_check.reshape(num_cubes, num_envs, 3).transpose(0, 1).contiguous() - torch.testing.assert_close(materials_check, materials) - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 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 _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - - # Obtain gravity direction - gravity_dir = (0.0, 0.0, -1.0) if gravity_enabled else (0.0, 0.0, 0.0) - - sim.reset() - - # Check if gravity vector is set correctly - gravity_vec = object_collection.data.GRAVITY_VEC_W.torch - assert gravity_vec[0, 0, 0] == gravity_dir[0] - assert gravity_vec[0, 0, 1] == gravity_dir[1] - assert gravity_vec[0, 0, 2] == gravity_dir[2] - - # Perform simulation - 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.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 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]) -def test_write_object_state_functions_data_consistency( - num_envs, num_cubes, device, with_offset, state_location, gravity_enabled -): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - with _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - # 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) - - 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) - ) - - com = cube_object.data.body_com_pose_b.torch.clone() # shape (num_envs, num_cubes, 7) - com[..., :3] = offset.to(com.device) - cube_object.set_coms_index( - coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_ids, dtype=wp.int32), - ) - - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) - - 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:]) + assert collection.is_initialized + assert collection.num_instances == 2 + assert collection.body_names == ["left", "middle", "right"] + assert collection.data.body_mass.torch.shape == (2, 3) + + env_ids = torch.tensor([1, 0], dtype=torch.int32) + body_ids = torch.tensor([2, 0], dtype=torch.int32) + initial_pose = collection.data.body_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids][:, body_ids].clone() + target_pose[0, 0, :3] += torch.tensor([0.2, 0.3, 0.4]) + target_pose[1, 1, :3] += torch.tensor([-0.1, -0.2, 0.1]) + collection.write_body_link_pose_to_sim_index(body_poses=target_pose, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(collection.data.body_link_pose_w.torch[env_ids][:, body_ids], target_pose) + torch.testing.assert_close(collection.data.body_link_pose_w.torch[:, 1], initial_pose[:, 1]) + + initial_mass = collection.data.body_mass.torch.clone() + raw_mass_before = wp.to_torch(collection.root_view.get_attribute(TT.BODY_MASS)).reshape(3, 2).T.clone() + masses = torch.tensor([[5.0, 6.0], [7.0, 8.0]]) + collection.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + expected_raw_mass = raw_mass_before.clone() + expected_raw_mass[env_ids[:, None], body_ids[None, :]] = masses + raw_mass = wp.to_torch(collection.root_view.get_attribute(TT.BODY_MASS)).reshape(3, 2).T + torch.testing.assert_close(raw_mass, expected_raw_mass) + torch.testing.assert_close(collection.data.body_mass.torch[env_ids][:, body_ids], masses) + torch.testing.assert_close(collection.data.body_mass.torch[:, 1], initial_mass[:, 1]) + + raw_com_before = ( + wp.to_torch(collection.root_view.get_attribute(TT.BODY_COM_POSE)).reshape(3, 2, 7).transpose(0, 1).clone() + ) + coms = collection.data.body_com_pose_b.torch[env_ids][:, body_ids].clone() + coms[..., :3] = torch.tensor( + [[[0.01, 0.02, 0.03], [-0.01, 0.03, 0.02]], [[0.02, -0.01, 0.01], [0.03, 0.01, -0.02]]] + ) + collection.set_coms_index(coms=coms, env_ids=env_ids, body_ids=body_ids) + expected_raw_com = raw_com_before.clone() + expected_raw_com[env_ids[:, None], body_ids[None, :]] = coms + raw_com = wp.to_torch(collection.root_view.get_attribute(TT.BODY_COM_POSE)).reshape(3, 2, 7).transpose(0, 1) + torch.testing.assert_close(raw_com, expected_raw_com) + torch.testing.assert_close(collection.data.body_com_pose_b.torch[env_ids][:, body_ids], coms) + + raw_inertia_before = ( + wp.to_torch(collection.root_view.get_attribute(TT.BODY_INERTIA)).reshape(3, 2, 9).transpose(0, 1).clone() + ) + inertias = collection.data.body_inertia.torch[env_ids][:, body_ids].clone() + inertias[..., 0] *= 1.2 + inertias[..., 4] *= 1.3 + collection.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) + expected_raw_inertia = raw_inertia_before.clone() + expected_raw_inertia[env_ids[:, None], body_ids[None, :]] = inertias + raw_inertia = wp.to_torch(collection.root_view.get_attribute(TT.BODY_INERTIA)).reshape(3, 2, 9).transpose(0, 1) + torch.testing.assert_close(raw_inertia, expected_raw_inertia) + torch.testing.assert_close(collection.data.body_inertia.torch[env_ids][:, body_ids], inertias) + + materials = torch.tensor( + [ + [[0.9, 0.4, 0.1], [0.8, 0.3, 0.2], [0.7, 0.2, 0.3]], + [[0.6, 0.1, 0.4], [0.5, 0.2, 0.1], [0.4, 0.3, 0.2]], + ] + ) + fused_materials = collection.reshape_data_to_view_3d( + wp.from_torch(materials, dtype=wp.float32), 3, device="cpu" + ) + collection.root_view.set_attribute(TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION, fused_materials) + raw_materials = wp.to_torch(collection.root_view.get_attribute(TT.RIGID_BODY_SHAPE_FRICTION_AND_RESTITUTION)) + torch.testing.assert_close(raw_materials.reshape(3, 2, 3).transpose(0, 1), materials) diff --git a/source/isaaclab_ov/test/assets/test_articulation_helpers.py b/source/isaaclab_ov/test/assets/unit/test_articulation_helpers.py similarity index 84% rename from source/isaaclab_ov/test/assets/test_articulation_helpers.py rename to source/isaaclab_ov/test/assets/unit/test_articulation_helpers.py index 48122a8662c1..7186eff284ec 100644 --- a/source/isaaclab_ov/test/assets/test_articulation_helpers.py +++ b/source/isaaclab_ov/test/assets/unit/test_articulation_helpers.py @@ -23,6 +23,8 @@ # CI jobs that need OVPhysX coverage install it explicitly. pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") +pytestmark = pytest.mark.unit + from isaaclab_ov.assets.articulation.articulation import Articulation # noqa: E402 from isaaclab_ov.physics import OvPhysxManager # noqa: E402 from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet # noqa: E402 @@ -118,27 +120,6 @@ def test_process_tendons_scopes_to_articulation_root(): assert articulation.spatial_tendon_names == ["spatial_joint"] -def test_mock_binding_set_rigid_object_shapes(): - pytest.importorskip("isaaclab_ov.tensor_types").RIGID_BODY_POSE # gates on wheel - from isaaclab_ov import tensor_types as TT - from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet - - bindings = MockOvPhysxBindingSet( - num_instances=4, - num_joints=0, - num_bodies=1, - asset_kind="rigid_object", - ) - assert bindings.bindings[TT.RIGID_BODY_POSE].shape == (4, 7) - assert bindings.bindings[TT.RIGID_BODY_VELOCITY].shape == (4, 6) - assert bindings.bindings[TT.RIGID_BODY_WRENCH].shape == (4, 9) - assert bindings.bindings[TT.RIGID_BODY_MASS].shape == (4,) - assert bindings.bindings[TT.RIGID_BODY_INERTIA].shape == (4, 9) - # Articulation-only bindings must be absent - assert TT.DOF_POSITION not in bindings.bindings - assert TT.LINK_WRENCH not in bindings.bindings - - def test_mock_binding_read_preserves_structured_warp_dtype(): """Mock bindings should read flat component data into structured Warp arrays.""" from isaaclab_ov import tensor_types as TT diff --git a/source/isaaclab_ov/test/assets/test_articulation_kernels.py b/source/isaaclab_ov/test/assets/unit/test_articulation_kernels.py similarity index 99% rename from source/isaaclab_ov/test/assets/test_articulation_kernels.py rename to source/isaaclab_ov/test/assets/unit/test_articulation_kernels.py index a8c6b79f8f5e..177eadf5d0be 100644 --- a/source/isaaclab_ov/test/assets/test_articulation_kernels.py +++ b/source/isaaclab_ov/test/assets/unit/test_articulation_kernels.py @@ -13,6 +13,8 @@ import warp as wp from isaaclab_ov.assets import kernels +pytestmark = pytest.mark.unit + def _selector(values: list[int], dtype: type) -> wp.array: return wp.array(values, dtype=dtype, device="cpu") diff --git a/source/isaaclab_ov/test/assets/test_deformable_views.py b/source/isaaclab_ov/test/assets/unit/test_deformable_views.py similarity index 99% rename from source/isaaclab_ov/test/assets/test_deformable_views.py rename to source/isaaclab_ov/test/assets/unit/test_deformable_views.py index e521784d303f..5ba39643502e 100644 --- a/source/isaaclab_ov/test/assets/test_deformable_views.py +++ b/source/isaaclab_ov/test/assets/unit/test_deformable_views.py @@ -13,6 +13,8 @@ pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") +pytestmark = pytest.mark.unit + import warp as wp # noqa: E402 from isaaclab_ov.assets.deformable_object.views import OvPhysxDeformableBodyView # noqa: E402 from isaaclab_ov.sim.views import OvPhysxView # noqa: E402 diff --git a/source/isaaclab_ov/test/assets/unit/test_ovphysx_actuator_control.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_actuator_control.py new file mode 100644 index 000000000000..8d8f20b3d5bd --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_actuator_control.py @@ -0,0 +1,220 @@ +# 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 + +"""Focused tests for OVPhysX actuator command and native-runtime adaptation.""" + +from types import SimpleNamespace +from unittest.mock import Mock, call + +import pytest +import warp as wp +from isaaclab_ov import tensor_types as TT +from isaaclab_ov.assets.articulation import actuator_control as actuator_control_module +from isaaclab_ov.assets.articulation.actuator_control import OvPhysxActuatorControl + +from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg + +pytestmark = pytest.mark.unit + + +class _RecordingView: + """Capture values written to each OVPhysX tensor type.""" + + def __init__(self) -> None: + self.values = {} + self.kwargs = {} + + def set_attribute(self, tensor_type, values, **kwargs) -> None: + self.values[tensor_type] = values + self.kwargs[tensor_type] = kwargs + + +def _buffer(value: float) -> wp.array: + return wp.full((1, 2), value, dtype=wp.float32, device="cpu") + + +def test_prepare_and_finalize_native_actuators_own_runtime_boundary(monkeypatch) -> None: + """OV preparation must classify groups and expose the shared runtime selection.""" + calls = [] + wrapper = SimpleNamespace() + native_actuator = object() + adapter = SimpleNamespace(actuators=[native_actuator]) + stage = object() + + class _Runtime: + def __init__(self, owner, *, logger): + calls.append(("construct", owner)) + self.wrapper = wrapper + self.adapter = adapter + + def prepare(self, collection, **kwargs) -> None: + calls.append(("prepare", collection, kwargs)) + + def finalize(self, collection) -> None: + calls.append(("finalize", collection)) + + monkeypatch.setattr(actuator_control_module, "_validate_newton_native_actuator_cfgs", lambda cfgs: None) + monkeypatch.setattr(actuator_control_module, "find_first_matching_prim", lambda path: None) + monkeypatch.setattr(actuator_control_module, "get_current_stage", lambda: stage) + monkeypatch.setattr(actuator_control_module, "PhysxActuatorRuntime", _Runtime) + articulation = SimpleNamespace( + _sim_cfg=SimpleNamespace(use_newton_actuators=True), + cfg=SimpleNamespace(prim_path="/World/Robot"), + num_instances=2, + num_joints=3, + num_fixed_tendons=0, + device="cpu", + ) + collection = SimpleNamespace() + control = OvPhysxActuatorControl(articulation) + + native_groups = control.prepare_native_actuators( + collection, + { + "implicit": ImplicitActuatorCfg(joint_names_expr=["joint_0"], stiffness=2.0, damping=0.2), + "explicit": IdealPDActuatorCfg(joint_names_expr=["joint_[12]"], stiffness=3.0, damping=0.3), + }, + ) + selection = control.finalize_native_actuators(collection) + + assert native_groups == {"explicit"} + assert control.native_actuator_path_active + assert articulation._has_newton_actuators + assert articulation._physx_actuator_wrapper is wrapper + assert articulation.newton_actuator_adapter is adapter + assert selection is not None + assert selection.actuators == [native_actuator] + assert selection.view.world_count == 2 + assert calls == [ + ("construct", articulation), + ( + "prepare", + collection, + {"stage": stage, "articulation_prim_path": None, "adapt_usd_actuators": True}, + ), + ("finalize", collection), + ] + + +def test_submit_commands_uses_processed_lab_buffers_without_native_runtime() -> None: + """The standard path must submit applied effort and implicit drive targets.""" + view = _RecordingView() + articulation = SimpleNamespace( + _can_write_effort=True, + _can_write_pos_target=True, + _can_write_vel_target=True, + _has_implicit_actuators=True, + data=SimpleNamespace(has_joint_ordering=False), + _root_view=view, + ) + collection = SimpleNamespace( + _applied_effort=_buffer(1.0), + _joint_pos_target=_buffer(2.0), + _joint_vel_target=_buffer(3.0), + ) + control = object.__new__(OvPhysxActuatorControl) + control._articulation = articulation + control._actuator_runtime = None + + control.submit_commands(collection) + + assert view.values[TT.DOF_ACTUATION_FORCE] is collection._applied_effort + assert view.values[TT.DOF_POSITION_TARGET] is collection._joint_pos_target + assert view.values[TT.DOF_VELOCITY_TARGET] is collection._joint_vel_target + + +def test_submit_commands_uses_native_raw_effort_without_double_counting_implicit_pd() -> None: + """A native runtime must replace telemetry effort while retaining public drive targets.""" + view = _RecordingView() + native_effort = _buffer(4.0) + articulation = SimpleNamespace( + _can_write_effort=True, + _can_write_pos_target=True, + _can_write_vel_target=True, + _has_implicit_actuators=True, + data=SimpleNamespace(has_joint_ordering=False), + _root_view=view, + ) + collection = SimpleNamespace( + _applied_effort=_buffer(1.0), + _joint_pos_target=_buffer(5.0), + _joint_vel_target=_buffer(6.0), + ) + control = object.__new__(OvPhysxActuatorControl) + control._articulation = articulation + control._actuator_runtime = SimpleNamespace(wrapper=SimpleNamespace(joint_f_2d=native_effort)) + + control.submit_commands(collection) + + assert view.values[TT.DOF_ACTUATION_FORCE] is native_effort + assert view.values[TT.DOF_POSITION_TARGET] is collection._joint_pos_target + assert view.values[TT.DOF_VELOCITY_TARGET] is collection._joint_vel_target + + +def test_native_compute_refreshes_owned_state_before_controller() -> None: + """The native controller must observe fresh OVPhysX position and velocity shadows.""" + ordered_calls = Mock() + articulation = SimpleNamespace( + _data=SimpleNamespace( + _refresh_joint_pos=ordered_calls.refresh_position, + _refresh_joint_vel=ordered_calls.refresh_velocity, + ) + ) + control = object.__new__(OvPhysxActuatorControl) + control._articulation = articulation + control._native_actuator_path_active = True + control._actuator_runtime = SimpleNamespace(compute=ordered_calls.compute) + collection = SimpleNamespace() + + assert control.compute_native_actuators(collection, 0.01) + + assert ordered_calls.mock_calls == [ + call.refresh_position(), + call.refresh_velocity(), + call.compute(collection, 0.01), + ] + + +def test_native_reset_delegates_exact_environment_selector() -> None: + """Selective reset must pass the caller's environment selector unchanged.""" + runtime = Mock() + env_ids = wp.array([1, 0], dtype=wp.int32, device="cpu") + control = object.__new__(OvPhysxActuatorControl) + control._native_actuator_path_active = True + control._actuator_runtime = runtime + + control.reset_native_actuators(env_ids) + + runtime.reset.assert_called_once_with(env_ids) + + +def test_stage_user_command_converts_partial_environment_selector_to_sim_indices() -> None: + """Partial public commands must use the OVPhysX int32 simulator selector.""" + view = _RecordingView() + user = _buffer(7.0) + backend = _buffer(0.0) + sim_ids = wp.array([1], dtype=wp.int32, device="cpu") + articulation = SimpleNamespace( + _can_write_pos_target=True, + _joint_pos_target_backend=backend, + _get_backend_ordered_joint_buffer=lambda values, staging: values, + _get_sim_env_ids=lambda values: sim_ids, + _root_view=view, + ) + collection = SimpleNamespace(_joint_pos_target=user) + control = object.__new__(OvPhysxActuatorControl) + control._articulation = articulation + + control.stage_user_command( + "position", + collection, + env_ids=wp.array([1], dtype=wp.int64, device="cpu"), + joint_ids=None, + env_mask=None, + joint_mask=None, + ) + + assert view.values[TT.DOF_POSITION_TARGET] is user + assert view.kwargs[TT.DOF_POSITION_TARGET]["indices"] is sim_ids diff --git a/source/isaaclab_ov/test/assets/unit/test_ovphysx_articulation.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_articulation.py new file mode 100644 index 000000000000..093c56e7d3db --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_articulation.py @@ -0,0 +1,114 @@ +# 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 + +"""Focused OVPhysX articulation ordering and derived-cache tests.""" + +from unittest.mock import Mock + +import pytest +import torch +import warp as wp +from isaaclab_ov import tensor_types as TT +from isaaclab_ov.assets import Articulation +from isaaclab_ov.assets.articulation.articulation_data import ArticulationData + +from pxr import Usd, UsdGeom, UsdPhysics + +from isaaclab.assets.articulation import ordering_kernels +from isaaclab.utils.warp.launch_cache import _WarpLaunchCache + +pytestmark = pytest.mark.unit + + +def test_joint_dof_sign_resolution_traverses_instance_proxies() -> None: + """Joint direction resolution must inspect joints below instance proxies.""" + source_stage = Usd.Stage.CreateInMemory() + UsdGeom.Xform.Define(source_stage, "/Robot") + base = UsdGeom.Xform.Define(source_stage, "/Robot/base").GetPrim() + link = UsdGeom.Xform.Define(source_stage, "/Robot/link").GetPrim() + joint = UsdPhysics.RevoluteJoint.Define(source_stage, "/Robot/joint") + joint.CreateBody0Rel().SetTargets([link.GetPath()]) + joint.CreateBody1Rel().SetTargets([base.GetPath()]) + + stage = Usd.Stage.CreateInMemory() + instance = UsdGeom.Xform.Define(stage, "/World/Robot").GetPrim() + instance.GetReferences().AddReference(source_stage.GetRootLayer().identifier, "/Robot") + instance.SetInstanceable(True) + articulation = Mock( + cfg=Mock(prim_path="/World/Robot"), + _joint_names=["joint"], + _body_names=["base", "link"], + ) + + assert Articulation._resolve_joint_dof_signs(articulation, stage) == (-1,) + + +def test_ordering_install_and_invalidation_clear_recorded_reads() -> None: + """Ordering replacement and scene invalidation must discard stale read launches.""" + + class MinimalData(ArticulationData): + def __dir__(self): + return [] + + class Buffer: + timestamp = 1.0 + + data = MinimalData.__new__(MinimalData) + read_launch_cache = Mock() + data._read_launch_cache = read_launch_cache + data._configure_ordering_buffers = lambda: None + data._make_jacobian_body_user_to_backend = lambda: object() + data.joint_ordering = None + data._body_com_jacobian_w = Buffer() + data._mass_matrix = Buffer() + data._gravity_compensation_forces = Buffer() + + data._apply_ordering_maps_after_resolve() + + read_launch_cache.clear.assert_called_once_with() + assert data._body_com_jacobian_w.timestamp == -1.0 + assert data._mass_matrix.timestamp == -1.0 + assert data._gravity_compensation_forces.timestamp == -1.0 + + data._is_primed = True + data._sim_timestamp = 1.0 + data._invalidate_initialize_callback(None) + + assert read_launch_cache.clear.call_count == 2 + assert data._is_primed is False + assert data._sim_timestamp == 0.0 + + +def test_generalized_dynamics_gathers_both_axes_into_public_joint_order() -> None: + """A nonidentity joint map must reorder both axes of the mass matrix.""" + + class Buffer: + def __init__(self): + self.data = wp.zeros((1, 2, 2), dtype=wp.float32, device="cpu") + self.timestamp = -1.0 + + data = ArticulationData.__new__(ArticulationData) + data.device = "cpu" + data._sim_timestamp = 1.0 + data._read_launch_cache = _WarpLaunchCache("cpu") + data.joint_ordering = object() + data._jacobian_joint_user_to_backend = wp.array([1, 0], dtype=wp.int32, device="cpu") + data._joint_dof_signs = wp.ones(2, dtype=wp.int32, device="cpu") + data._has_reversed_joints = False + data._num_base_dofs = 0 + backend_values = wp.array([[[1.0, 2.0], [3.0, 4.0]]], dtype=wp.float32, device="cpu") + backend_buffer = wp.zeros_like(backend_values) + buffer = Buffer() + data._binding_read = lambda tensor_type, destination: destination.assign(backend_values) + + data._refresh_generalized_dynamics_buffer( + buffer, + backend_buffer, + TT.MASS_MATRIX, + ordering_kernels.reorder_mass_matrix_backend_to_user, + ) + + torch.testing.assert_close(wp.to_torch(buffer.data), torch.tensor([[[4.0, 3.0], [2.0, 1.0]]])) + assert buffer.timestamp == 1.0 diff --git a/source/isaaclab_ov/test/assets/test_deformable_object_helpers.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_deformable_object.py similarity index 99% rename from source/isaaclab_ov/test/assets/test_deformable_object_helpers.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_deformable_object.py index 42e236925d66..a04ce520a678 100644 --- a/source/isaaclab_ov/test/assets/test_deformable_object_helpers.py +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_deformable_object.py @@ -14,6 +14,8 @@ pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") +pytestmark = pytest.mark.unit + import torch # noqa: E402 import warp as wp # noqa: E402 from isaaclab_ov import tensor_types as TT # noqa: E402 diff --git a/source/isaaclab_ov/test/assets/test_rigid_object_helpers.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object.py similarity index 98% rename from source/isaaclab_ov/test/assets/test_rigid_object_helpers.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object.py index 7fdb408cdd29..f8e2800e898d 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object_helpers.py +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object.py @@ -19,6 +19,8 @@ # CI jobs that need OVPhysX coverage install it explicitly. pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") +pytestmark = pytest.mark.unit + from isaaclab_ov import tensor_types as TT # noqa: E402 from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet # noqa: E402 diff --git a/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object_collection.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object_collection.py new file mode 100644 index 000000000000..1a7760ac8329 --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object_collection.py @@ -0,0 +1,191 @@ +# 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 + +"""Focused OVPhysX fused-layout and staging tests for rigid-object collections.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import torch +import warp as wp + +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + +pytestmark = pytest.mark.unit + +from isaaclab_ov import tensor_types as TT # noqa: E402 +from isaaclab_ov.assets.rigid_object_collection.rigid_object_collection import ( # noqa: E402 + RigidObjectCollection, +) +from isaaclab_ov.assets.rigid_object_collection.rigid_object_collection_data import ( # noqa: E402 + RigidObjectCollectionData, +) + +from isaaclab.utils.buffers.timestamped_buffer_warp import TimestampedBufferWarp # noqa: E402 + + +def _collection_shell() -> RigidObjectCollection: + """Create an N=2, B=3 collection shell with selector scratch buffers.""" + collection = object.__new__(RigidObjectCollection) + collection._device = "cpu" + collection._num_instances = 2 + collection._num_bodies = 3 + collection._ALL_ENV_INDICES = wp.array([0, 1], dtype=wp.int32, device="cpu") + collection._ALL_BODY_INDICES = wp.array([0, 1, 2], dtype=wp.int32, device="cpu") + collection._ALL_VIEW_INDICES = wp.array([0, 1, 2, 3, 4, 5], dtype=wp.int32, device="cpu") + collection._cpu_all_view_ids = collection._ALL_VIEW_INDICES + collection._sim_view_ids = wp.empty(6, dtype=wp.int32, device="cpu") + collection._sim_view_ids_views = {} + collection._cpu_view_ids = wp.empty(6, dtype=wp.int32, device="cpu") + collection._cpu_view_ids_views = {} + return collection + + +def _data_shell(device: str = "cpu") -> RigidObjectCollectionData: + """Create an N=2, B=3 collection-data shell for pure layout helpers.""" + data = object.__new__(RigidObjectCollectionData) + data.device = device + data.num_instances = 2 + data.num_bodies = 3 + data._cpu_staging_buffers = {} + return data + + +def test_instance_major_scalar_layout_round_trips_through_body_major_binding() -> None: + """Scalar values must follow literal body-major fused order and invert losslessly.""" + collection = _collection_shell() + data = _data_shell() + public = wp.array([[0.0, 1.0, 2.0], [10.0, 11.0, 12.0]], dtype=wp.float32, device="cpu") + + fused = collection.reshape_data_to_view_2d(public) + restored = data._reshape_view_to_data_2d(fused) + + np.testing.assert_array_equal(fused.numpy(), [0.0, 10.0, 1.0, 11.0, 2.0, 12.0]) + np.testing.assert_array_equal(restored.numpy(), public.numpy()) + + +@pytest.mark.parametrize("library", ["warp", "torch"]) +def test_instance_major_vector_layout_round_trips_through_body_major_binding(library: str) -> None: + """Vector values must keep component rows attached while transposing N and B.""" + collection = _collection_shell() + data = _data_shell() + public_torch = torch.tensor( + [ + [[0.0, 0.5], [1.0, 1.5], [2.0, 2.5]], + [[10.0, 10.5], [11.0, 11.5], [12.0, 12.5]], + ] + ) + public = public_torch if library == "torch" else wp.from_torch(public_torch, dtype=wp.float32) + + fused = collection.reshape_data_to_view_3d(public, 2, device="cpu") + fused_warp = wp.from_torch(fused, dtype=wp.float32) if isinstance(fused, torch.Tensor) else fused + restored = data._reshape_view_to_data_3d(fused_warp, 2) + + expected_fused = torch.tensor([[0.0, 0.5], [10.0, 10.5], [1.0, 1.5], [11.0, 11.5], [2.0, 2.5], [12.0, 12.5]]) + torch.testing.assert_close(wp.to_torch(fused_warp), expected_fused) + torch.testing.assert_close(wp.to_torch(restored), public_torch) + + +def test_env_body_selectors_map_to_literal_body_major_view_ids() -> None: + """Nonidentity environment and body selectors must produce body-major flat IDs.""" + collection = _collection_shell() + + view_ids = collection._env_body_ids_to_view_ids( + wp.array([1, 0], dtype=wp.int64, device="cpu"), + wp.array([2, 0], dtype=wp.int32, device="cpu"), + device="cpu", + ) + + np.testing.assert_array_equal(view_ids.numpy(), [5, 4, 1, 0]) + assert view_ids.dtype == wp.int32 + + +def test_native_and_mock_binding_writes_use_their_distinct_index_domains() -> None: + """Native fused writes use view IDs while contract mocks use environment IDs.""" + collection = _collection_shell() + public = wp.array([[0.0, 1.0, 2.0], [10.0, 11.0, 12.0]], dtype=wp.float32, device="cpu") + env_ids = wp.array([1], dtype=wp.int32, device="cpu") + calls: list[tuple[wp.array, wp.array]] = [] + collection._get_sim_env_ids = lambda ids, sim_ids=None: ids + collection._root_view = SimpleNamespace( + set_attribute=lambda tensor_type, values, indices=None: calls.append((values, indices)) + ) + + collection._get_binding = lambda tensor_type: SimpleNamespace(shape=(6,)) + collection._binding_write(TT.BODY_MASS, public, env_ids=env_ids, device="cpu") + native_values, native_ids = calls.pop() + np.testing.assert_array_equal(native_values.numpy(), [0.0, 10.0, 1.0, 11.0, 2.0, 12.0]) + np.testing.assert_array_equal(native_ids.numpy(), [1, 3, 5]) + + collection._get_binding = lambda tensor_type: SimpleNamespace(shape=(2, 3)) + collection._binding_write(TT.BODY_MASS, public, env_ids=env_ids, device="cpu") + mock_values, mock_ids = calls.pop() + assert mock_values.ptr == public.ptr + np.testing.assert_array_equal(mock_ids.numpy(), [1]) + + +def test_cpu_only_read_scratch_is_cached_by_tensor_type() -> None: + """Repeated CPU-only property reads must reuse one correctly shaped staging allocation.""" + data = _data_shell() + binding = SimpleNamespace(shape=(6, 9)) + + first = data._read_view_scratch(TT.BODY_INERTIA, binding) + second = data._read_view_scratch(TT.BODY_INERTIA, binding) + + assert first is second + assert first.shape == (6, 9) + assert str(first.device) == "cpu" + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA copy-back requires a GPU") +def test_cuda_collection_reuses_pinned_cpu_property_scratch_and_copies_instance_major() -> None: + """A CUDA collection must read CPU properties through pinned, reusable body-major staging.""" + data = _data_shell(device="cuda:0") + data._sim_timestamp = 1.0 + binding = SimpleNamespace(shape=(6, 1)) + body_major = wp.array([[0.0], [10.0], [1.0], [11.0], [2.0], [12.0]], dtype=wp.float32, device="cpu") + + class _View: + def try_binding_for(self, tensor_type): + return binding + + def read_into(self, tensor_type, destination) -> None: + wp.copy(destination, body_major) + + data._view = _View() + destination = TimestampedBufferWarp((2, 3), device="cuda:0", dtype=wp.float32) + + data._read_binding_into_instance_major(TT.BODY_MASS, destination, floats_per_elem=1) + + scratch = data._read_view_scratch(TT.BODY_MASS, binding) + assert scratch is data._read_view_scratch(TT.BODY_MASS, binding) + assert scratch.pinned + assert str(scratch.device) == "cpu" + torch.testing.assert_close( + wp.to_torch(destination.data), + torch.tensor([[0.0, 1.0, 2.0], [10.0, 11.0, 12.0]], device="cuda:0"), + ) + + +def test_transform_component_views_share_storage_with_structured_parent() -> None: + """Position and quaternion adapters must reinterpret, not copy, transform storage.""" + data = _data_shell() + transforms = wp.array( + [ + [[1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9], [4.0, 5.0, 6.0, 0.4, 0.5, 0.6, 0.7]], + [[7.0, 8.0, 9.0, 0.7, 0.8, 0.9, 0.1], [10.0, 11.0, 12.0, 0.2, 0.3, 0.4, 0.8]], + ], + dtype=wp.transformf, + device="cpu", + ) + + positions = data._get_pos_from_transform(transforms) + quaternions = data._get_quat_from_transform(transforms) + + assert positions.ptr == transforms.ptr + assert quaternions.ptr == transforms.ptr + 3 * 4 + np.testing.assert_array_equal(positions.numpy()[0, 0], [1.0, 2.0, 3.0]) + np.testing.assert_allclose(quaternions.numpy()[0, 0], [0.1, 0.2, 0.3, 0.9], atol=1e-6) diff --git a/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py b/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py index 1758a9c2a5e1..958992a44c6a 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py @@ -12,8 +12,12 @@ import textwrap from pathlib import Path from types import ModuleType, SimpleNamespace +from unittest.mock import Mock import pytest +import torch + +from pxr import Usd pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") @@ -25,9 +29,11 @@ def __init__(self, num_threads=None, carbonite_overrides=None): class _FakePhysX: + cpu_mode_calls: list[bool] = [] + @classmethod def set_cpu_mode(cls, enabled): - pass + cls.cpu_mode_calls.append(enabled) def __init__(self, active_cuda_gpus=None, config=None): self.active_cuda_gpus = active_cuda_gpus @@ -49,7 +55,6 @@ def manager_module(monkeypatch): "_next_control_ordinal": 2, "_warmup_done": False, "_requires_full_stage": False, - "_locked_device": None, "_active_clone_recipes": [], "_pending_clones": [], "_atexit_registered": False, @@ -69,6 +74,64 @@ def _fake_ovphysx_module(bootstrap): return module +def test_cpu_runtime_construction_does_not_enable_sticky_cpu_mode(manager_module): + """An ordinary CPU scene must not enable the wheel's process-global CPU-only mode.""" + manager = manager_module.OvPhysxManager + _FakePhysX.cpu_mode_calls = [] + + manager._create_physx_instance(_fake_ovphysx_module(lambda: None), "cpu", 0) + + assert _FakePhysX.cpu_mode_calls == [] + + +def test_cpu_stage_warmup_does_not_call_gpu_warmup(monkeypatch, manager_module): + """Attaching a CPU stage must not invoke the runtime's GPU-only warmup.""" + from isaaclab.physics import PhysicsManager + + manager = manager_module.OvPhysxManager + physx = SimpleNamespace(warmup_gpu=Mock()) + scene_backend = SimpleNamespace(setup=Mock()) + sim = SimpleNamespace( + stage=Usd.Stage.CreateInMemory(), + cfg=SimpleNamespace(physics_prim_path="/World/physicsScene"), + ) + monkeypatch.setattr(PhysicsManager, "_sim", sim) + monkeypatch.setattr(PhysicsManager, "_device", "cpu") + monkeypatch.setattr(PhysicsManager, "_cfg", None) + monkeypatch.setattr(manager, "_physx", physx) + monkeypatch.setattr(manager, "_scene_data_backend", scene_backend) + monkeypatch.setattr(manager, "_rearm_pending_clones", lambda: None) + monkeypatch.setattr(manager, "_serialize_selected_stage", lambda stage: "#usda 1.0") + monkeypatch.setattr(manager, "_prepare_physx_for_stage_reuse", lambda: None) + monkeypatch.setattr(manager, "_attach_ovstage", lambda stage_usda: None) + monkeypatch.setattr(manager, "_replay_pending_clones", lambda runtime, requires_full_stage: None) + monkeypatch.setattr(manager, "dispatch_event", lambda event, payload=None: None) + + manager._warmup_and_load() + + physx.warmup_gpu.assert_not_called() + scene_backend.setup.assert_called_once_with(physx, sim.stage, "cpu") + assert manager._warmup_done + + +@pytest.mark.parametrize( + ("device", "expected_gpu_dynamics", "expected_broadphase"), + [("cpu", False, "MBP"), ("gpu", True, "GPU")], +) +def test_scene_configuration_authors_device_specific_dynamics_and_broadphase( + manager_module, device, expected_gpu_dynamics, expected_broadphase +): + """CPU and GPU scenes must explicitly author their respective PhysX modes.""" + manager = manager_module.OvPhysxManager + stage = Usd.Stage.CreateInMemory() + scene_prim = stage.DefinePrim("/World/PhysicsScene", "PhysicsScene") + + manager._configure_physx_scene_prim(scene_prim, cfg=None, device=device) + + assert scene_prim.GetAttribute("physxScene:enableGPUDynamics").Get() is expected_gpu_dynamics + assert scene_prim.GetAttribute("physxScene:broadphaseType").Get() == expected_broadphase + + @pytest.mark.parametrize( ("registered_names", "expected_paths"), [ @@ -392,6 +455,57 @@ def report_unraisable(unraisable): ) +def _device_reuse_script() -> str: + return textwrap.dedent( + """ + import torch + + import isaaclab.sim as sim_utils + from isaaclab.assets import RigidObjectCfg + from isaaclab.sim import SimulationCfg, build_simulation_context + from isaaclab_ov.assets import RigidObject + from isaaclab_ov.physics import OvPhysxCfg + + def run_scene(device): + sim_cfg = SimulationCfg(physics=OvPhysxCfg(), device=device, dt=1.0 / 60.0) + with build_simulation_context(device=device, sim_cfg=sim_cfg) as sim: + cube = RigidObject( + RigidObjectCfg( + prim_path="/World/Cube", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), + spawn=sim_utils.CuboidCfg( + size=(0.5, 0.5, 0.5), + rigid_props=sim_utils.RigidBodyBaseCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionBaseCfg(), + ), + ) + ) + sim.reset() + + root_pose = cube.data.root_link_pose_w.torch.clone() + root_velocity = cube.data.root_com_vel_w.torch.clone() + assert str(root_pose.device) == device + assert str(root_velocity.device) == device + root_pose[:, 0] = 0.25 + cube.write_root_link_pose_to_sim_index(root_pose=root_pose) + cube.write_root_com_velocity_to_sim_index(root_velocity=root_velocity) + cube.update(sim.get_physics_dt()) + torch.testing.assert_close(cube.data.root_link_pose_w.torch, root_pose) + torch.testing.assert_close(cube.data.root_com_vel_w.torch, root_velocity) + + sim.step() + cube.update(sim.get_physics_dt()) + advanced_pose = cube.data.root_link_pose_w.torch + assert str(advanced_pose.device) == device + assert advanced_pose[:, 2].item() < root_pose[:, 2].item() + + for device in ("cpu", "cuda:0", "cpu"): + run_scene(device) + """ + ) + + def _run_child(script: str) -> tuple[subprocess.CompletedProcess[str], str]: completed = subprocess.run( [sys.executable, "-c", script], @@ -419,3 +533,11 @@ def test_retained_binding_preserves_uncaught_failure_exit_status(): assert "NORMAL_ATEXIT" in output, output[-8000:] assert "OVPHYSX_STOP" in output, output[-8000:] _assert_no_atexit_errors(output) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") +def test_manager_reuses_cpu_and_cuda_scenes_in_one_process(): + """A process must run CPU, CUDA, then CPU cuboid scenes with state I/O on each device.""" + completed, output = _run_child(_device_reuse_script()) + + assert completed.returncode == 0, output[-8000:] diff --git a/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py b/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py index bad5af8750ce..6fa1ce479b01 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py @@ -363,10 +363,10 @@ def test_manager_forced_rewarm_invalidates_bindings_before_loading(monkeypatch): @pytest.mark.parametrize( - ("device", "gpu_index", "expected_cpu_mode", "expected_active_cuda_gpus"), - [("cpu", 0, True, None), ("gpu", 2, False, "2")], + ("device", "gpu_index", "expected_active_cuda_gpus"), + [("cpu", 0, None), ("gpu", 2, "2")], ) -def test_manager_supports_pinned_runtime_api(device, gpu_index, expected_cpu_mode, expected_active_cuda_gpus): +def test_manager_supports_pinned_runtime_api(device, gpu_index, expected_active_cuda_gpus): """The pinned OVPhysX wheel keeps its constructor, step, and reset API.""" from isaaclab_ov.physics import OvPhysxManager @@ -400,7 +400,7 @@ def wait_op(self, operation): OvPhysxManager._step_physx(physx, dt=0.02) OvPhysxManager._reset_physx_stage(physx) - assert PinnedPhysX.cpu_mode is expected_cpu_mode + assert PinnedPhysX.cpu_mode is None assert physx.constructor["active_cuda_gpus"] == expected_active_cuda_gpus assert physx.constructor["config"].num_threads == 8 assert physx.calls == [("step_sync", 0.02), ("reset_stage",), ("wait_op", 23)] diff --git a/source/isaaclab_physx/changelog.d/asset-tests-redesign.rst b/source/isaaclab_physx/changelog.d/asset-tests-redesign.rst new file mode 100644 index 000000000000..37668041f766 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/asset-tests-redesign.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed rigid-object collection center-of-mass and inertia setters to provide + correctly shaped buffers to the PhysX TensorAPI. diff --git a/source/isaaclab_physx/isaaclab_physx/assets/deformable_object/deformable_object.py b/source/isaaclab_physx/isaaclab_physx/assets/deformable_object/deformable_object.py index 58abfdb614e9..826fa37b7e0a 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/deformable_object/deformable_object.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/deformable_object/deformable_object.py @@ -42,6 +42,19 @@ logger = logging.getLogger(__name__) +def _infer_deformable_type(material_schemas: Sequence[str], *, has_tetmesh: bool, has_mesh: bool) -> str | None: + """Infer the PhysX deformable view family from material schemas or mesh topology.""" + if "PhysxSurfaceDeformableMaterialAPI" in material_schemas: + return "surface" + if "PhysxDeformableMaterialAPI" in material_schemas: + return "volume" + if has_tetmesh: + return "volume" + if has_mesh: + return "surface" + return None + + class DeformableObject(AssetBase): """A deformable object asset class. @@ -605,11 +618,6 @@ def has_deformable_body_api(prim) -> bool: mat_prim = root_prim.GetStage().GetPrimAtPath(mat_path) if "OmniPhysicsDeformableMaterialAPI" in mat_prim.GetAppliedSchemas(): material_prim = mat_prim - # determine deformable material type - if "PhysxSurfaceDeformableMaterialAPI" in mat_prim.GetAppliedSchemas(): - self._deformable_type = "surface" - elif "PhysxDeformableMaterialAPI" in mat_prim.GetAppliedSchemas(): - self._deformable_type = "volume" break if material_prim is None: @@ -620,16 +628,17 @@ def has_deformable_body_api(prim) -> bool: "bound to the deformable body." ) - # fall back to prim hierarchy heuristic when material type detection was inconclusive - if self._deformable_type is None: + material_schemas = () if material_prim is None else tuple(material_prim.GetAppliedSchemas()) + has_tetmesh = False + has_mesh = False + # Avoid walking the hierarchy when the bound material already identifies the view family. + if _infer_deformable_type(material_schemas, has_tetmesh=False, has_mesh=False) is None: # volume deformables must have a tetmesh in the hierarchy has_tetmesh = ( len(sim_utils.get_all_matching_child_prims(root_prim.GetPath(), lambda p: p.GetTypeName() == "TetMesh")) > 0 ) - if has_tetmesh: - self._deformable_type = "volume" - else: + if not has_tetmesh: # surface deformables must have a mesh in the hierarchy has_mesh = ( len( @@ -637,8 +646,7 @@ def has_deformable_body_api(prim) -> bool: ) > 0 ) - if has_mesh: - self._deformable_type = "surface" + self._deformable_type = _infer_deformable_type(material_schemas, has_tetmesh=has_tetmesh, has_mesh=has_mesh) # -- object view if self._deformable_type == "surface": diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py index f7b41249097f..3973acdb95ce 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py @@ -1028,6 +1028,7 @@ def set_coms_index( # Convert from instance order (num_instances, num_bodies, 7) to view order (num_bodies*num_instances, 7) for # PhysX. com_view_order = self.reshape_data_to_view_2d(self.data._body_com_pose_b.data, device="cpu") # (B*I, 7) + com_view_order = com_view_order.view(wp.float32).reshape((self.num_instances * self.num_bodies, 7)) view_ids = self._env_body_ids_to_view_ids(env_ids, body_ids, device="cpu") self.root_view.set_coms(com_view_order, indices=view_ids) @@ -1118,7 +1119,7 @@ def set_inertias_index( ) # Set into simulation, note that when updating "model" properties with PhysX we need to do it on CPU. # Convert from instance order (num_instances, num_bodies) to view order for PhysX. - inertia_view_order = self.reshape_data_to_view_2d(self.data._body_inertia, device="cpu") + inertia_view_order = self.reshape_data_to_view_3d(self.data._body_inertia, data_dim=9, device="cpu") view_ids = self._env_body_ids_to_view_ids(env_ids, body_ids, device="cpu") self.root_view.set_inertias(inertia_view_order, indices=view_ids) diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 6263ce09eb84..de0860671e5f 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -3,31 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - -"""Launch Isaac Sim Simulator first.""" +"""Real PhysX articulation coverage on one module-scoped scene.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import DeviceScope, resolve_test_sim_device, test_devices -from isaaclab.test.utils.articulation_ordering import ( - BRANCHING_MJWARP_BODY_NAMES, - BRANCHING_MJWARP_JOINT_NAMES, - BRANCHING_PHYSX_BODY_NAMES, - BRANCHING_PHYSX_JOINT_NAMES, - PANDA_BODY_NAMES, - PANDA_JOINT_NAMES, - PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES, -) - -HEADLESS = True -# launch omniverse app -simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app +simulation_app = AppLauncher(headless=True, device="cpu").app -"""Rest everything follows.""" - -import sys +from dataclasses import dataclass from pathlib import Path import pytest @@ -35,3050 +17,294 @@ import warp as wp from isaaclab_physx.assets import Articulation -from pxr import UsdPhysics +from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils -import isaaclab.utils.math as math_utils -import isaaclab.utils.string as string_utils -from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg -from isaaclab.assets import ArticulationCfg, get_articulation_name_ordering -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.sim import 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.version import get_isaac_sim_version, has_kit +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.sim import SimulationContext, build_simulation_context -## -# Pre-defined configs -## -from isaaclab_assets import ( # isort:skip - ANYMAL_C_CFG, - FRANKA_PANDA_CFG, - FRANKA_PANDA_HIGH_PD_CFG, - SHADOW_HAND_CFG, -) +pytestmark = pytest.mark.integration +_FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" +_NUM_ENVS = 2 -def generate_articulation_cfg( - articulation_type: str, - stiffness: float | None = 10.0, - damping: float | None = 2.0, - actuator_velocity_limit: float | None = None, - actuator_effort_limit: float | None = None, - joint_velocity_limit: float | None = None, - joint_effort_limit: float | None = None, -) -> ArticulationCfg: - """Generate an articulation configuration. - Args: - articulation_type: Type of articulation to generate. - It should be one of: "humanoid", "panda", "anymal", "shadow_hand", "single_joint_implicit", - "single_joint_explicit". - stiffness: Stiffness value for the articulation's actuators. Only currently used for "humanoid". - Defaults to 10.0. - damping: Damping value for the articulation's actuators. Only currently used for "humanoid". - Defaults to 2.0. - actuator_velocity_limit: Velocity limit for the actuators. Only currently used for "single_joint_implicit" - and "single_joint_explicit". - actuator_effort_limit: Effort limit for explicit actuators. Only currently used for - "single_joint_explicit". - joint_velocity_limit: Velocity limit for the actuators (set into the simulation). - Only currently used for "single_joint_implicit" and "single_joint_explicit". - joint_effort_limit: Effort limit for the actuators (set into the simulation). - Only currently used for "single_joint_implicit" and "single_joint_explicit". +@dataclass +class _ArticulationScene: + """Articulations that share one real PhysX lifecycle.""" - Returns: - The articulation configuration for the requested articulation type. + sim: SimulationContext + ordered: Articulation + floating: Articulation + tendon: Articulation + device: str - """ - if articulation_type == "humanoid": - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/Humanoid/humanoid_instanceable.usd" - ), - init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)), - actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, - ) - elif articulation_type == "panda": - articulation_cfg = FRANKA_PANDA_CFG - elif articulation_type == "anymal": - articulation_cfg = ANYMAL_C_CFG - elif articulation_type == "shadow_hand": - articulation_cfg = SHADOW_HAND_CFG - elif articulation_type == "single_joint_implicit": - articulation_cfg = ArticulationCfg( - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), - ), - actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - joint_effort_limit=joint_effort_limit, - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=actuator_velocity_limit, - stiffness=2000.0, - damping=100.0, - ), - }, - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos=({"RevoluteJoint": 1.5708}), - rot=(0.7071081, 0, 0, 0.7071055), - ), - ) - elif articulation_type == "single_joint_explicit": - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), - ), - actuators={ - "joint": IdealPDActuatorCfg( - joint_names_expr=[".*"], - joint_effort_limit=joint_effort_limit, - joint_velocity_limit=joint_velocity_limit, - actuator_effort_limit=actuator_effort_limit, - actuator_velocity_limit=actuator_velocity_limit, - stiffness=0.0, - damping=10.0, - ), - }, - ) - elif articulation_type == "spatial_tendon_test_asset": - # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/IsaacLab/Tests/spatial_tendons.usd", - ), + +def _spawn_articulation(name: str, *, y_offset: float, fixed_base: bool, spatial_tendon: bool = False) -> Articulation: + """Spawn one two-environment branching-articulation island.""" + island_path = f"/World/{name}" + sim_utils.create_prim(island_path, "Xform", translation=(0.0, y_offset, 0.0)) + for env_index in range(_NUM_ENVS): + sim_utils.create_prim(f"{island_path}/Env_{env_index}", "Xform", translation=(3.0 * env_index, 0.0, 0.0)) + articulation = Articulation( + ArticulationCfg( + prim_path=f"{island_path}/Env_[^/]*/Robot", + spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=2000.0, - damping=100.0, - ), + "joints": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=5.0, damping=0.5), }, + joint_ordering="mjwarp", + body_ordering="mjwarp", ) - else: - raise ValueError( - f"Invalid articulation type: {articulation_type}, valid options are 'humanoid', 'panda', 'anymal'," - " 'shadow_hand', 'single_joint_implicit', 'single_joint_explicit' or 'spatial_tendon_test_asset'." - ) - - return articulation_cfg - - -def generate_articulation( - articulation_cfg: ArticulationCfg, num_articulations: int, device: str -) -> tuple[Articulation, torch.tensor]: - """Generate an articulation from a configuration. - - Handles the creation of the articulation, the environment prims and the articulation's environment - translations - - Args: - articulation_cfg: Articulation configuration. - num_articulations: Number of articulations to generate. - device: Device to use for the tensors. - - Returns: - The articulation and environment translations. - - """ - # Generate translations of 2.5 m in x for each articulation - translations = torch.zeros(num_articulations, 3, device=device) - translations[:, 0] = torch.arange(num_articulations) * 2.5 - - # Create Top-level Xforms, one for each articulation - for i in range(num_articulations): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) - articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_[^/]*/Robot")) - - return articulation, translations - - -# --------------------------------------------------------------------------- -# Franka task-space tracking helpers (shared between IK and OSC tests). -# Mirrors the helpers in ``isaaclab_newton/test/assets/test_articulation.py``. -# --------------------------------------------------------------------------- - - -def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, enable_rigid_body_gravity: bool = False): - """Build a Franka articulation at its configured home pose. - - See the Newton-side mirror for full docs. Standalone tests skip the - env reset path that normally pushes ``default_joint_pos`` to sim, - so we teleport explicitly to avoid the URDF-neutral - near-singular pose where the Franka wrist axes nearly align. - - Args: - sim: The simulation context to use. - zero_actuator_pd: If True, sets the panda_shoulder/panda_forearm - actuator stiffness and damping to zero. - enable_rigid_body_gravity: If True, override - ``FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity`` - (which defaults to True) so gravity actually loads the arm. Required - for any test that wants to exercise gravity-related dynamics - (e.g. gravity-compensation accuracy tests). - - Returns: - Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. - """ - cfg = FRANKA_PANDA_HIGH_PD_CFG.copy().replace(prim_path="/World/Env_[^/]*/Robot") - if zero_actuator_pd: - cfg.actuators["panda_shoulder"].stiffness = 0.0 - cfg.actuators["panda_shoulder"].damping = 0.0 - cfg.actuators["panda_forearm"].stiffness = 0.0 - cfg.actuators["panda_forearm"].damping = 0.0 - if enable_rigid_body_gravity: - cfg = cfg.replace( - spawn=cfg.spawn.replace( - rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False), - ), - ) - sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) - robot = Articulation(cfg) - sim.reset() - assert robot.is_initialized - - ee_frame_idx = robot.find_bodies("panda_hand")[0][0] - ee_jacobi_idx = ee_frame_idx - 1 - arm_joint_ids = robot.find_joints(["panda_joint.*"])[0] - - robot.write_joint_state_to_sim( - position=robot.data.default_joint_pos.torch[:, :].clone(), - velocity=robot.data.default_joint_vel.torch[:, :].clone(), - ) - return robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids - - -def _compute_ee_pose_root(robot, ee_frame_idx): - """Return ``(ee_pos_b, ee_quat_b, root_pose_w)`` in the root frame.""" - ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx] - root_pose_w = robot.data.root_pose_w.torch - ee_pos_b, ee_quat_b = subtract_frame_transforms( - root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] ) - return ee_pos_b, ee_quat_b, root_pose_w - - -def _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids): - """Return the EE Jacobian sliced to ``arm_joint_ids`` and rotated to the root frame.""" - jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, :][:, :, arm_joint_ids] - base_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_pose_w.torch[:, 3:7])) - jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) - jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) - return jacobian - - -def _compute_ee_vel_root(jacobian_b, joint_vel): - """Return the EE 6D velocity in the root frame as ``J · q_dot``. - - Required to make OSC's ``kd * ee_vel_b`` damping term meaningful. - Passing zero EE velocity (the convenient hack) leaves the impedance - undamped and the EE oscillates around the target. ``J · q_dot`` - avoids relying on ``data.body_vel_w`` (Newton's lazy velocity - buffers can return stale/zero values until forced materialization), - keeping the helper backend-symmetric. ``J`` correctness is pinned - independently by ``test_get_jacobians_link_origin_contract``. - """ - return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) - - -def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device): - """Build a target pose = (current EE pose) + ``delta_xyz``, preserving orientation.""" - initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - target_pos_b = initial_ee_pos_b + torch.tensor([list(delta_xyz)], device=device, dtype=initial_ee_pos_b.dtype) - 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.""" - tail_slice = history[-tail:] - return min(tail_slice), sum(tail_slice) / len(tail_slice) - - -def _to_device_tensor(array: wp.array, device: str) -> torch.Tensor: - """Convert a Warp array to a torch tensor on :paramref:`device`.""" - return wp.to_torch(array).to(device=device) - - -def _assert_backend_to_user( - public_tensor: torch.Tensor, backend_tensor: torch.Tensor, user_to_backend: list[int] -) -> None: - """Assert a public tensor equals a backend tensor reordered to user order.""" - torch.testing.assert_close(public_tensor, backend_tensor.to(device=public_tensor.device)[:, user_to_backend]) - - -def _assert_user_write_reaches_backend( - user_tensor: torch.Tensor, backend_tensor: torch.Tensor, backend_to_user: list[int] -) -> None: - """Assert a user-order write reached backend storage in backend order.""" - torch.testing.assert_close(backend_tensor.to(device=user_tensor.device), user_tensor[:, backend_to_user]) - - -@pytest.fixture -def sim(request): - """Create simulation context with the specified device.""" - device = request.getfixturevalue("device") - if "gravity_enabled" in request.fixturenames: - gravity_enabled = request.getfixturevalue("gravity_enabled") - else: - gravity_enabled = True # default to gravity enabled - if "add_ground_plane" in request.fixturenames: - add_ground_plane = request.getfixturevalue("add_ground_plane") - else: - add_ground_plane = False # default to no ground plane - with build_simulation_context( - device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane - ) as sim: - sim._app_control_on_stop_handle = None - yield sim + stage = sim_utils.get_current_stage() + for env_index in range(_NUM_ENVS): + prim_path = f"{island_path}/Env_{env_index}/Robot" + for joint_name in ("left_shoulder", "left_elbow", "right_shoulder", "right_elbow"): + drive = UsdPhysics.DriveAPI.Apply(stage.GetPrimAtPath(f"{prim_path}/{joint_name}"), "angular") + drive.CreateStiffnessAttr(5.0) + drive.CreateDampingAttr(0.5) + drive.CreateMaxForceAttr(100.0) + collision = UsdGeom.Cube.Define(stage, f"{prim_path}/base/collision") + collision.CreateSizeAttr(0.1) + UsdPhysics.CollisionAPI.Apply(collision.GetPrim()) + if fixed_base: + fixed_joint = UsdPhysics.FixedJoint.Define(stage, f"{prim_path}/fixed_root") + fixed_joint.GetBody1Rel().SetTargets([f"{prim_path}/base"]) + if spatial_tendon: + root_prim = stage.GetPrimAtPath(f"{prim_path}/base") + root_attachment = PhysxSchema.PhysxTendonAttachmentAPI(root_prim, "root") + root_attachment.CreateLocalPosAttr(Gf.Vec3f(0.0)) + root = PhysxSchema.PhysxTendonAttachmentRootAPI.Apply(root_prim, "root") + root.CreateStiffnessAttr(5.0) + root.CreateDampingAttr(0.5) + root.CreateLimitStiffnessAttr(1.0) + root.CreateOffsetAttr(0.0) + + leaf_prim = stage.GetPrimAtPath(f"{prim_path}/left_tip") + leaf_attachment = PhysxSchema.PhysxTendonAttachmentAPI(leaf_prim, "leaf") + leaf_attachment.CreateLocalPosAttr(Gf.Vec3f(0.0)) + leaf_attachment.CreateParentAttachmentAttr("root") + leaf_attachment.CreateParentLinkRel().SetTargets([root_prim.GetPath()]) + leaf = PhysxSchema.PhysxTendonAttachmentLeafAPI.Apply(leaf_prim, "leaf") + leaf.CreateRestLengthAttr(0.5) + leaf.CreateLowerLimitAttr(0.0) + leaf.CreateUpperLimitAttr(2.0) + return articulation + + +@pytest.fixture(scope="module") +def articulation_scene() -> _ArticulationScene: + """Initialize every real PhysX articulation once for this module.""" + device = "cuda:0" if wp.is_cuda_available() else "cpu" + with build_simulation_context(device=device, gravity_enabled=False) as sim: + ordered = _spawn_articulation("Ordered", y_offset=0.0, fixed_base=True) + floating = _spawn_articulation("Floating", y_offset=3.0, fixed_base=False) + tendon = _spawn_articulation("Tendon", y_offset=6.0, fixed_base=True, spatial_tendon=True) + sim.reset() + yield _ArticulationScene(sim=sim, ordered=ordered, floating=floating, tendon=tendon, device=device) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_write_joint_state_accepts_int64_selector(sim, device, gravity_enabled): - """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) - sim.reset() +def test_articulation_initialization_and_partial_state(articulation_scene: _ArticulationScene) -> None: + """Prove ordering and indexed state writes against the real PhysX view.""" + articulation = articulation_scene.ordered + assert articulation.is_initialized + assert articulation.is_fixed_base + assert articulation.joint_ordering is not None + assert articulation.body_ordering is not None + assert articulation.num_instances == 2 assert articulation.num_joints >= 2 + assert articulation.num_bodies >= 3 - 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) - + env_ids = torch.tensor([1], dtype=torch.int32, device=articulation_scene.device) + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32, device=articulation_scene.device) + target_position = torch.tensor([[0.21, -0.13]], device=articulation_scene.device) + target_velocity = torch.tensor([[0.41, -0.23]], device=articulation_scene.device) expected_position = articulation.data.joint_pos.torch.clone() expected_velocity = articulation.data.joint_vel.torch.clone() - + expected_position[env_ids[:, None], joint_ids] = target_position + expected_velocity[env_ids[:, None], joint_ids] = target_velocity articulation.write_joint_state_to_sim_index( - position=position, velocity=velocity, env_ids=env_ids, joint_ids=joint_ids + position=target_position, + velocity=target_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", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_live_manual_root_preserving_ordering_reorders_backend_reads_and_writes(sim, device, gravity_enabled): - """Smoke-test non-identity joint/body ordering through a live PhysX articulation.""" - articulation_cfg = FRANKA_PANDA_CFG.replace( - prim_path="/World/Robot", - joint_ordering=tuple(reversed(PANDA_JOINT_NAMES)), - body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES, - ) - articulation = Articulation(articulation_cfg) - - sim.reset() - assert articulation.is_initialized - assert articulation.backend_joint_names == list(PANDA_JOINT_NAMES) - assert articulation.backend_body_names == list(PANDA_BODY_NAMES) - assert articulation.joint_ordering is not None - assert articulation.body_ordering is not None - - joint_user_to_backend = list(articulation.joint_ordering.user_to_backend_indices) joint_backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) - body_user_to_backend = list(articulation.body_ordering.user_to_backend_indices) - - joint_index = torch.arange(articulation.num_joints, device=device, dtype=torch.float32).unsqueeze(0) - joint_pos = torch.linspace(-0.3, 0.3, articulation.num_joints, device=device).unsqueeze(0) - joint_vel = torch.linspace(0.05, 0.13, articulation.num_joints, device=device).unsqueeze(0) - joint_stiffness = 10.0 + joint_index - - articulation.write_joint_stiffness_to_sim_index(stiffness=joint_stiffness, full_data=True) - articulation.write_joint_state_to_sim_index(position=joint_pos, velocity=joint_vel, full_data=True) - articulation.write_data_to_sim() - - _assert_user_write_reaches_backend( - joint_pos, _to_device_tensor(articulation.root_view.get_dof_positions(), device), joint_backend_to_user - ) - _assert_user_write_reaches_backend( - joint_vel, _to_device_tensor(articulation.root_view.get_dof_velocities(), device), joint_backend_to_user - ) - _assert_user_write_reaches_backend( - joint_stiffness, - _to_device_tensor(articulation.root_view.get_dof_stiffnesses(), device), - joint_backend_to_user, - ) - - sim.step() - articulation.update(sim.cfg.dt) - - _assert_backend_to_user( - articulation.data.joint_pos.torch, - _to_device_tensor(articulation.root_view.get_dof_positions(), device), - joint_user_to_backend, - ) - _assert_backend_to_user( - articulation.data.joint_vel.torch, - _to_device_tensor(articulation.root_view.get_dof_velocities(), device), - joint_user_to_backend, - ) - _assert_backend_to_user( - articulation.data.joint_stiffness.torch, - _to_device_tensor(articulation.root_view.get_dof_stiffnesses(), device), - joint_user_to_backend, - ) - _assert_backend_to_user( - articulation.data.body_link_pose_w.torch, - _to_device_tensor(articulation.root_view.get_link_transforms(), device), - body_user_to_backend, - ) - _assert_backend_to_user( - articulation.data.body_com_pose_b.torch, - _to_device_tensor(articulation.root_view.get_coms(), device), - body_user_to_backend, - ) - - torch.testing.assert_close(articulation.data.body_com_pos_b.torch, articulation.data.body_com_pose_b.torch[..., :3]) torch.testing.assert_close( - articulation.data.body_com_quat_b.torch, articulation.data.body_com_pose_b.torch[..., 3:] - ) - torch.testing.assert_close(articulation.data.body_pos_w.torch, articulation.data.body_link_pose_w.torch[..., :3]) - torch.testing.assert_close(articulation.data.body_quat_w.torch, articulation.data.body_link_pose_w.torch[..., 3:]) - - -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_reversed_joint_dynamics_use_public_joint_basis(sim, device, gravity_enabled): - """Keep dynamics tensors consistent with public joint velocity.""" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg( - usd_path=str(Path(__file__).parent / "data" / "articulation_ordering_branching.usda") - ), - actuators={}, - ) - ) - UsdPhysics.FixedJoint.Define(sim.stage, "/World/Robot/fixed_root").GetBody1Rel().SetTargets(["/World/Robot/base"]) - joint = UsdPhysics.RevoluteJoint.Get(sim.stage, "/World/Robot/left_elbow") - body0, body1 = joint.GetBody0Rel().GetTargets(), joint.GetBody1Rel().GetTargets() - joint.GetBody0Rel().SetTargets(body1) - joint.GetBody1Rel().SetTargets(body0) - sim.reset() - - velocity = torch.zeros((1, articulation.num_joints), device=device) - velocity[:, articulation.find_joints("left_shoulder")[0][0]] = 0.4 - velocity[:, articulation.find_joints("left_elbow")[0][0]] = 0.7 - articulation.write_joint_velocity_to_sim_index(velocity=velocity) - sim.step() - articulation.update(sim.cfg.dt) - - joint_velocity = articulation.data.joint_vel.torch - predicted_velocity = torch.einsum("nbij,nj->nbi", articulation.data.body_com_jacobian_w.torch, joint_velocity) - torch.testing.assert_close(predicted_velocity, articulation.data.body_com_vel_w.torch[:, 1:], atol=1e-5, rtol=1e-5) - - generalized_energy = 0.5 * torch.einsum( - "ni,nij,nj->n", joint_velocity, articulation.data.mass_matrix.torch, joint_velocity - ) - body_velocity = articulation.data.body_com_vel_w.torch - body_inertia = articulation.data.body_inertia.torch.reshape(1, articulation.num_bodies, 3, 3) - body_energy = 0.5 * ( - (articulation.data.body_mass.torch.unsqueeze(-1) * body_velocity[..., :3].square()).sum((-1, -2)) - + torch.einsum("nbi,nbij,nbj->n", body_velocity[..., 3:], body_inertia, body_velocity[..., 3:]) + wp.to_torch(articulation.root_view.get_dof_positions()), expected_position[:, joint_backend_to_user] ) - torch.testing.assert_close(generalized_energy, body_energy, atol=1e-5, rtol=1e-5) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_live_floating_root_writers_match_identity_after_body_reordering(sim, device, gravity_enabled): - """Keep floating-base root writes invariant when public body order moves the root.""" - floating_spawn = FRANKA_PANDA_CFG.spawn.replace( - articulation_props=FRANKA_PANDA_CFG.spawn.articulation_props.replace(fix_root_link=False) - ) - identity = Articulation( - FRANKA_PANDA_CFG.replace( - prim_path="/World/IdentityRobot", - spawn=floating_spawn, - body_ordering=None, - ) - ) - ordered = Articulation( - FRANKA_PANDA_CFG.replace( - prim_path="/World/OrderedRobot", - spawn=floating_spawn, - body_ordering=tuple(reversed(PANDA_BODY_NAMES)), - ) - ) - - sim.reset() - assert identity.is_initialized and ordered.is_initialized - assert not identity.is_fixed_base and not ordered.is_fixed_base - assert identity.body_ordering is None - assert ordered.body_ordering is not None - assert ordered.body_ordering.backend_to_user_indices[0] != 0 - - backend_coms = torch.zeros((1, len(PANDA_BODY_NAMES), 7), device=device) - body_index = torch.arange(len(PANDA_BODY_NAMES), device=device, dtype=torch.float32) - backend_coms[0, :, 0] = 0.05 + 0.01 * body_index - backend_coms[0, :, 1] = -0.03 - 0.02 * body_index - backend_coms[0, :, 2] = 0.02 + 0.03 * body_index - backend_coms[..., 6] = 1.0 - identity.set_coms_index( - coms=wp.from_torch(backend_coms.contiguous(), dtype=wp.transformf), - full_data=True, - ) - ordered_user_to_backend = list(ordered.body_ordering.user_to_backend_indices) - ordered.set_coms_index( - coms=wp.from_torch(backend_coms[:, ordered_user_to_backend].contiguous(), dtype=wp.transformf), - full_data=True, +def test_articulation_joint_and_body_properties_round_trip(articulation_scene: _ArticulationScene) -> None: + """Prove selected joint, body, and material properties round-trip through PhysX.""" + articulation = articulation_scene.ordered + device = articulation_scene.device + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32, device=device) + backend_joint_ids = torch.as_tensor(articulation.joint_ordering.user_to_backend_indices)[joint_ids.cpu()] + raw_friction_before = wp.to_torch(articulation.root_view.get_dof_friction_properties()).clone() + static_friction = torch.tensor([[0.9, 0.7]], device=device) + dynamic_friction = torch.tensor([[0.4, 0.3]], device=device) + viscous_friction = torch.tensor([[0.11, 0.22]], device=device) + articulation.write_joint_friction_coefficient_to_sim_index( + joint_friction_coeff=static_friction, + joint_dynamic_friction_coeff=dynamic_friction, + joint_viscous_friction_coeff=viscous_friction, + env_ids=env_ids, + joint_ids=joint_ids, ) - torch.testing.assert_close(_to_device_tensor(identity.root_view.get_coms(), device), backend_coms) - torch.testing.assert_close(_to_device_tensor(ordered.root_view.get_coms(), device), backend_coms) - - root_com_pose = torch.tensor([[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]], device=device) - root_link_velocity = torch.tensor([[0.4, -0.3, 0.2, 1.1, -0.7, 0.9]], device=device) - for articulation in (identity, ordered): - articulation.write_root_com_pose_to_sim_index(root_pose=root_com_pose) - articulation.write_root_link_velocity_to_sim_index(root_velocity=root_link_velocity) - + expected_raw_friction = raw_friction_before.clone() + expected_raw_friction[1, backend_joint_ids, 0] = static_friction.cpu() + expected_raw_friction[1, backend_joint_ids, 1] = dynamic_friction.cpu() + expected_raw_friction[1, backend_joint_ids, 2] = viscous_friction.cpu() + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_dof_friction_properties()), + expected_raw_friction, + ) + + body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32, device=device) + initial_mass = articulation.data.body_mass.torch.clone() + masses = torch.tensor([[2.5, 3.5]], device=device) + articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_mass.torch[env_ids][:, body_ids], masses) + torch.testing.assert_close(articulation.data.body_mass.torch[:1], initial_mass[:1]) + + initial_com = articulation.data.body_com_pose_b.torch.clone() + coms = articulation.data.body_com_pose_b.torch[env_ids][:, body_ids].clone() + coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03], device=device) + coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02], device=device) + articulation.set_coms_index(coms=coms, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[env_ids][:, body_ids], coms) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:1], initial_com[:1]) + + initial_inertia = articulation.data.body_inertia.torch.clone() + inertias = articulation.data.body_inertia.torch[env_ids][:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[0, 1, 4] *= 1.3 + articulation.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_inertia.torch[env_ids][:, body_ids], inertias) + torch.testing.assert_close(articulation.data.body_inertia.torch[:1], initial_inertia[:1]) + + body_backend_to_user = list(articulation.body_ordering.backend_to_user_indices) torch.testing.assert_close( - _to_device_tensor(ordered.root_view.get_root_transforms(), device), - _to_device_tensor(identity.root_view.get_root_transforms(), device), + wp.to_torch(articulation.root_view.get_masses()).to(device), + articulation.data.body_mass.torch[:, body_backend_to_user], ) torch.testing.assert_close( - _to_device_tensor(ordered.root_view.get_root_velocities(), device), - _to_device_tensor(identity.root_view.get_root_velocities(), device), + wp.to_torch(articulation.root_view.get_coms()).to(device), + articulation.data.body_com_pose_b.torch[:, body_backend_to_user], ) - torch.testing.assert_close(ordered.data.root_com_vel_w.torch, identity.data.root_com_vel_w.torch) - for articulation in (identity, ordered): - torch.testing.assert_close(articulation.data.root_com_pose_w.torch, root_com_pose) - torch.testing.assert_close(articulation.data.root_link_vel_w.torch, root_link_velocity) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("body_ordering", ["identity", "reversed"]) -def test_live_direct_view_mass_inertia_writes_become_visible(sim, device, gravity_enabled, body_ordering): - """Direct tensor-view writes to body masses/inertias become visible on the lazy read. - - Develop reads these properties directly from the tensor view on every access. The timestamp-lazy - implementation could instead hide direct ``root_view.set_masses`` / ``set_inertias`` writes after - its first read. This regression requires those writes to become visible through ``data.body_mass`` / - ``data.body_inertia`` when the lazy buffer is next eligible to refresh: - - - Case A: after a primed read and a subsequent simulation update (the lazy gate opens once per - step). - - Case B: on the very first read of a cold buffer (initial timestamp -1.0). - - Both identity and non-identity (reversed) body ordering are covered; under ordering the public - buffers must equal the backend-order view gathered through ``user_to_backend``. - """ - body_ordering_arg = None if body_ordering == "identity" else PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES - articulation = Articulation(FRANKA_PANDA_CFG.replace(prim_path="/World/Robot", body_ordering=body_ordering_arg)) - sim.reset() - assert articulation.is_initialized - - if body_ordering == "identity": - assert articulation.body_ordering is None - body_user_to_backend = list(range(articulation.num_bodies)) - else: - assert articulation.body_ordering is not None - body_user_to_backend = list(articulation.body_ordering.user_to_backend_indices) - - cpu_env_ids = wp.array(list(range(articulation.num_instances)), dtype=wp.int32, device="cpu") - - def write_backend_mass_inertia(delta_mass: float, delta_inertia: float) -> tuple[torch.Tensor, torch.Tensor]: - """Write distinct backend-order masses/inertias straight through the tensor view.""" - backend_masses = wp.to_torch(articulation.root_view.get_masses()).clone() + delta_mass - backend_inertias = wp.to_torch(articulation.root_view.get_inertias()).clone() + delta_inertia - articulation.root_view.set_masses( - wp.from_torch(backend_masses.contiguous(), dtype=wp.float32), indices=cpu_env_ids - ) - articulation.root_view.set_inertias( - wp.from_torch(backend_inertias.contiguous(), dtype=wp.float32), indices=cpu_env_ids - ) - return backend_masses, backend_inertias - - # Case A: prime the buffers, write through the view, then advance the sim so the lazy gate opens. - _ = articulation.data.body_mass.torch - _ = articulation.data.body_inertia.torch - backend_masses, backend_inertias = write_backend_mass_inertia(0.137, 0.011) - articulation.update(sim.cfg.dt) - _assert_backend_to_user(articulation.data.body_mass.torch, backend_masses, body_user_to_backend) - _assert_backend_to_user(articulation.data.body_inertia.torch, backend_inertias, body_user_to_backend) - - # Case B: a cold buffer (initial timestamp -1.0) must reflect a write on its first read. - articulation.data._body_mass.timestamp = -1.0 - articulation.data._body_inertia.timestamp = -1.0 - backend_masses, backend_inertias = write_backend_mass_inertia(0.293, 0.023) - _assert_backend_to_user(articulation.data.body_mass.torch, backend_masses, body_user_to_backend) - _assert_backend_to_user(articulation.data.body_inertia.torch, backend_inertias, body_user_to_backend) - - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_branching_fixture_resolves_distinct_conventions(sim, device, gravity_enabled): - """Resolve concrete breadth-first PhysX and depth-first MJWarp name orders.""" - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - joint_ordering="mjwarp", - body_ordering="mjwarp", - ) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_inertias()).to(device), + articulation.data.body_inertia.torch[:, body_backend_to_user], ) - sim.reset() - assert articulation.is_initialized - - assert tuple(articulation.backend_joint_names) == BRANCHING_PHYSX_JOINT_NAMES - assert tuple(articulation.backend_body_names) == BRANCHING_PHYSX_BODY_NAMES - assert get_articulation_name_ordering(articulation, "mjwarp", "joint") == BRANCHING_MJWARP_JOINT_NAMES - assert get_articulation_name_ordering(articulation, "mjwarp", "body") == BRANCHING_MJWARP_BODY_NAMES - assert tuple(articulation.joint_names) == BRANCHING_MJWARP_JOINT_NAMES - assert tuple(articulation.body_names) == BRANCHING_MJWARP_BODY_NAMES - assert articulation.joint_ordering is not None - assert articulation.body_ordering is not None - -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("ordering_axis", ["joint", "body"]) -def test_unused_jacobian_ordering_map_is_none(sim, device, gravity_enabled, ordering_axis): - """Keep the inactive Jacobian-axis map unset under single-axis ordering.""" - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - articulation = Articulation( - ArticulationCfg( - prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - joint_ordering="mjwarp" if ordering_axis == "joint" else None, - body_ordering="mjwarp" if ordering_axis == "body" else None, - ) + materials_before = wp.to_torch(articulation.root_view.get_material_properties()).clone() + materials = materials_before.clone() + materials[1, :, 0] = 0.91 + materials[1, :, 1] = 0.17 + materials[1, :, 2] = 0.63 + articulation.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.array([1], dtype=wp.int32, device="cpu") + ) + materials_after = wp.to_torch(articulation.root_view.get_material_properties()) + torch.testing.assert_close(materials_after[1:], materials[1:]) + torch.testing.assert_close(materials_after[:1], materials_before[:1]) + + +def test_articulation_drive_and_dynamics(articulation_scene: _ArticulationScene) -> None: + """Prove implicit drive delivery and live PhysX dynamics access.""" + articulation = articulation_scene.ordered + device = articulation_scene.device + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + joint_ids = torch.tensor([0], dtype=torch.int32, device=device) + articulation.write_joint_velocity_to_sim_index(velocity=torch.zeros_like(articulation.data.joint_vel.torch)) + initial_drive_position = articulation.data.joint_pos.torch[:, 0].clone() + raw_target_before = wp.to_torch(articulation.root_view.get_dof_position_targets()).clone() + drive_target = articulation.data.joint_pos.torch[env_ids][:, joint_ids].clone() + 0.4 + articulation.actuators.target_command.set_position_index( + value=drive_target, + env_ids=env_ids, + joint_ids=joint_ids, ) - sim.reset() - assert articulation.is_initialized - - if ordering_axis == "joint": - assert articulation.joint_ordering is not None - assert articulation.body_ordering is None - assert articulation.data._jacobian_joint_user_to_backend is not None - assert articulation.data._jacobian_body_user_to_backend is None - else: - assert articulation.joint_ordering is None - assert articulation.body_ordering is not None - assert articulation.data._jacobian_joint_user_to_backend is None - assert articulation.data._jacobian_body_user_to_backend is not None - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_floating_base_non_root(sim, num_articulations, device, add_ground_plane): - """Test initialization for a floating-base with articulation root on a rigid body. - - 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="humanoid", stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - - # Check if articulation is initialized - assert articulation.is_initialized - # Check that is fixed 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, 21) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [path.split("/")[-1] for path in articulation.root_view.link_paths[0]] - assert prim_path_body_names == articulation.body_names - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_floating_base(sim, num_articulations, device, add_ground_plane): - """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="anymal", stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [path.split("/")[-1] for path in articulation.root_view.link_paths[0]] - assert prim_path_body_names == articulation.body_names - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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("device", test_devices()) -def test_initialization_fixed_base(sim, num_articulations, device): - """Test initialization for fixed base. - - 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="panda") - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 9) - 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) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [path.split("/")[-1] for path in articulation.root_view.link_paths[0]] - assert prim_path_body_names == articulation.body_names - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_fixed_base_single_joint(sim, num_articulations, device, add_ground_plane): - """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="single_joint_implicit") - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [path.split("/")[-1] for path in articulation.root_view.link_paths[0]] - assert prim_path_body_names == articulation.body_names - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_hand_with_tendons(sim, num_articulations, device): - """Test initialization for fixed base articulated hand with tendons. - - 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 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="shadow_hand") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 24) - 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) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert actuator.is_implicit_model == 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("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_floating_base_made_fixed_base(sim, num_articulations, device, add_ground_plane): - """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="anymal").copy() - # Fix root link by making it kinematic - articulation_cfg.spawn.articulation_props.fix_root_link = True - articulation, translations = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [path.split("/")[-1] for path in articulation.root_view.link_paths[0]] - assert prim_path_body_names == articulation.body_names - - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_initialization_fixed_base_made_floating_base(sim, num_articulations, device, add_ground_plane): - """Test initialization for fixed base made floating-base using schema properties. - - This test verifies that: - 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 - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="panda").copy() - # Unfix root link by making it non-kinematic - articulation_cfg.spawn.articulation_props.fix_root_link = False - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - # Check if articulation is initialized + articulation.write_data_to_sim() + raw_target_after = wp.to_torch(articulation.root_view.get_dof_position_targets()) + backend_joint_id = articulation.joint_ordering.user_to_backend_indices[0] + torch.testing.assert_close(raw_target_after[1, backend_joint_id], drive_target[0, 0]) + torch.testing.assert_close(raw_target_after[0], raw_target_before[0]) + for _ in range(8): + articulation.write_data_to_sim() + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + assert torch.abs(articulation.data.joint_pos.torch[1, 0] - initial_drive_position[1]) > 1e-6 + jacobian = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobian.shape == (_NUM_ENVS, articulation.num_bodies - 1, 6, articulation.num_joints) + assert mass_matrix.shape == (_NUM_ENVS, articulation.num_joints, articulation.num_joints) + assert jacobian.device.type == torch.device(articulation_scene.device).type + assert mass_matrix.device.type == torch.device(articulation_scene.device).type + assert torch.isfinite(jacobian).all() + assert torch.isfinite(mass_matrix).all() + torch.testing.assert_close(mass_matrix, mass_matrix.transpose(-1, -2), atol=1e-5, rtol=1e-5) + + +def test_floating_articulation_root_and_wrench_response(articulation_scene: _ArticulationScene) -> None: + """Prove floating-root COM/link state and a real external-wrench response.""" + articulation = articulation_scene.floating + device = articulation_scene.device assert articulation.is_initialized - # 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) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 9) - - # Check some internal physx data for debugging - # -- joint related - assert articulation.root_view.max_dofs == articulation.root_view.shared_metatype.dof_count - # -- link related - assert articulation.root_view.max_links == articulation.root_view.shared_metatype.link_count - # -- link names (check within articulation ordering is correct) - prim_path_body_names = [path.split("/")[-1] for path in articulation.root_view.link_paths[0]] - assert prim_path_body_names == articulation.body_names - - # 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("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_ground_plane): - """Test that the default joint position from configuration is out of range. - - This test verifies that: - 1. The articulation fails to initialize when joint positions are out of range - 2. The error is properly handled - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="panda").copy() - articulation_cfg.init_state.joint_pos = { - "panda_joint1": 10.0, - "panda_joint[2, 4]": -20.0, - } - - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(ValueError): - sim.reset() - - -@pytest.mark.parametrize("device", test_devices()) -def test_out_of_range_default_joint_vel(sim, device): - """Test that the default joint velocity from configuration is out of range. - - This test verifies that: - 1. The articulation fails to initialize when joint velocities are out of range - 2. The error is properly handled - """ - articulation_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot") - articulation_cfg.init_state.joint_vel = { - "panda_joint1": 100.0, - "panda_joint[2, 4]": -60.0, - } - articulation = Articulation(articulation_cfg) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(ValueError): - sim.reset() - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): - """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="panda") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Play sim - 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 articulation.data.root_link_pose_w.torch.shape == (_NUM_ENVS, 7) + assert articulation.data.root_com_pose_w.torch.shape == (_NUM_ENVS, 7) + assert articulation.data.body_link_pose_w.torch.shape == (_NUM_ENVS, articulation.num_bodies, 7) + assert articulation.data.body_com_pose_w.torch.shape == (_NUM_ENVS, articulation.num_bodies, 7) + assert torch.isfinite(articulation.data.root_com_pose_w.torch).all() + + env_ids = torch.tensor([1], dtype=torch.int32, device=device) + initial_pose = articulation.data.root_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids].clone() + target_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3], device=device) + articulation.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[env_ids], target_pose) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_pose[:1]) + + initial_velocity = articulation.data.root_com_lin_vel_w.torch.clone() + articulation.permanent_wrench_composer.set_forces_and_torques_index( + forces=torch.tensor([[[8.0, 0.0, 0.0]]], device=device), + torques=torch.zeros((1, 1, 3), device=device), + env_ids=env_ids, + body_ids=torch.tensor([0], dtype=torch.int32, device=device), ) - assert torch.all(within_bounds) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_joint_effort_limits(sim, num_articulations, device, add_ground_plane): - """Validate joint effort limits via joint_effort_out_of_limit().""" - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) - - # Minimal env wrapper exposing scene["robot"] - class _Env: - def __init__(self, art): - self.scene = {"robot": art} + articulation.write_data_to_sim() + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + assert articulation.data.root_com_lin_vel_w.torch[1, 0] > initial_velocity[1, 0] + torch.testing.assert_close(articulation.data.root_com_lin_vel_w.torch[0], initial_velocity[0], atol=1e-6, rtol=0) - env = _Env(articulation) - robot_all = SceneEntityCfg(name="robot") - sim.reset() +def test_spatial_tendon_properties_round_trip(articulation_scene: _ArticulationScene) -> None: + """Prove a locally authored spatial tendon is discovered and writable.""" + articulation = articulation_scene.tendon 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(sim, num_articulations, device): - """Test if external force buffer correctly updates in the force value is zero case. - - This test verifies that: - 1. External forces can be applied correctly - 2. Force buffers are updated properly - 3. Zero forces are handled correctly - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # play the simulator - 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) - - # reset articulation - articulation.reset() - - # 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, - ) - - # 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) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body(sim, num_articulations, device): - """Test application of external force on the base of the articulation. - - 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 - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # Play the simulator - 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[..., 1] = 1000.0 - - # Now we are ready! - for _ in range(5): - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(sim, num_articulations, device): - """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="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # Play the simulator - 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] = 500.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] = 1000.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 1000.0 - - # Now we are ready! - for i in range(5): - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_multiple_bodies(sim, num_articulations, device): - """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="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - 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] = 100.0 - - # Now we are ready! - for _ in range(5): - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device): - """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="anymal") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - 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] = 500.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] = 1000.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 1000.0 - - # Now we are ready! - for i in range(5): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - 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() - - 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 - - # apply force - 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 - for i in range(num_articulations): - # since there is a moment applied on the articulation, the articulation should rotate - assert torch.abs(articulation.data.root_ang_vel_w.torch[i, 2]).item() > 0.1 - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_loading_gains_from_usd(sim, num_articulations, device): - """Test that gains are loaded from USD file if actuator model has them as None. - - This test verifies that: - 1. Gains are loaded correctly from USD file - 2. Default gains are applied when not specified - 3. The gains match the expected values - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid", stiffness=None, damping=None) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play sim - sim.reset() - - # Expected gains - # -- Stiffness values - expected_stiffness = { - ".*_waist.*": 20.0, - ".*_upper_arm.*": 10.0, - "pelvis": 10.0, - ".*_lower_arm": 2.0, - ".*_thigh:0": 10.0, - ".*_thigh:1": 20.0, - ".*_thigh:2": 10.0, - ".*_shin": 5.0, - ".*_foot.*": 2.0, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_stiffness, articulation.joint_names - ) - expected_stiffness = torch.zeros(articulation.num_instances, articulation.num_joints, device=articulation.device) - expected_stiffness[:, indices_list] = torch.tensor(values_list, device=articulation.device) - # -- Damping values - expected_damping = { - ".*_waist.*": 5.0, - ".*_upper_arm.*": 5.0, - "pelvis": 5.0, - ".*_lower_arm": 1.0, - ".*_thigh:0": 5.0, - ".*_thigh:1": 5.0, - ".*_thigh:2": 5.0, - ".*_shin": 0.1, - ".*_foot.*": 1.0, - } - indices_list, _, values_list = string_utils.resolve_matching_names_values( - expected_damping, articulation.joint_names - ) - expected_damping = torch.zeros_like(expected_stiffness) - expected_damping[:, indices_list] = torch.tensor(values_list, device=articulation.device) - - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane): - """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="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device - ) - - # Play sim - 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_setting_gains_from_cfg_dict(sim, num_articulations, device): - """Test that gains are loaded from the configuration dictionary correctly. - - This test verifies that: - 1. Gains are loaded correctly from configuration dictionary - 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="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device - ) - # Play sim - 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("device", test_devices()) -@pytest.mark.parametrize("joint_velocity_limit", [1e5, None]) -def test_setting_velocity_limit_writes_to_solver(sim, device, joint_velocity_limit): - """Test that the resolved joint velocity limit reaches the PhysX solver. - - The full limit-resolution matrix (config override vs. USD default, implicit and explicit - actuators, actuator-limit soft fallback) is covered on the Newton backend and at unit - level. This smoke test only verifies the PhysX write path: the configured limit (or the - USD-authored default when unset) lands in the native solver buffers and matches - ``data.joint_vel_limits``. - """ - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - joint_velocity_limit=joint_velocity_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=1, - device=device, - ) - # Play sim - sim.reset() - - # read the values set into the simulation - physx_vel_limit = wp.to_torch(articulation.root_view.get_dof_max_velocities()).to(device) - # check data buffer - torch.testing.assert_close(articulation.data.joint_vel_limits.torch, physx_vel_limit) - # the solver clamp comes from joint_velocity_limit when set, otherwise the USD-authored value - if joint_velocity_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_joint_velocity - else: - limit = joint_velocity_limit - expected_velocity_limit = torch.full_like(physx_vel_limit, limit) - torch.testing.assert_close(physx_vel_limit, expected_velocity_limit) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("joint_effort_limit", [1e5, None]) -def test_setting_effort_limit_writes_to_solver(sim, device, joint_effort_limit): - """Test that the resolved joint effort limit reaches the PhysX solver. - - The full limit-resolution matrix (config override vs. USD default, implicit and explicit - actuators, actuator-limit soft fallback) is covered on the Newton backend and at unit - level. This smoke test only verifies the PhysX write path: the configured limit (or the - USD-authored default when unset) lands in the native solver buffers and matches - ``data.joint_effort_limits``. - """ - 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=1, - device=device, - ) - # Play sim - sim.reset() - - # obtain the physx effort limits - physx_effort_limit = wp.to_torch(articulation.root_view.get_dof_max_forces()).to(device=device) - # check data buffer - torch.testing.assert_close(articulation.data.joint_effort_limits.torch, physx_effort_limit) - # the solver keeps the USD-authored limit unless the user overrides it explicitly - if joint_effort_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_force - else: - limit = joint_effort_limit - expected_effort_limit = torch.full_like(physx_effort_limit, limit) - torch.testing.assert_close(physx_effort_limit, expected_effort_limit) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset(sim, num_articulations, device, monkeypatch): - """Test that reset method works properly.""" - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Now we are ready! - actuator = next(iter(articulation.actuators.values())) - actuator_reset = actuator.reset - reset_env_ids = [] - - def record_actuator_reset(env_ids=None): - reset_env_ids.append(env_ids) - actuator_reset(env_ids) - - monkeypatch.setattr(actuator, "reset", record_actuator_reset) - 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 - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): - """Test applying of joint position target functions correctly for a robotic arm.""" - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - 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) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -def test_body_root_state(sim, num_articulations, device, with_offset): - """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 - - 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="single_joint_implicit") - 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) - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10, "Possible reference leak for articulation" - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized, "Articulation is not initialized" - # Check that fixed base - assert articulation.is_fixed_base, "Articulation is not a fixed base" - - # 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] - - # create com offsets — apply offset to the Arm body - num_bodies = articulation.num_bodies - com = wp.to_torch(articulation.root_view.get_coms()) - link_offset = [1.0, 0.0, 0.0] # the offset from CenterPivot to Arm frames - new_com = torch.tensor(offset, device=device).repeat(num_articulations, 1, 1) - com[:, arm_idx, :3] = new_com.squeeze(-2) - articulation.set_coms_index( - coms=wp.from_torch(com.to(device).contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), - ) - - # check they are set - torch.testing.assert_close(wp.to_torch(articulation.root_view.get_coms()), com.cpu()) - - for i in range(50): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # 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 - body_link_pose_w = articulation.data.body_link_pose_w.torch - body_link_vel_w = articulation.data.body_link_vel_w.torch - 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", [1, 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]) -def test_write_root_state(sim, num_articulations, device, with_offset, state_location, gravity_enabled): - """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="anymal") - 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 - sim.reset() - - # 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_coms()) - new_com = offset - com[:, 0, :3] = new_com.squeeze(-2) - articulation.set_coms_index( - coms=wp.from_torch(com.to(device).contiguous(), dtype=wp.transformf), - env_ids=wp.from_torch(env_idx, dtype=wp.int32), - ) - - # check they are set - torch.testing.assert_close(wp.to_torch(articulation.root_view.get_coms()), 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) - - -@pytest.mark.parametrize("device", test_devices()) -def test_setting_articulation_root_prim_path(sim, device): - """Test that the articulation root prim path can be set explicitly.""" - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation_cfg.articulation_root_prim_path = "/torso" - articulation, _ = generate_articulation(articulation_cfg, 1, device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - sim.reset() - # Check if articulation is initialized - assert articulation._is_initialized - - -@pytest.mark.parametrize("device", test_devices()) -def test_setting_invalid_articulation_root_prim_path(sim, device): - """Test that the articulation root prim path can be set explicitly.""" - sim._app_control_on_stop_handle = None - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") - articulation_cfg.articulation_root_prim_path = "/non_existing_prim_path" - articulation, _ = generate_articulation(articulation_cfg, 1, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_write_joint_state_data_consistency(sim, num_articulations, device, gravity_enabled): - """Test the setters for root_state using both the link frame and center of mass as reference frame. - - 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 - device: The device to run the simulation on - """ - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type="anymal") - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) - env_idx = torch.tensor([x for x in range(num_articulations)]) - - # Play sim - sim.reset() - - 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) - - from torch.distributions import Uniform - - joint_pos_limits = articulation.data.joint_pos_limits.torch - joint_vel_limits = articulation.data.joint_vel_limits.torch - pos_dist = Uniform(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) - vel_dist = Uniform(-joint_vel_limits, joint_vel_limits) - - original_body_link_pose_w = articulation.data.body_link_pose_w.torch.clone() - original_body_com_vel_w = articulation.data.body_com_vel_w.torch.clone() - - rand_joint_pos = pos_dist.sample() - rand_joint_vel = vel_dist.sample() - - articulation.write_joint_position_to_sim_index(position=rand_joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=rand_joint_vel) - # make sure valued updated - body_link_pose_w = articulation.data.body_link_pose_w.torch - body_com_vel_w = articulation.data.body_com_vel_w.torch - original_body_states = torch.cat([original_body_link_pose_w, original_body_com_vel_w], dim=-1) - body_state_w = torch.cat([body_link_pose_w, body_com_vel_w], dim=-1) - 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 - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # validate link - com conistency - body_com_pos_b = articulation.data.body_com_pos_b.torch - body_com_quat_b = articulation.data.body_com_quat_b.torch - expected_com_pos, expected_com_quat = math_utils.combine_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:].view(-1, 4), - 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("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_spatial_tendons(sim, num_articulations, device): - """Test spatial tendons apis. - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation has spatial tendons - 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 - """ - # skip test if Isaac Sim version is less than 5.0 - if has_kit() and get_isaac_sim_version().major < 5: - pytest.skip("Spatial tendons are not supported in Isaac Sim < 5.0. Please update to Isaac Sim 5.0 or later.") - return - articulation_cfg = generate_articulation_cfg(articulation_type="spatial_tendon_test_asset") - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - 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, 3) - 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) + assert articulation.num_instances == _NUM_ENVS assert articulation.num_spatial_tendons == 1 - - articulation.set_spatial_tendon_stiffness_index(stiffness=10.0) - articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=10.0) - articulation.set_spatial_tendon_damping_index(damping=10.0) - articulation.set_spatial_tendon_offset_index(offset=10.0) - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground_plane): - """Test applying of joint position target functions correctly for a robotic arm.""" - articulation_cfg = generate_articulation_cfg(articulation_type="panda") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # apply action to the articulation - dynamic_friction = torch.rand(num_articulations, articulation.num_joints, device=device) - viscous_friction = torch.rand(num_articulations, articulation.num_joints, device=device) - friction = torch.rand(num_articulations, articulation.num_joints, device=device) - - # Guarantee that the dynamic friction is not greater than the static friction - dynamic_friction = torch.min(dynamic_friction, friction) - - # The static friction must be set first to be sure the dynamic friction is not greater than static - # when both are set. - articulation.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=friction, - joint_dynamic_friction_coeff=dynamic_friction, - joint_viscous_friction_coeff=viscous_friction, - ) - articulation.write_data_to_sim() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - friction_props_from_sim = wp.to_torch(articulation.root_view.get_dof_friction_properties()) - joint_friction_coeff_sim = friction_props_from_sim[:, :, 0] - joint_dynamic_friction_coeff_sim = friction_props_from_sim[:, :, 1] - joint_viscous_friction_coeff_sim = friction_props_from_sim[:, :, 2] - assert torch.allclose(joint_dynamic_friction_coeff_sim, dynamic_friction.cpu()) - assert torch.allclose(joint_viscous_friction_coeff_sim, viscous_friction.cpu()) - assert torch.allclose(joint_friction_coeff_sim, friction.cpu()) - - # For Isaac Sim >= 5.0: also test the combined API that can set dynamic and viscous via - # write_joint_friction_coefficient_to_sim; reset the sim to isolate this path. - if has_kit() and get_isaac_sim_version().major >= 5: - # Reset simulator to ensure a clean state for the alternative API path - sim.reset() - - # Warm up a few steps to populate buffers - for _ in range(100): - sim.step() - articulation.update(sim.cfg.dt) - - # New random coefficients - dynamic_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) - viscous_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) - friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) - - # Guarantee that the dynamic friction is not greater than the static friction - dynamic_friction_2 = torch.min(dynamic_friction_2, friction_2) - - # Use the combined setter to write all three at once - articulation.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=friction_2, - joint_dynamic_friction_coeff=dynamic_friction_2, - joint_viscous_friction_coeff=viscous_friction_2, - ) - articulation.write_data_to_sim() - - # Step to let sim ingest new params and refresh data buffers - for _ in range(100): - sim.step() - articulation.update(sim.cfg.dt) - - friction_props_from_sim_2 = wp.to_torch(articulation.root_view.get_dof_friction_properties()) - joint_friction_coeff_sim_2 = friction_props_from_sim_2[:, :, 0] - friction_dynamic_coef_sim_2 = friction_props_from_sim_2[:, :, 1] - friction_viscous_coeff_sim_2 = friction_props_from_sim_2[:, :, 2] - - # Validate values propagated - assert torch.allclose(friction_viscous_coeff_sim_2, viscous_friction_2.cpu()) - assert torch.allclose(friction_dynamic_coef_sim_2, dynamic_friction_2.cpu()) - assert torch.allclose(joint_friction_coeff_sim_2, friction_2.cpu()) - - -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_set_material_properties(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test getting and setting material properties (friction/restitution) of articulation shapes.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Get number of shapes from the articulation - max_shapes = articulation.root_view.max_shapes - - # Generate random material properties: (static_friction, dynamic_friction, restitution) - materials = torch.empty(num_articulations, max_shapes, 3, device="cpu").uniform_(0.0, 1.0) - # Ensure dynamic friction <= static friction - materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) - - # Set material properties via the PhysX view-level API - env_ids = torch.arange(num_articulations, dtype=torch.int32) - articulation.root_view.set_material_properties( - wp.from_torch(materials, dtype=wp.float32), wp.from_torch(env_ids, dtype=wp.int32) - ) - - # Simulate physics - sim.step() - articulation.update(sim.cfg.dt) - - # Get material properties from simulation - materials_check = wp.to_torch(articulation.root_view.get_material_properties()) - - # Check if material properties are set correctly - torch.testing.assert_close(materials_check, materials) - - -## -# Shape-contract regression tests for the new BaseArticulation accessors. -# Mirror the Newton-side tests so both backends can be diffed against the -# same documented contract. These are PhysX's reference shapes — when the -# Newton-side tests pass with the same expected_shape formulas, the -# cross-backend contract holds. -## - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_jacobians_shape_fixed_base(sim, num_articulations, device, articulation_type): - """PhysX reference: fixed-base ``body_link_jacobian_w`` is ``(N, num_bodies-1, 6, num_joints)``.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base - - J = articulation.data.body_link_jacobian_w.torch - expected = (num_articulations, articulation.num_bodies - 1, 6, articulation.num_joints) - assert J.shape == torch.Size(expected), f"expected {expected}, got {tuple(J.shape)}" - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations, device, articulation_type): - """PhysX reference: fixed-base ``mass_matrix`` shape + non-singular.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - sim.step() - articulation.update(sim.cfg.dt) - - M = articulation.data.mass_matrix.torch - expected = (num_articulations, articulation.num_joints, articulation.num_joints) - assert M.shape == torch.Size(expected), f"expected {expected}, got {tuple(M.shape)}" - - # Each diagonal entry is the joint's effective inertia and must be positive - # for any physical articulation. Padded zero rows/cols (the bug) would show - # up here as zero diagonal entries — much more sensitive than checking the - # determinant, which can be small for a well-conditioned 9x9 just from - # numerical cancellation. - diag = M.diagonal(dim1=-2, dim2=-1) - assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" - - -@pytest.mark.parametrize("num_articulations", [1, 4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -@pytest.mark.isaacsim_ci -def test_get_jacobians_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): - """PhysX reference: floating-base ``body_link_jacobian_w``. - - Floating-base articulations include the 6 floating-base spatial-velocity columns - at the front of the DoF axis, so the shape is - ``(N, num_bodies, 6, num_joints + num_base_dofs)`` — matching Newton and the - cross-library industry convention (Pinocchio, Drake, MuJoCo, RBDL, OCS2, - iDynTree). - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - assert not articulation.is_fixed_base - - J = articulation.data.body_link_jacobian_w.torch - expected = (num_articulations, articulation.num_bodies, 6, articulation.num_joints + articulation.num_base_dofs) - assert J.shape == torch.Size(expected), f"expected {expected}, got {tuple(J.shape)}" - - -@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): - """PhysX reference: ``J · q_dot`` matches ``[body_link_lin_vel_w; body_link_ang_vel_w]``. - - The cross-backend contract on - :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` says - the Jacobian's linear rows reference each body's link origin. PhysX's - raw ``_root_view.get_jacobians()`` returns COM-referenced linear rows; - the IsaacLab wrapper applies the COM→origin shift kernel so the contract - holds. This test pins the identity from the PhysX side and parametrizes - on Anymal so the (non-trivial) shift surfaces if it ever regresses. - - Scene gravity is disabled (``gravity_enabled=False``) so the only source - of a J · q_dot ↔ body_*_w mismatch is the reference-point contract (or a - regression). The tolerance ``5e-2`` is loose enough to absorb the small - PhysX state-propagation lag between the Jacobian and the velocity - buffers (~2% on max angular speed) but well below the - COM-vs-link-origin bug magnitude (panda hand COM offset ≈ 3 cm × ω at - typical motion ≈ several rad/s gives a 0.1+ m/s linear-row residual, - 2× the tolerance). - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - torch.manual_seed(0) - qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 - articulation.write_joint_velocity_to_sim(velocity=qdot) - sim.step() - 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) - - body_lin_w = articulation.data.body_link_lin_vel_w.torch - body_ang_w = articulation.data.body_link_ang_vel_w.torch - if articulation.is_fixed_base: - body_lin_w = body_lin_w[:, 1:] - body_ang_w = body_ang_w[:, 1:] - - torch.testing.assert_close(v_pred[..., 3:6], body_ang_w, atol=1.5e-1, rtol=5e-2) - torch.testing.assert_close(v_pred[..., 0:3], body_lin_w, atol=1.5e-1, rtol=5e-2) - - -@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_mass_matrix_symmetry_pd(sim, num_articulations, device, articulation_type, gravity_enabled): - """The joint-space mass matrix ``M(q)`` must be square, symmetric, and positive-definite. - - Mirrors the Newton-side test in - ``source/isaaclab_newton/test/assets/test_articulation.py``. Pins - three structural properties of :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix` - that every backend must satisfy. Both backends include the 6 floating-base - rows/cols on floating-base assets (matching the cross-library industry - convention); this test cares about square + symmetric + PD across both - fixed- and floating-base, not the absolute column count. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - sim.step() - articulation.update(sim.cfg.dt) - - M = articulation.data.mass_matrix.torch # (N, J, J) - assert M.dim() == 3, f"expected 3-D mass matrix, got shape {tuple(M.shape)}" - assert M.shape[0] == num_articulations - assert M.shape[1] == M.shape[2], f"mass matrix is not square: {tuple(M.shape)}" - - 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" - - eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) - torch.linalg.cholesky(M + 1e-6 * eye) - - -@pytest.mark.parametrize("num_articulations", [1]) -@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_jacobian_refreshes_after_manual_joint_write( - sim, num_articulations, device, articulation_type, gravity_enabled -): - """After ``write_joint_position_to_sim_index`` (no sim step), the Jacobian read - must reflect the new joint state — not the previous one. - - PhysX-side counterpart to the Newton test of the same name. PhysX's - :attr:`body_link_jacobian_w` triggers FK indirectly through - :attr:`body_link_pose_w` (used by the shift kernel); :attr:`body_com_jacobian_w` is - a passthrough to ``_root_view.get_jacobians()``. This test confirms that PhysX's - tensor view returns up-to-date Jacobians after a manual joint write — i.e., that - PhysX internally refreshes FK on ``get_jacobians`` (or that our property does). - Failure means we need to add ``update_articulations_kinematic()`` before the - passthrough. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) - - # Read J at the baseline joint state. - J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() - J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() - - # Manually write a different joint state — large delta to make the change visible. - # No sim.step / update — FK becomes stale. - q_target = articulation.data.joint_pos.torch.clone() + 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) - - # Read J again. With the FK trigger, J reflects q_target and differs from J at baseline. - # Without the trigger, body_q stays at baseline, J unchanged. - J_link_1 = articulation.data.body_link_jacobian_w.torch.clone() - J_com_1 = articulation.data.body_com_jacobian_w.torch.clone() - - assert not torch.allclose(J_link_0, J_link_1, atol=1e-3), ( - "body_link_jacobian_w did not change after manual joint write — " - "FK trigger likely missing (eval_jacobian / shift kernel reading stale state.body_q)." - ) - assert not torch.allclose(J_com_0, J_com_1, atol=1e-3), ( - "body_com_jacobian_w did not change after manual joint write — " - "PhysX get_jacobians may not auto-refresh FK; consider adding update_articulations_kinematic()." - ) - - -@pytest.mark.parametrize("num_articulations", [1]) -@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_mass_matrix_refreshes_after_manual_joint_write( - sim, num_articulations, device, articulation_type, gravity_enabled -): - """After ``write_joint_position_to_sim_index`` (no sim step), the mass matrix read - must reflect the new joint state. - - PhysX-side counterpart. :attr:`mass_matrix` is a passthrough to - ``_root_view.get_generalized_mass_matrices()``. Failure means PhysX's tensor view - does not auto-refresh FK on this getter, and we need to add - ``update_articulations_kinematic()`` before the passthrough. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) - - M_0 = articulation.data.mass_matrix.torch.clone() - q_target = articulation.data.joint_pos.torch.clone() + 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) - M_1 = articulation.data.mass_matrix.torch.clone() - - assert not torch.allclose(M_0, M_1, atol=1e-3), ( - "mass_matrix did not change after manual joint write — " - "PhysX get_generalized_mass_matrices may not auto-refresh FK." - ) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulations, device, articulation_type): - """PhysX accuracy: ``τ_gc`` must hold the manipulator in static equilibrium. - - The contract is the EOM identity ``M(q) q̈ + C(q,q̇) q̇ + g(q) = τ_input``. - Setting ``τ_input = g(q)`` at ``q̇ = 0`` gives ``q̈ = 0`` — the arm should - not move. This pins - :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` - in isolation: sign errors, frame errors, and DoF-ordering errors all - surface as joint drift, while a controller-level test would have those - bugs averaged out by PD damping. - - Newton-side variant of the same name lives in - ``isaaclab_newton/test/assets/test_articulation.py`` (backend parity). - """ - base_cfg = generate_articulation_cfg(articulation_type=articulation_type) - # Replace default Franka actuators with a passthrough implicit actuator - # (stiffness = 0, damping = 0). With both gains zero the effort target - # we set IS the joint torque applied — no PD spring-damper masks the - # gravity-comp signal. Default Franka cfg has stiffness=80 / damping=4 - # which would absorb gravity through PD bias and hide accessor bugs. - cfg = base_cfg.replace( - actuators={ - "all": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=0.0, - damping=0.0, - ), - }, - ) - # FRANKA_PANDA_CFG has rigid_props.disable_gravity=False already, but be - # defensive — gravity must be ON for τ_gc to have anything to cancel. - cfg = cfg.replace( - spawn=cfg.spawn.replace( - rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False), - ), - ) - - articulation, _ = generate_articulation(cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - # Force a clean static state: default joint positions, zero velocities. - # ``sim.reset`` may leave residual ``q_dot`` from solver settling under - # gravity, so we pin it explicitly here. - default_q = articulation.data.default_joint_pos.torch.clone() - default_qd = torch.zeros_like(default_q) - articulation.write_joint_state_to_sim(default_q, default_qd) - articulation.update(sim.cfg.dt) - - # Default joint pose from FRANKA_PANDA_CFG bends the elbow - # (joint2=-0.569, joint4=-2.81, joint6=3.04) so several links carry a - # gravity load — τ_gc is non-trivial in this configuration. A natural- - # hang pose (all zeros) would produce near-zero τ_gc and make this - # test uninformative. - init_q = articulation.data.joint_pos.torch.clone() - - # Step 100 times applying only τ_gc as joint efforts. - for _ in range(100): - # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)`` - # — leading ``num_base_dofs`` floating-base entries (0 on fixed-base) followed - # by the actuated-joint entries. Slice past the floating-base entries so the - # remaining tensor aligns with ``set_joint_effort_target`` (actuated only). - tau_gc = articulation.data.gravity_compensation_forces.torch[:, articulation.num_base_dofs :] - articulation.set_joint_effort_target(tau_gc) - articulation.write_data_to_sim() - sim.step() - articulation.update(sim.cfg.dt) - - final_q = articulation.data.joint_pos.torch - drift = (final_q - init_q).abs().max() - # Tight bound: 5e-3 rad ≈ 0.3°. Numerical integration over 100 steps will - # accumulate some floor (sub-millirad on Franka), but a sign or frame bug - # in τ_gc produces drift of at least a degree per step on bent-elbow - # poses. This bound separates "correct" from "broken" cleanly. - assert drift < 5e-3, ( - f"max joint drift {drift:.5f} rad after 100 gravity-comp-only steps —" - " τ_gc did not hold static equilibrium. Check sign, DoF ordering, and" - " whether gravity_compensation_forces returns g(q) (positive) or" - " its negation." - ) - - -@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): - """PhysX-side IK convergence sentinel — backend parity with the Newton test. - - Mirrors :func:`isaaclab_newton.test.assets.test_articulation.test_franka_ik_tracking_accuracy` - so both backends are pinned by the same IK trajectory. With the - robot teleported to its configured init_state home pose and scene - gravity off, PhysX's IK converges to ~mm precision on this 5 cm - Cartesian step. A bridge regression (wrong J shape, wrong DoF - ordering) would push the steady-state error well past the - threshold. - """ - 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(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") - - # Assert on tail mean (not min) so an oscillating envelope can't - # squeeze through. Threshold matched to the Newton-side test - # (5 mm / 0.05 rad). - 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]) -@pytest.mark.isaacsim_ci -def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_enabled): - """PhysX-side OSC pose tracking sentinel — backend parity with Newton. - - Mirrors :func:`isaaclab_newton.test.assets.test_articulation.test_franka_osc_tracking_accuracy`. - Zero out the actuator's PD gains so OSC's joint-effort output is - not opposed by the implicit-PD term, matching the Newton test setup. - """ - robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim, zero_actuator_pd=True) - - osc = OperationalSpaceController( - OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ), - num_envs=1, - device=device, - ) - - 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) - - pos_history: list[float] = [] - rot_history: list[float] = [] - for _ in range(800): - jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) - mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] - ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) - joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] - ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) - - osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) - joint_efforts = osc.compute( - jacobian_b=jacobian_b, - current_ee_pose_b=ee_pose_b, - current_ee_vel_b=ee_vel_b, - mass_matrix=mass_matrix, - gravity=None, - ) - - robot.set_joint_effort_target(joint_efforts, 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(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") - - # Assert on tail mean. Threshold matched to the Newton-side test - # (5 mm / 0.05 rad). Both backends converge to machine precision - # with proper ee-velocity feedback (``J · q_dot``). - assert pos_mean < 5e-3, f"OSC pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" - assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" - - -def _run_osc_stay_still_under_gravity( - sim, - device: str, - *, - gravity_compensation_enabled: bool, - num_steps: int = 100, -): - """Run OSC with a stay-still target on Franka under gravity, return EE drift summary. - - Shared helper for the gravity-comp tests. Setup mirrors - :func:`test_franka_osc_tracking_accuracy` (zero actuator PD so OSC's joint-effort - output is not opposed by an implicit-PD spring), but with scene gravity ON and the - target = the EE pose captured after the first sim step (which already includes a - fraction-of-a-mm of gravity-induced motion; that's the baseline drift starts from). - - Args: - gravity_compensation_enabled: If ``True``, the OSC controller cfg has - ``gravity_compensation=True`` and ``osc.compute(gravity=g(q))`` receives - the data-layer ``gravity_compensation_forces`` slice. If ``False``, - ``gravity_compensation=False`` and ``gravity=None``. - - Returns: - Tuple ``((pos_min, pos_mean), (rot_min, rot_mean))`` over the last 20% of - steps (per :func:`_summarize_history`), where ``pos`` is in meters and - ``rot`` in radians. - """ - # Enable rigid-body gravity so the arm actually feels weight. - # ``FRANKA_PANDA_HIGH_PD_CFG`` defaults ``disable_gravity=True`` for IK/OSC tests. - robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( - sim, zero_actuator_pd=True, enable_rigid_body_gravity=True - ) - - osc = OperationalSpaceController( - OperationalSpaceControllerCfg( - target_types=["pose_abs"], - impedance_mode="fixed", - inertial_dynamics_decoupling=True, - partial_inertial_dynamics_decoupling=False, - gravity_compensation=gravity_compensation_enabled, - motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, - ), - num_envs=1, - device=device, - ) - - sim.step() - robot.update(sim.cfg.dt) - - # Stay-still target = current EE pose in root frame, captured right after the - # first step. The OSC loop must hold this pose under gravity. - initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - target_pose_b = torch.cat([initial_ee_pos_b, initial_ee_quat_b], dim=-1) - - pos_history: list[float] = [] - rot_history: list[float] = [] - for _ in range(num_steps): - jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) - mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] - ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) - joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] - ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) - - # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``; - # slice past the leading floating-base columns (0 for fixed-base Franka, so a - # no-op here, but the pattern matches the action-term convention). - gravity = ( - robot.data.gravity_compensation_forces.torch[:, [j + robot.num_base_dofs for j in arm_joint_ids]] - if gravity_compensation_enabled - else None - ) - - osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) - joint_efforts = osc.compute( - jacobian_b=jacobian_b, - current_ee_pose_b=ee_pose_b, - current_ee_vel_b=ee_vel_b, - mass_matrix=mass_matrix, - gravity=gravity, - ) - robot.set_joint_effort_target(joint_efforts, 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()) - - return _summarize_history(pos_history), _summarize_history(rot_history) - - -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.parametrize("gravity_enabled", [True]) -@pytest.mark.isaacsim_ci -def test_franka_osc_gravity_compensation_holds_under_gravity(sim, device, articulation_type, gravity_enabled): - """OSC with ``gravity_compensation=True`` must hold the EE pose under gravity. - - With scene gravity ON and zero actuator PD (so OSC torques are not opposed by an - implicit-PD spring), passing - :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` through - ``osc.compute(gravity=...)`` should keep the arm at the initial pose. - - Pins three things that the existing direct-primitive - :func:`test_get_gravity_compensation_forces_static_equilibrium` does not: - 1. OSC's ``_jacobi_joint_idx`` indexing — the ``+ num_base_dofs`` shift. - 2. OSC's :meth:`OperationalSpaceController.compute` correctly adds ``g(q)`` to - its torque output. - 3. The data-property ``gravity_compensation_forces`` is reachable from the OSC - pipeline (catches gating regressions in - :meth:`OperationalSpaceControllerAction._compute_dynamic_quantities`). - - Companion test :func:`test_franka_osc_no_gravity_compensation_sags_under_gravity` - runs the same setup with ``gravity_compensation=False`` and reports the - uncompensated drift magnitude — a sanity check that gravity is loading the arm. - """ - (pos_min, pos_mean), (rot_min, rot_mean) = _run_osc_stay_still_under_gravity( - sim, device, gravity_compensation_enabled=True - ) - print(f"OSC_GC_ON pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") - - assert pos_mean < 5e-3, f"OSC + gravity_compensation pos_mean {pos_mean:.5f} > 5 mm — regression?" - assert rot_mean < 5e-2, f"OSC + gravity_compensation rot_mean {rot_mean:.5f} > 0.05 rad — regression?" - - -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.parametrize("gravity_enabled", [True]) -@pytest.mark.isaacsim_ci -def test_franka_osc_no_gravity_compensation_sags_under_gravity(sim, device, articulation_type, gravity_enabled): - """OSC without ``gravity_compensation`` under gravity: sanity check that the arm sags. - - Companion to :func:`test_franka_osc_gravity_compensation_holds_under_gravity`. - Same setup, but ``gravity_compensation=False`` and ``osc.compute(gravity=None)``. - With zero actuator PD, OSC's task-space impedance is the only restoring force — - the steady-state solution is whatever pose error the impedance produces enough - joint torque to balance ``g(q)``. - - Asserts the drift is **non-trivially larger** than the with-comp threshold (5 mm). - Without this check, a regression that broke ``gravity_compensation_forces`` by - returning zeros (or a no-op `g(q)`) would pass the with-comp test silently. The - bound here proves gravity is actually loading the arm and the with-comp pass is - meaningful. - """ - (pos_min, pos_mean), (rot_min, rot_mean) = _run_osc_stay_still_under_gravity( - sim, device, gravity_compensation_enabled=False - ) - print(f"OSC_GC_OFF pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") - - # Sanity: with gravity on and no comp, OSC's task-space spring vs gravity-load - # equilibrium produces a non-zero pose error. If this asserts fails, the test - # setup itself is broken (e.g., gravity is not on, or the home pose has no - # gravity load), which would invalidate the with-comp test as well. - assert pos_mean > 5e-3, ( - f"OSC + no gravity_compensation pos_mean {pos_mean:.5f} ≤ 5 mm — gravity not loading the arm?" - ) - - -if __name__ == "__main__": - pytest.main([__file__, "-v", "--maxfail=1"]) + env_ids = torch.tensor([1], dtype=torch.int32, device=articulation_scene.device) + initial_stiffness = articulation.data.spatial_tendon_stiffness.torch.clone() + stiffness = torch.tensor([[12.0]], device=articulation_scene.device) + damping = torch.tensor([[1.5]], device=articulation_scene.device) + limit_stiffness = torch.tensor([[3.0]], device=articulation_scene.device) + offset = torch.tensor([[0.1]], device=articulation_scene.device) + articulation.set_spatial_tendon_stiffness_index(stiffness=stiffness, env_ids=env_ids) + articulation.set_spatial_tendon_damping_index(damping=damping, env_ids=env_ids) + articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=limit_stiffness, env_ids=env_ids) + articulation.set_spatial_tendon_offset_index(offset=offset, env_ids=env_ids) + torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch[env_ids], stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch[:1], initial_stiffness[:1]) + torch.testing.assert_close(articulation.data.spatial_tendon_damping.torch[env_ids], damping) + torch.testing.assert_close(articulation.data.spatial_tendon_limit_stiffness.torch[env_ids], limit_stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_offset.torch[env_ids], offset) diff --git a/source/isaaclab_physx/test/assets/test_deformable_object.py b/source/isaaclab_physx/test/assets/test_deformable_object.py index d1bd725bd618..f29ad8888953 100644 --- a/source/isaaclab_physx/test/assets/test_deformable_object.py +++ b/source/isaaclab_physx/test/assets/test_deformable_object.py @@ -3,343 +3,185 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Launch Isaac Sim Simulator first.""" +"""Minimal stable real-PhysX probes for surface and volume deformables.""" from isaaclab.app import AppLauncher -# launch omniverse app simulation_app = AppLauncher(headless=True).app -"""Rest everything follows.""" - -import sys - import pytest import torch import warp as wp -from flaky import flaky from isaaclab_physx.assets import DeformableObject from isaaclab_physx.sim import ( PhysxDeformableBodyMaterialCfg, - PhysxDeformableBodyPropertiesCfg, PhysxSurfaceDeformableBodyMaterialCfg, ) -import carb +from pxr import Gf, Sdf, UsdGeom, UsdShade import isaaclab.sim as sim_utils -import isaaclab.utils.math as math_utils from isaaclab.assets import DeformableObjectCfg from isaaclab.sim import build_simulation_context -# Temporarily disabled: this suite intermittently aborts with SIGABRT on CI. -# Re-enable once the underlying crash is fixed. -pytestmark = pytest.mark.skip(reason="Temporarily disabled due to intermittent crash on CI.") +pytestmark = pytest.mark.integration -def generate_cubes_scene( - num_cubes: int = 1, - height: float = 1.0, - initial_rot: tuple[float, ...] = (0.0, 0.0, 0.0, 1.0), - has_api: bool = True, - material_path: str | None = "material", - kinematic_enabled: bool = False, - deformable_type: str = "volume", - device: str = "cuda:0", -) -> DeformableObject: - """Generate a scene with the provided number of cubes. +def _add_api_schemas(prim, schemas: list[str]) -> None: + schemas_op = Sdf.TokenListOp() + schemas_op.explicitItems = schemas + prim.SetMetadata("apiSchemas", schemas_op) - Args: - num_cubes: Number of cubes to generate. - height: Height of the cubes. Default is 1.0. - initial_rot: Initial rotation of the cubes (xyzw format). Default is (0.0, 0.0, 0.0, 1.0). - has_api: Whether the cubes have a deformable body API on them. - material_path: Path to the material file. If None, no material is added. Default is "material", - which is path relative to the spawned object prim path. - kinematic_enabled: Whether the cubes are kinematic. - deformable_type: The type of deformable body to spawn. Supported values are "volume" and "surface". - device: Device to use for the simulation. - Returns: - The deformable object representing the cubes. - - """ - origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) - - # Resolve spawn configuration - if has_api: - spawn_cfg = sim_utils.MeshCuboidCfg( - size=(0.2, 0.2, 0.2), - deformable_props=PhysxDeformableBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - # Add physics material if provided - if material_path is not None: - if deformable_type == "surface": - spawn_cfg.physics_material = PhysxSurfaceDeformableBodyMaterialCfg() - else: - spawn_cfg.physics_material = PhysxDeformableBodyMaterialCfg() - spawn_cfg.physics_material_path = material_path - else: - spawn_cfg.physics_material = None - else: - # since no deformable body properties defined, this is just a static collider - spawn_cfg = sim_utils.MeshCuboidCfg( - size=(0.2, 0.2, 0.2), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - # Create deformable object - cube_object_cfg = DeformableObjectCfg( - prim_path="/World/Table_[^/]*/Object", - spawn=spawn_cfg, - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height), rot=initial_rot), +def _bind_material(body_prim, material_cfg) -> None: + material_prim = material_cfg.func(f"{body_prim.GetPath()}/material", material_cfg) + UsdShade.MaterialBindingAPI.Apply(body_prim) + UsdShade.MaterialBindingAPI(body_prim).Bind( + UsdShade.Material(material_prim), + bindingStrength=UsdShade.Tokens.weakerThanDescendants, + materialPurpose="physics", ) - cube_object = DeformableObject(cfg=cube_object_cfg) - - return cube_object - - -@pytest.fixture -def sim(): - """Create simulation context.""" - with build_simulation_context(auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - yield sim - - -@pytest.mark.parametrize( - "num_cubes, material_path", - [ - (1, "material"), - (2, None), - (2, "/World/SoftMaterial"), - (2, "material"), - ], -) -def test_initialization(sim, num_cubes, material_path): - """Test initialization for prim with deformable body API at the provided prim path.""" - cube_object = generate_cubes_scene(num_cubes=num_cubes, material_path=material_path) - - # Check that the framework doesn't hold excessive strong references. - # sys.getrefcount() adds 1 for its own argument. The baseline is 2 (local var + - # getrefcount arg) but Omniverse event-bus subscriptions and Python/torch runtime - # internals may legitimately add a few more. We use a threshold to catch real leaks. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - sim.reset() - - # Check if object is initialized - assert cube_object.is_initialized - # Check correct number of cubes - assert cube_object.num_instances == num_cubes - assert cube_object.root_view.count == num_cubes - # Check correct number of materials in the view - if material_path: - if material_path.startswith("/"): - assert cube_object.material_physx_view.count == 1 - else: - assert cube_object.material_physx_view.count == num_cubes - else: - assert cube_object.material_physx_view is None - - # Check buffers that exist and have correct shapes - # nodal_state_w is (N, V) vec6f -> wp.to_torch gives (N, V, 6) - assert cube_object.data.nodal_state_w.torch.shape == (num_cubes, cube_object.max_sim_vertices_per_body, 6) - # nodal_kinematic_target is (N, V) vec4f -> .torch gives (N, V, 4) - assert cube_object.data.nodal_kinematic_target.torch.shape == ( - num_cubes, - cube_object.max_sim_vertices_per_body, - 4, +def _spawn_volume_deformable() -> DeformableObject: + """Author a five-node, two-tetrahedron volume fixture with no optional mesher.""" + stage = sim_utils.get_current_stage() + UsdGeom.Xform.Define(stage, "/World/Volume") + tet_mesh = UsdGeom.TetMesh.Define(stage, "/World/Volume/simulation") + body_prim = tet_mesh.GetPrim() + _add_api_schemas( + body_prim, + [ + "OmniPhysicsDeformableBodyAPI", + "OmniPhysicsVolumeDeformableSimAPI", + "OmniPhysicsDeformablePoseAPI:default", + "PhysicsCollisionAPI", + ], ) - # root_pos_w is (N,) vec3f -> wp.to_torch gives (N, 3) - assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) - assert cube_object.data.root_vel_w.torch.shape == (num_cubes, 3) - - -@pytest.mark.isaacsim_ci -def test_initialization_surface_deformable(sim): - """Test initialization of a surface deformable body.""" - num_cubes = 2 - cube_object = generate_cubes_scene(num_cubes=num_cubes, deformable_type="surface") - - # Play sim - sim.reset() - - # Check if object is initialized - assert cube_object.is_initialized - assert cube_object._deformable_type == "surface" - - # Check correct number of instances - assert cube_object.num_instances == num_cubes - assert cube_object.root_view.count == num_cubes - - # Check material view is created - assert cube_object.material_physx_view is not None - assert cube_object.material_physx_view.count == num_cubes - - # Check nodal state buffers have correct shapes - assert cube_object.data.nodal_state_w.torch.shape == (num_cubes, cube_object.max_sim_vertices_per_body, 6) - assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) - assert cube_object.data.root_vel_w.torch.shape == (num_cubes, 3) - - # Kinematic targets are not allocated for surface deformables - assert cube_object.data.nodal_kinematic_target is None - - # Writing kinematic targets should raise ValueError - dummy_targets = torch.zeros(num_cubes, cube_object.max_sim_vertices_per_body, 4, device=sim.device) - with pytest.raises(ValueError, match="Kinematic targets can only be set for volume deformable bodies"): - cube_object.write_nodal_kinematic_target_to_sim_index(dummy_targets) - - -@pytest.mark.isaacsim_ci -def test_initialization_on_device_cpu(): - """Test that initialization fails with deformable body API on the CPU.""" - with build_simulation_context(device="cpu", auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object = generate_cubes_scene(num_cubes=5, device="cpu") - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.isaacsim_ci -def test_set_nodal_state(sim): - """Test setting the state of the deformable object.""" - num_cubes = 2 - cube_object = generate_cubes_scene(num_cubes=num_cubes) - - # Play the simulator - sim.reset() - - for state_type_to_randomize in ["nodal_pos_w", "nodal_vel_w"]: - state_dict = { - "nodal_pos_w": torch.zeros_like(cube_object.data.nodal_pos_w.torch), - "nodal_vel_w": torch.zeros_like(cube_object.data.nodal_vel_w.torch), - } - - for _ in range(5): - cube_object.reset() - - state_dict[state_type_to_randomize] = torch.randn( - num_cubes, cube_object.max_sim_vertices_per_body, 3, device=sim.device - ) - - for _ in range(5): - nodal_state = torch.cat( - [ - state_dict["nodal_pos_w"], - state_dict["nodal_vel_w"], - ], - dim=-1, - ) - cube_object.write_nodal_state_to_sim_index(nodal_state) - - torch.testing.assert_close(cube_object.data.nodal_state_w.torch, nodal_state, rtol=1e-5, atol=1e-5) - - sim.step() - cube_object.update(sim.cfg.dt) - + points = [ + Gf.Vec3f(0.0, 0.0, 0.0), + Gf.Vec3f(0.2, 0.0, 0.0), + Gf.Vec3f(0.0, 0.2, 0.0), + Gf.Vec3f(0.0, 0.0, 0.2), + Gf.Vec3f(0.2, 0.2, 0.2), + ] + tets = [Gf.Vec4i(0, 1, 2, 3), Gf.Vec4i(1, 2, 3, 4)] + faces = [ + Gf.Vec3i(0, 2, 1), + Gf.Vec3i(0, 1, 3), + Gf.Vec3i(0, 3, 2), + Gf.Vec3i(1, 2, 4), + Gf.Vec3i(2, 3, 4), + Gf.Vec3i(3, 1, 4), + ] + tet_mesh.CreatePointsAttr(points) + tet_mesh.CreateTetVertexIndicesAttr(tets) + tet_mesh.CreateSurfaceFaceVertexIndicesAttr(faces) + body_prim.CreateAttribute("deformablePose:default:omniphysics:points", Sdf.ValueTypeNames.Point3fArray).Set(points) + body_prim.CreateAttribute("deformablePose:default:omniphysics:purposes", Sdf.ValueTypeNames.TokenArray).Set( + ["bindPose"] + ) + body_prim.CreateAttribute("omniphysics:restShapePoints", Sdf.ValueTypeNames.Point3fArray).Set(points) + body_prim.CreateAttribute("omniphysics:restTetVtxIndices", Sdf.ValueTypeNames.Int4Array).Set(tets) + body_prim.CreateAttribute("velocities", Sdf.ValueTypeNames.Vector3fArray).Set([Gf.Vec3f()] * len(points)) + visual = UsdGeom.Mesh.Define(stage, "/World/Volume/visual") + visual.CreatePointsAttr(points) + visual.CreateFaceVertexCountsAttr([3] * len(faces)) + visual.CreateFaceVertexIndicesAttr([index for face in faces for index in face]) + _bind_material(body_prim, PhysxDeformableBodyMaterialCfg()) + return DeformableObject(DeformableObjectCfg(prim_path="/World/Volume")) + + +def _spawn_surface_deformable() -> DeformableObject: + """Author a four-node, two-triangle surface fixture.""" + stage = sim_utils.get_current_stage() + mesh = UsdGeom.Mesh.Define(stage, "/World/Surface") + body_prim = mesh.GetPrim() + _add_api_schemas( + body_prim, + [ + "OmniPhysicsDeformableBodyAPI", + "OmniPhysicsSurfaceDeformableSimAPI", + "OmniPhysicsDeformablePoseAPI:default", + "PhysicsCollisionAPI", + ], + ) + points = [ + Gf.Vec3f(0.0, 0.0, 0.0), + Gf.Vec3f(0.2, 0.0, 0.0), + Gf.Vec3f(0.2, 0.2, 0.0), + Gf.Vec3f(0.0, 0.2, 0.0), + ] + triangles = [Gf.Vec3i(0, 1, 2), Gf.Vec3i(0, 2, 3)] + mesh.CreatePointsAttr(points) + mesh.CreateFaceVertexCountsAttr([3, 3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2, 0, 2, 3]) + body_prim.CreateAttribute("deformablePose:default:omniphysics:points", Sdf.ValueTypeNames.Point3fArray).Set(points) + body_prim.CreateAttribute("deformablePose:default:omniphysics:purposes", Sdf.ValueTypeNames.TokenArray).Set( + ["bindPose"] + ) + body_prim.CreateAttribute("omniphysics:restShapePoints", Sdf.ValueTypeNames.Point3fArray).Set(points) + body_prim.CreateAttribute("omniphysics:restTriVtxIndices", Sdf.ValueTypeNames.Int3Array).Set(triangles) + body_prim.CreateAttribute("velocities", Sdf.ValueTypeNames.Vector3fArray).Set([Gf.Vec3f()] * len(points)) + _bind_material( + body_prim, + PhysxSurfaceDeformableBodyMaterialCfg( + density=900.0, + static_friction=0.35, + dynamic_friction=0.4, + youngs_modulus=2000.0, + poissons_ratio=0.25, + surface_thickness=0.02, + surface_stretch_stiffness=0.8, + surface_shear_stiffness=0.7, + surface_bend_stiffness=0.6, + elasticity_damping=0.03, + bend_damping=0.04, + ), + ) + return DeformableObject(DeformableObjectCfg(prim_path="/World/Surface")) -@pytest.mark.parametrize( - "num_cubes, randomize_pos, randomize_rot", - [ - (1, False, False), - (1, True, False), - (1, False, True), - (2, True, True), - ], -) -@flaky(max_runs=3, min_passes=1) -@pytest.mark.isaacsim_ci -def test_set_nodal_state_with_applied_transform(num_cubes, randomize_pos, randomize_rot): - """Test setting the state of the deformable object with applied transform.""" - carb_settings_iface = carb.settings.get_settings() - carb_settings_iface.set_bool("/physics/cooking/ujitsoCollisionCooking", False) - # Create simulation context with gravity disabled (no fixture needed) - with build_simulation_context(auto_add_lighting=True, gravity_enabled=False) as sim: - sim._app_control_on_stop_handle = None - cube_object = generate_cubes_scene(num_cubes=num_cubes) +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") +def test_volume_deformable_real_physx_state_and_kinematic_target() -> None: + """Prove volume view/material creation plus nodal and kinematic-target writes.""" + with build_simulation_context(device="cuda:0", gravity_enabled=False) as sim: + deformable = _spawn_volume_deformable() sim.reset() - for _ in range(5): - nodal_state = cube_object.data.default_nodal_state_w.torch.clone() - mean_nodal_pos_default = nodal_state[..., :3].mean(dim=1) - - if randomize_pos: - pos_w = 0.5 * torch.rand(cube_object.num_instances, 3, device=sim.device) - pos_w[:, 2] += 0.5 - else: - pos_w = None - if randomize_rot: - quat_w = math_utils.random_orientation(cube_object.num_instances, device=sim.device) - else: - quat_w = None - - nodal_state[..., :3] = cube_object.transform_nodal_pos(nodal_state[..., :3], pos_w, quat_w) - mean_nodal_pos_init = nodal_state[..., :3].mean(dim=1) - - if pos_w is None: - torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default, rtol=1e-5, atol=1e-5) - else: - torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default + pos_w, rtol=1e-5, atol=1e-5) - - cube_object.write_nodal_state_to_sim_index(nodal_state) - cube_object.reset() - - for _ in range(50): - sim.step() - cube_object.update(sim.cfg.dt) - - torch.testing.assert_close(cube_object.data.root_pos_w.torch, mean_nodal_pos_init, rtol=1e-4, atol=1e-4) - - -@pytest.mark.isaacsim_ci -def test_set_kinematic_targets(sim): - """Test setting kinematic targets for the deformable object.""" - num_cubes = 2 - cube_object = generate_cubes_scene(num_cubes=num_cubes, height=1.0) - - sim.reset() - - nodal_kinematic_targets = wp.to_torch(cube_object.root_view.get_simulation_nodal_kinematic_targets()).clone() - - for _ in range(5): - cube_object.write_nodal_state_to_sim_index(cube_object.data.default_nodal_state_w.torch) - - default_root_pos = cube_object.data.default_nodal_state_w.torch.mean(dim=1) - - cube_object.reset() - - nodal_kinematic_targets[1:, :, 3] = 1.0 - nodal_kinematic_targets[0, :, 3] = 0.0 - nodal_kinematic_targets[0, :, :3] = cube_object.data.default_nodal_state_w.torch[0, :, :3] - cube_object.write_nodal_kinematic_target_to_sim_index( - nodal_kinematic_targets[0:1], env_ids=torch.tensor([0], device=sim.device) - ) - - for _ in range(20): - sim.step() - cube_object.update(sim.cfg.dt) + assert deformable.is_initialized + assert deformable._deformable_type == "volume" + assert deformable.material_physx_view is not None + assert deformable.data.nodal_state_w.torch.shape[-1] == 6 + positions = deformable.data.nodal_pos_w.torch.clone() + velocities = torch.zeros_like(positions) + velocities[:, 0, 0] = 0.25 + state = torch.cat((positions, velocities), dim=-1) + deformable.write_nodal_state_to_sim_index(state) + torch.testing.assert_close(deformable.data.nodal_state_w.torch, state) + + targets = deformable.data.nodal_kinematic_target.torch.clone() + targets[:, 0, :3] = positions[:, 0] + torch.tensor([0.01, 0.02, 0.03], device="cuda:0") + targets[:, 0, 3] = 0.0 + deformable.write_nodal_kinematic_target_to_sim_index(targets) + raw_targets = wp.to_torch(deformable.root_view.get_simulation_nodal_kinematic_targets()).reshape_as(targets) + torch.testing.assert_close(raw_targets, targets) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") +def test_surface_deformable_real_physx_state_probe() -> None: + """Prove surface view/material creation and a nodal-position TensorAPI write.""" + with build_simulation_context(device="cuda:0", gravity_enabled=False) as sim: + deformable = _spawn_surface_deformable() + sim.reset() - torch.testing.assert_close( - cube_object.data.nodal_pos_w.torch[0], - nodal_kinematic_targets[0, :, :3], - rtol=1e-5, - atol=1e-5, - ) - root_pos_w = cube_object.data.root_pos_w.torch - assert torch.all(root_pos_w[1:, 2] < default_root_pos[1:, 2]) + assert deformable.is_initialized + assert deformable._deformable_type == "surface" + assert deformable.root_view.count == 1 + assert deformable.max_sim_vertices_per_body == 4 + assert deformable.material_physx_view is not None + positions = deformable.data.nodal_pos_w.torch.clone() + positions[:, 0, 2] += 0.01 + deformable.write_nodal_pos_to_sim_index(positions) + raw_positions = wp.to_torch(deformable.root_view.get_simulation_nodal_positions()).reshape_as(positions) + torch.testing.assert_close(raw_positions, positions) diff --git a/source/isaaclab_physx/test/assets/test_newton_actuators_physx.py b/source/isaaclab_physx/test/assets/test_newton_actuators_physx.py index 06e8e6f9e6f6..6755af8dbfd1 100644 --- a/source/isaaclab_physx/test/assets/test_newton_actuators_physx.py +++ b/source/isaaclab_physx/test/assets/test_newton_actuators_physx.py @@ -3,909 +3,108 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PD actuator equivalence tests on ANYmal-C (floating-base quadruped) — PhysX backend. - -Compares IsaacLab-native actuators against Newton-native actuators (created -from the same Lab configs via USD authoring, stepped via -:class:`PhysxActuatorWrapper`) on the PhysX physics backend. Both paths -must produce identical joint trajectories within tolerance. - -Using ANYmal-C — a 12-DOF quadruped on a floating base — exercises the -full Lab-to-Newton config translation pipeline on a real-world robot. -""" +"""Minimal real-PhysX integration proof for Lab and Newton actuator dispatch.""" from isaaclab.app import AppLauncher simulation_app = AppLauncher(headless=True).app -import functools -import os -import unittest -from types import SimpleNamespace +from pathlib import Path +from typing import TypedDict import pytest import torch -import warp as wp from isaaclab_physx.assets import Articulation -from isaaclab_physx.assets.articulation.actuator_control import PhysxActuatorControl from isaaclab_physx.physics import PhysxCfg +from pxr import UsdPhysics + import isaaclab.sim as sim_utils from isaaclab.actuators import IdealPDActuatorCfg -from isaaclab.actuators.newton import read_group_parameter +from isaaclab.assets import ArticulationCfg 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, - MIXED_WITH_IMPLICIT_ACTUATORS, - ActuatorStateResetBase, - EquivalenceAssertionsMixin, - MockEnv, - build_dr_term, - make_dummy_lstm_checkpoint, - make_dummy_mlp_checkpoint, -) -from isaaclab.test.utils.articulation_ordering import assert_articulation_ordering_trace_matches - -from isaaclab_assets import ANYMAL_C_CFG -from isaaclab_assets.robots.spot import joint_parameter_lookup as SPOT_KNEE_LOOKUP - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- - -NUM_ENVS = 2 -NUM_STEPS = 10 -DT = 1.0 / 120.0 -TARGET_OFFSET = 0.1 # [rad] added to initial joint positions -_ANYMAL_C_PHYSX_JOINT_NAMES = ( - "LF_HAA", - "LH_HAA", - "RF_HAA", - "RH_HAA", - "LF_HFE", - "LH_HFE", - "RF_HFE", - "RH_HFE", - "LF_KFE", - "LH_KFE", - "RF_KFE", - "RH_KFE", -) - - -def test_prepare_native_actuators_does_not_zero_solver_gains(monkeypatch): - """Leave solver gains untouched until collection construction resolves actuator defaults.""" - from isaaclab_physx.assets.articulation import actuator_control - - from isaaclab.actuators.newton import NewtonActuatorAdapter, PhysxActuatorWrapper - - joint_buffer = SimpleNamespace(warp=wp.zeros((1, 1), dtype=wp.float32, device="cpu")) - collection = SimpleNamespace( - target_command=SimpleNamespace(position=joint_buffer, velocity=joint_buffer, effort=joint_buffer) - ) - gain_writes = [] - articulation = SimpleNamespace( - _sim_cfg=SimpleNamespace(use_newton_actuators=True), - cfg=SimpleNamespace(prim_path="/World/Robot"), - joint_names=["joint"], - num_instances=1, - num_joints=1, - device="cpu", - _data=SimpleNamespace(joint_pos=joint_buffer, joint_vel=joint_buffer), - write_joint_stiffness_to_sim_index=lambda **_: gain_writes.append("stiffness"), - write_joint_damping_to_sim_index=lambda **_: gain_writes.append("damping"), - ) - wrapper = SimpleNamespace() - adapter = SimpleNamespace(joint_indices=wp.array([0], dtype=wp.int32), finalize=lambda _: None) - monkeypatch.setattr(actuator_control, "find_first_matching_prim", lambda _: None) - monkeypatch.setattr(PhysxActuatorWrapper, "create", lambda **_: wrapper) - monkeypatch.setattr(NewtonActuatorAdapter, "from_usd", lambda **_: adapter) - - native_groups = PhysxActuatorControl(articulation).prepare_native_actuators( - collection, - {"explicit": IdealPDActuatorCfg(joint_names_expr=["joint"], stiffness=None, damping=None)}, - ) - - assert native_groups == {"explicit"} - assert gain_writes == [] +pytestmark = pytest.mark.integration -# --------------------------------------------------------------------------- -# Simulation runner -# --------------------------------------------------------------------------- +_FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" -def _run_simulation( - actuators: dict, - use_newton_actuators: bool, - *, - num_steps: int = NUM_STEPS, - feedforward: float | None = None, - joint_ordering: tuple[str, ...] | None = None, - permutation_sensitive_commands: bool = False, - capture_first_compute: bool = False, -) -> dict: - """Run ANYmal-C on PhysX and return recorded trajectories + telemetry. +class _ActuatorResult(TypedDict): + """Observations from one real actuator path.""" - Always records public joint state/telemetry and the Newton adapter outputs. + initial_position: torch.Tensor + final_position: torch.Tensor + applied_effort: torch.Tensor + native_path: bool + has_wrapper: bool + has_nonidentity_ordering: bool - Args: - actuators: Actuator configuration replacing ANYmal-C defaults. - use_newton_actuators: Whether to use the Newton actuator fast path. - num_steps: Number of simulation steps to record. - feedforward: Optional constant effort target for every joint. - joint_ordering: Optional explicit public joint-name order. - permutation_sensitive_commands: Whether to command distinct position, velocity, and effort values by - physical joint name. - capture_first_compute: Whether to invoke the first actuator computation inside an outer CUDA capture. - Returns: - Recorded joint-name metadata, commands, public trajectories and torque telemetry, and adapter effort traces. - """ - sim_cfg = SimulationCfg(dt=DT, physics=PhysxCfg(), use_newton_actuators=use_newton_actuators) +def _run_actuator_path(use_newton_actuators: bool) -> _ActuatorResult: + """Run four steps through one actuator path on the local branching fixture.""" 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=actuators, - prim_path="/World/Env_[^/]*/Robot", - joint_ordering=joint_ordering, - ) - articulation = Articulation(art_cfg) - sim.reset() - assert articulation.is_initialized - - joint_names = tuple(articulation.joint_names) - backend_joint_names = tuple(articulation.backend_joint_names) - installed_ordering = articulation.joint_ordering - joint_ordering_state = ( - None - if installed_ordering is None - else { - "user_names": joint_names, - "backend_names": backend_joint_names, - "user_to_backend_indices": installed_ordering.user_to_backend_indices, - "backend_to_user_indices": installed_ordering.backend_to_user_indices, - } - ) - init_pos = wp.to_torch(articulation.data.joint_pos).clone() - if permutation_sensitive_commands: - scale_by_name = {name: index + 1 for index, name in enumerate(backend_joint_names)} - joint_scale = torch.tensor( - [scale_by_name[name] for name in joint_names], - device=articulation.device, - dtype=init_pos.dtype, - ).unsqueeze(0) - joint_scale = joint_scale.expand_as(init_pos) - target_pos = init_pos + 0.01 * joint_scale - target_vel = 0.001 * joint_scale - effort_target = 0.1 * joint_scale - else: - target_pos = init_pos + TARGET_OFFSET - target_vel = torch.zeros_like(init_pos) - effort_target = None if feedforward is None else torch.full_like(init_pos, feedforward) - - articulation.set_joint_position_target_index(target=target_pos) - articulation.set_joint_velocity_target_index(target=target_vel) - if effort_target is not None: - articulation.set_joint_effort_target_index(target=effort_target) - - recorded_pos, recorded_vel = [], [] - recorded_computed_effort, recorded_applied_effort = [], [] - recorded_adapter_applied = [] - if capture_first_compute: - with wp.ScopedCapture(device=articulation.device, force_module_load=True): - articulation.actuators.compute(DT) - 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()) - recorded_vel.append(wp.to_torch(articulation.data.joint_vel).clone()) - recorded_computed_effort.append(articulation.actuators.computed_effort.torch.clone()) - recorded_applied_effort.append(articulation.actuators.applied_effort.torch.clone()) - if use_newton_actuators: - recorded_adapter_applied.append(wp.to_torch(articulation._physx_actuator_wrapper.joint_f_2d).clone()) - native_actuator_graph_count = len(getattr(articulation._actuator_control, "_native_actuator_graphs", ()) or ()) - - return { - "joint_names": joint_names, - "backend_joint_names": backend_joint_names, - "joint_ordering": joint_ordering_state, - "adapter_joint_names": joint_names, - "joint_pos": recorded_pos, - "joint_vel": recorded_vel, - "computed_effort": recorded_computed_effort, - "applied_effort": recorded_applied_effort, - "adapter_applied_effort": recorded_adapter_applied, - "target_pos": target_pos.clone(), - "target_vel": target_vel.clone(), - "effort_target": None if effort_target is None else effort_target.clone(), - "native_actuator_graph_count": native_actuator_graph_count, - } - - -def test_graphable_newton_actuators_capture_ping_pong_graphs() -> None: - result = _run_simulation(DELAYED_PD_ACTUATORS, use_newton_actuators=True, num_steps=2) - - assert result["native_actuator_graph_count"] == 2 - - -def test_newton_actuator_graph_capture_failure_falls_back_to_eager(monkeypatch: pytest.MonkeyPatch) -> None: - class FailingCapture: - def __init__(self, *args, **kwargs): - pass - - def __enter__(self): - raise RuntimeError("capture unavailable") - - def __exit__(self, exc_type, exc_value, traceback): - return False - - monkeypatch.setattr(wp, "ScopedCapture", FailingCapture) - - result = _run_simulation( - DC_MOTOR_ACTUATORS, - use_newton_actuators=True, - num_steps=2, - feedforward=1.0, - ) - - assert result["native_actuator_graph_count"] == 0 - assert len(result["joint_pos"]) == 2 - assert all(torch.isfinite(joint_pos).all() for joint_pos in result["joint_pos"]) - assert all(torch.any(effort != 0.0) for effort in result["applied_effort"]) - assert all(torch.any(effort != 0.0) for effort in result["adapter_applied_effort"]) - - -def test_stateful_newton_actuators_reject_outer_cuda_capture() -> None: - with pytest.raises(RuntimeError, match="stateful Newton actuators cannot run inside an outer CUDA graph capture"): - _run_simulation( - DELAYED_PD_ACTUATORS, - use_newton_actuators=True, - num_steps=0, - capture_first_compute=True, + sim_cfg=SimulationCfg( + device="cuda:0", + dt=1.0 / 120.0, + gravity=(0.0, 0.0, 0.0), + physics=PhysxCfg(), + use_newton_actuators=use_newton_actuators, ) - - -def test_newton_actuator_rollout_matches_reversed_joint_ordering() -> None: - """Match PhysX Newton-actuator traces under reversed public joint ordering.""" - identity_result = _run_simulation( - IDEAL_PD_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, - use_newton_actuators=True, - joint_ordering=requested_joint_names, - permutation_sensitive_commands=True, - ) - - assert_articulation_ordering_trace_matches(identity_result, reversed_result, requested_joint_names) - - -def _assert_newton_actuator_uses_current_joint_state( - joint_ordering: tuple[str, ...] | None, *, num_steps: int = NUM_STEPS -) -> None: - """Check that ``applied_effort`` always matches the IdealPD formula on *this* step's true state. - - Ground truth is read every step via ``root_view.get_dof_positions()``/``get_dof_velocities()`` -- - the raw PhysX view, bypassing :class:`ArticulationData`'s cached ``joint_pos``/``joint_vel`` shadow - entirely -- so the read itself cannot refresh (and thereby mask staleness in) the shadow under test. - - Args: - joint_ordering: Optional explicit public joint-name order to install on the articulation. - num_steps: Number of simulation steps to check. - """ - kp, kd, effort_limit = 40.0, 5.0, 80.0 - actuators = { - "legs": IdealPDActuatorCfg( - joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], - stiffness=kp, - damping=kd, - actuator_effort_limit=effort_limit, - ), - } - sim_cfg = SimulationCfg(dt=DT, physics=PhysxCfg(), 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=actuators, - prim_path="/World/Env_[^/]*/Robot", - joint_ordering=joint_ordering, - ) - articulation = Articulation(art_cfg) - sim.reset() - assert articulation.is_initialized - - ordering = articulation.joint_ordering - if ordering is not None: - user_to_backend = torch.tensor( - ordering.user_to_backend_indices, dtype=torch.long, device=articulation.device + articulation = Articulation( + ArticulationCfg( + prim_path="/World/Robot", + spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), + actuators={ + "joints": IdealPDActuatorCfg( + joint_names_expr=[".*"], + stiffness=20.0, + damping=2.0, + actuator_effort_limit=50.0, + ) + }, + joint_ordering="mjwarp", ) - - def to_user_order(raw_backend: wp.array) -> torch.Tensor: - return wp.to_torch(raw_backend).index_select(1, user_to_backend) - - else: - - def to_user_order(raw_backend: wp.array) -> torch.Tensor: - return wp.to_torch(raw_backend) - - init_pos = to_user_order(articulation.root_view.get_dof_positions()).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) - - for step in range(num_steps): - # Ground truth for *this* step, independent of ArticulationData's joint_pos/joint_vel shadow. - true_pos = to_user_order(articulation.root_view.get_dof_positions()).clone() - true_vel = to_user_order(articulation.root_view.get_dof_velocities()).clone() - - articulation.write_data_to_sim() - applied = articulation.actuators.applied_effort.torch.clone() - - expected = torch.clamp(kp * (target_pos - true_pos) - kd * true_vel, -effort_limit, effort_limit) - torch.testing.assert_close( - applied, - expected, - atol=1e-3, - rtol=1e-3, - msg=( - f"applied_effort at step {step} does not match the IdealPD formula evaluated on this" - " step's true PhysX joint state -- the Newton actuator likely used a stale" - " joint_pos/joint_vel shadow" - ), - ) - - sim.step() - articulation.update(DT) - - -def test_newton_actuator_identity_ordering_uses_current_joint_state() -> None: - """Sanity check: with identity joint ordering, ``applied_effort`` always reflects this step's state.""" - _assert_newton_actuator_uses_current_joint_state(None) - - -def test_newton_actuator_reversed_ordering_uses_current_joint_state() -> None: - """Regression test: non-identity joint ordering must preserve current-state torque evaluation. - - The adapter must resolve joint state in articulation order before evaluating the actuator model, - regardless of the backend view ordering. - """ - reversed_joint_names = tuple(reversed(_ANYMAL_C_PHYSX_JOINT_NAMES)) - _assert_newton_actuator_uses_current_joint_state(reversed_joint_names) - - -# --------------------------------------------------------------------------- -# Base test class -# --------------------------------------------------------------------------- - - -class _EquivalenceTestBase(EquivalenceAssertionsMixin, unittest.TestCase): - """Base for Lab-vs-Newton equivalence tests on the PhysX backend. - - Subclasses set ``actuators`` to the config under test. ``setUpClass`` - runs the simulation with both ``use_newton_actuators=False`` (Lab path) - and ``True`` (Newton via PhysxActuatorWrapper) and stores the results. - The ``test_*_match`` oracles come from :class:`EquivalenceAssertionsMixin`. - """ - - __test__ = False - actuators: dict = {} - feedforward: float | None = None - - @classmethod - def setUpClass(cls): - cls.lab_result = _run_simulation( - cls.actuators, - use_newton_actuators=False, - feedforward=cls.feedforward, ) - cls.newton_result = _run_simulation( - cls.actuators, - use_newton_actuators=True, - feedforward=cls.feedforward, + UsdPhysics.FixedJoint.Define(sim_utils.get_current_stage(), "/World/Robot/fixed_root").GetBody1Rel().SetTargets( + ["/World/Robot/base"] ) - - -# --------------------------------------------------------------------------- -# Equivalence tests with different actuator types -# --------------------------------------------------------------------------- - - -class TestIdealPDEquivalence(_EquivalenceTestBase): - """IdealPDActuator on all 12 joints: Lab vs Newton (PhysX backend).""" - - __test__ = True - actuators = IDEAL_PD_ACTUATORS - - -class TestDCMotorEquivalence(_EquivalenceTestBase): - """DCMotor actuator on all 12 joints: Lab vs Newton (PhysX backend).""" - - __test__ = True - actuators = DC_MOTOR_ACTUATORS - - -class TestDelayedPDEquivalence(_EquivalenceTestBase): - """DelayedPDActuator on all 12 joints: Lab vs Newton (PhysX). - - Verifies that actuator command delays are correctly authored and - produce matching trajectories on the PhysX backend. - """ - - __test__ = True - actuators = DELAYED_PD_ACTUATORS - - -class TestMixedWithImplicitEquivalence(_EquivalenceTestBase): - """Implicit HAA + IdealPD HFE + DCMotor KFE: Lab vs Newton (PhysX). - - Verifies that implicit actuators (handled by PhysX joint drives) - coexist correctly with explicit Newton actuators via PhysxActuatorWrapper. - """ - - __test__ = True - actuators = MIXED_WITH_IMPLICIT_ACTUATORS - - -# --------------------------------------------------------------------------- -# Implicit + non-zero feedforward effort target on PhysX -# --------------------------------------------------------------------------- - - -class TestImplicitWithFeedforwardEquivalencePhysx(_EquivalenceTestBase): - """Implicit-only actuators with a non-zero feedforward effort target on PhysX.""" - - __test__ = True - actuators = IMPLICIT_ONLY_ACTUATORS - feedforward = 5.0 - - -# --------------------------------------------------------------------------- -# Heterogeneous multi-articulation (ANYmal floating-base + Cartpole fixed-base) -# --------------------------------------------------------------------------- - - -def _run_anymal_and_cartpole(use_newton_actuators: bool, *, num_steps: int = NUM_STEPS) -> dict: - """Spawn ANYmal-C + Cartpole per env on PhysX (different DOF counts, base types).""" - from isaaclab_assets import CARTPOLE_CFG # noqa: PLC0415 - - sim_cfg = SimulationCfg(dt=DT, physics=PhysxCfg(), use_newton_actuators=use_newton_actuators) - 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 - - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 6.0, 0, 0)) - - anymal_cfg = ANYMAL_C_CFG.replace(actuators=IDEAL_PD_ACTUATORS, prim_path="/World/Env_[^/]*/Anymal") - cartpole_cfg = CARTPOLE_CFG.replace( - actuators=CARTPOLE_EXPLICIT_ACTUATORS, - prim_path="/World/Env_[^/]*/Cartpole", - ) - cartpole_cfg.init_state = cartpole_cfg.init_state.replace(pos=(0.0, 3.0, 2.0)) - - anymal = Articulation(anymal_cfg) - cartpole = Articulation(cartpole_cfg) sim.reset() - assert anymal.is_initialized and cartpole.is_initialized - - init_anymal = wp.to_torch(anymal.data.joint_pos).clone() - init_cartpole = wp.to_torch(cartpole.data.joint_pos).clone() - anymal.set_joint_position_target_index(target=init_anymal + TARGET_OFFSET) - anymal.set_joint_velocity_target_index(target=torch.zeros_like(init_anymal)) - cartpole.set_joint_position_target_index(target=init_cartpole + TARGET_OFFSET) - cartpole.set_joint_velocity_target_index(target=torch.zeros_like(init_cartpole)) - - pos_anymal, pos_cartpole = [], [] - for _ in range(num_steps): - anymal.write_data_to_sim() - cartpole.write_data_to_sim() + initial_position = articulation.data.joint_pos.torch.clone() + articulation.actuators.target_command.set_position_index(value=initial_position + 0.05) + articulation.actuators.target_command.set_velocity_index(value=torch.zeros_like(initial_position)) + applied = [] + for _ in range(4): + articulation.write_data_to_sim() + applied.append(articulation.actuators.applied_effort.torch.clone()) sim.step() - anymal.update(DT) - cartpole.update(DT) - pos_anymal.append(wp.to_torch(anymal.data.joint_pos).clone()) - pos_cartpole.append(wp.to_torch(cartpole.data.joint_pos).clone()) - - return {"joint_pos_anymal": pos_anymal, "joint_pos_cartpole": pos_cartpole} - - -class TestHeterogeneousMultiArticulationPhysx(unittest.TestCase): - """Two structurally-different articulations (ANYmal floating + Cartpole fixed) on PhysX. - - Each PhysX articulation owns its own :class:`PhysxActuatorWrapper` - and per-art :class:`NewtonActuatorAdapter`. Heterogeneous DOF counts - (12 vs 2) and base types (floating vs fixed) verify the - per-articulation authoring + adapter construction works for varied - structures. Equivalence against the Lab actuator path is the - meaningful end-to-end check. - """ - - @classmethod - def setUpClass(cls): - cls.lab_result = _run_anymal_and_cartpole(use_newton_actuators=False) - cls.newton_result = _run_anymal_and_cartpole(use_newton_actuators=True) - - def test_anymal_matches_lab(self): - for step_i, (lab, newton) in enumerate( - zip(self.lab_result["joint_pos_anymal"], self.newton_result["joint_pos_anymal"]) - ): - torch.testing.assert_close( - newton, - lab, - atol=2e-3, - rtol=1e-3, - msg=f"ANYmal joint_pos diverged from Lab path at step {step_i}", - ) - - def test_cartpole_matches_lab(self): - for step_i, (lab, newton) in enumerate( - zip(self.lab_result["joint_pos_cartpole"], self.newton_result["joint_pos_cartpole"]) - ): - torch.testing.assert_close( - newton, - lab, - atol=2e-3, - rtol=1e-3, - msg=f"Cartpole joint_pos diverged from Lab path at step {step_i}", - ) - - -# --------------------------------------------------------------------------- -# Domain randomization via events.py — PhysX backend -# --------------------------------------------------------------------------- - - -class TestRandomizeActuatorGainsViaEventsPhysx(unittest.TestCase): - """End-to-end DR test for the PhysX backend. - - Drives ``randomize_actuator_gains`` (events.py) and verifies the new - kp/kd values reach the controllers of the articulation's Newton - actuators — exercising the full path: events → the actuator adapter → - write_stiffness/damping → propagation to controllers. The assertions - read the controllers back via the public - ``read_group_parameter``. - - The native-controller tests use degenerate ranges for exact expected values. - The implicit-storage regression instead seeds the generator and uses - non-degenerate ranges to verify one sampled payload reaches every storage. - """ - - def test_implicit_storage_reuses_randomized_payload(self): - """Keep actuator-owned and implicit-solver gains identical after randomization.""" - sim_cfg = SimulationCfg(dt=DT, physics=PhysxCfg(), use_newton_actuators=False) - 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=IMPLICIT_ONLY_ACTUATORS, - prim_path="/World/Env_.*/Robot", - ) - anymal = Articulation(art_cfg) - sim.reset() - - actuator = anymal.actuators["legs"] - stiffness_before = actuator.stiffness.clone() - damping_before = actuator.damping.clone() - 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) - torch.manual_seed(12345) - - term( - env, - env_ids=env_ids, - asset_cfg=asset_cfg, - stiffness_distribution_params=(25.0, 75.0), - damping_distribution_params=(1.0, 9.0), - operation="abs", - distribution="uniform", - ) - - randomized_stiffness = actuator.stiffness[env_ids] - randomized_damping = actuator.damping[env_ids] - self.assertGreater(torch.unique(randomized_stiffness).numel(), 1) - self.assertGreater(torch.unique(randomized_damping).numel(), 1) - torch.testing.assert_close(randomized_stiffness, anymal.data.joint_stiffness.torch[env_ids]) - torch.testing.assert_close(randomized_damping, anymal.data.joint_damping.torch[env_ids]) - torch.testing.assert_close(actuator.stiffness[1:], stiffness_before[1:]) - torch.testing.assert_close(actuator.damping[1:], damping_before[1:]) - - def test_single_articulation(self): - sim_cfg = SimulationCfg(dt=DT, physics=PhysxCfg(), 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0, 0)) - art_cfg = ANYMAL_C_CFG.replace( - actuators=IDEAL_PD_ACTUATORS, - prim_path="/World/Env_[^/]*/Robot", - ) - anymal = Articulation(art_cfg) - sim.reset() - - adapter = anymal.newton_actuator_adapter - self.assertIsNotNone(adapter, "PhysX per-articulation adapter should exist") - read = functools.partial(read_group_parameter, anymal.actuators) - n = anymal.num_joints - kp_before = read("legs", "controller", "kp").clone() - kd_before = read("legs", "controller", "kd").clone() - - 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], kp_before[env_idx]) - torch.testing.assert_close(read("legs", "controller", "kd")[env_idx], kd_before[env_idx]) - - def test_two_articulations(self): - from isaaclab_assets import CARTPOLE_CFG # noqa: PLC0415 - - sim_cfg = SimulationCfg(dt=DT, physics=PhysxCfg(), 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 - for i in range(NUM_ENVS): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 6.0, 0, 0)) - - anymal_cfg = ANYMAL_C_CFG.replace(actuators=IDEAL_PD_ACTUATORS, prim_path="/World/Env_[^/]*/Anymal") - cartpole_cfg = CARTPOLE_CFG.replace( - actuators=CARTPOLE_EXPLICIT_ACTUATORS, - prim_path="/World/Env_[^/]*/Cartpole", - ) - cartpole_cfg.init_state = cartpole_cfg.init_state.replace(pos=(0.0, 3.0, 2.0)) - anymal = Articulation(anymal_cfg) - cartpole = Articulation(cartpole_cfg) - sim.reset() - - # On PhysX each articulation owns its own adapter — they are distinct objects. - anymal_adapter = anymal.newton_actuator_adapter - cartpole_adapter = cartpole.newton_actuator_adapter - self.assertIsNotNone(anymal_adapter) - self.assertIsNotNone(cartpole_adapter) - self.assertIsNot(anymal_adapter, cartpole_adapter) - - anymal_read = functools.partial(read_group_parameter, anymal.actuators) - cartpole_read = functools.partial(read_group_parameter, cartpole.actuators) - n_cp = cartpole.num_joints - anymal_kp_before = anymal_read("legs", "controller", "kp").clone() - anymal_kd_before = anymal_read("legs", "controller", "kd").clone() - cp_kp_before = cartpole_read("all_joints", "controller", "kp").clone() - cp_kd_before = cartpole_read("all_joints", "controller", "kd").clone() - - env = MockEnv({"anymal": anymal, "cartpole": cartpole}, NUM_ENVS, anymal.device) - term, asset_cfg = build_dr_term(env, "cartpole") - 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", - ) - - cp_kp_after = cartpole_read("all_joints", "controller", "kp") - cp_kd_after = cartpole_read("all_joints", "controller", "kd") - torch.testing.assert_close(cp_kp_after[0], torch.full((n_cp,), 100.0, device=anymal.device)) - torch.testing.assert_close(cp_kd_after[0], torch.full((n_cp,), 5.0, device=anymal.device)) - # Cartpole's other envs are untouched (env_ids=[0] only). - for env_idx in range(1, NUM_ENVS): - torch.testing.assert_close(cp_kp_after[env_idx], cp_kp_before[env_idx]) - torch.testing.assert_close(cp_kd_after[env_idx], cp_kd_before[env_idx]) - - # ANYmal's controllers are fully untouched — DR was scoped to cartpole. - torch.testing.assert_close(anymal_read("legs", "controller", "kp"), anymal_kp_before) - torch.testing.assert_close(anymal_read("legs", "controller", "kd"), anymal_kd_before) - - -# --------------------------------------------------------------------------- -# Per-env reset: actuator state isolation -# --------------------------------------------------------------------------- - - -class TestActuatorStateReset(ActuatorStateResetBase, unittest.TestCase): - """Per-env actuator state reset isolation on the PhysX backend. - - The scenario and assertions live in :class:`ActuatorStateResetBase`; - this subclass provides the PhysX sim config and the per-articulation - adapter (``articulation.newton_actuator_adapter``). - """ - - def _make_sim_cfg(self, use_newton_actuators: bool) -> SimulationCfg: - return SimulationCfg(dt=DT, physics=PhysxCfg(), use_newton_actuators=use_newton_actuators) - - def _make_articulation(self) -> Articulation: - return Articulation(ANYMAL_C_CFG.replace(actuators=DELAYED_PD_ACTUATORS, prim_path="/World/Env_.*/Robot")) - - def _get_adapter(self, articulation): - return articulation.newton_actuator_adapter - - -# --------------------------------------------------------------------------- -# RemotizedPD equivalence: PD + delay + position-based clamping lookup table -# --------------------------------------------------------------------------- - - -class TestRemotizedPDEquivalence(_EquivalenceTestBase): - """RemotizedPD (PD + delay + position-based clamping): Lab vs Newton (PhysX). - - Uses the Spot knee lookup table on ANYmal's KFE joints with IdealPD - on HAA and HFE. - """ - - __test__ = True - - @classmethod - def setUpClass(cls): - from isaaclab.actuators.actuator_pd_cfg import RemotizedPDActuatorCfg # noqa: PLC0415 - - cls.actuators = { - "hips": IdealPDActuatorCfg( - joint_names_expr=[".*HAA", ".*HFE"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ), - "knees": RemotizedPDActuatorCfg( - joint_names_expr=[".*KFE"], - stiffness=60.0, - damping=1.5, - actuator_effort_limit=80.0, - max_delay=3, - joint_parameter_lookup=SPOT_KNEE_LOOKUP, - ), + articulation.update(sim.cfg.dt) + result = { + "initial_position": initial_position, + "final_position": articulation.data.joint_pos.torch.clone(), + "applied_effort": torch.stack(applied), + "native_path": articulation._has_newton_actuators, + "has_wrapper": articulation._physx_actuator_wrapper is not None, + "has_nonidentity_ordering": tuple(articulation.joint_names) != tuple(articulation.backend_joint_names), } - super().setUpClass() - - -# --------------------------------------------------------------------------- -# Neural network actuator authoring: MLP and LSTM -# --------------------------------------------------------------------------- - - -class TestNeuralMLPFunctional(unittest.TestCase): - """Verify ActuatorNetMLPCfg runs on PhysX with Newton actuators.""" - - @classmethod - def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetMLPCfg # noqa: PLC0415 - - cls.mlp_path = make_dummy_mlp_checkpoint() - cls.result = _run_simulation( - { - "mlp_legs": ActuatorNetMLPCfg( - joint_names_expr=[".*HAA"], - network_file=cls.mlp_path, - saturation_effort=120.0, - actuator_effort_limit=80.0, - actuator_velocity_limit=7.5, - pos_scale=-1.0, - vel_scale=1.0, - torque_scale=1.0, - 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, - ), - }, - use_newton_actuators=True, - ) - - @classmethod - def tearDownClass(cls): - os.unlink(cls.mlp_path) - - 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}", - ) - - -class TestNeuralLSTMFunctional(unittest.TestCase): - """Verify ActuatorNetLSTMCfg runs on PhysX with Newton actuators.""" - - @classmethod - def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetLSTMCfg # noqa: PLC0415 - - cls.lstm_path = make_dummy_lstm_checkpoint() - cls.result = _run_simulation( - { - "lstm_legs": ActuatorNetLSTMCfg( - joint_names_expr=[".*HAA"], - 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"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ), - }, - use_newton_actuators=True, - ) - - @classmethod - def tearDownClass(cls): - os.unlink(cls.lstm_path) - - 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}", - ) - - -if __name__ == "__main__": - unittest.main() + return result + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") +def test_lab_and_newton_actuator_paths_dispatch_equivalent_initial_command_and_move() -> None: + """Distinguish both dispatch paths and require the same first torque plus live motion.""" + lab = _run_actuator_path(use_newton_actuators=False) + native = _run_actuator_path(use_newton_actuators=True) + + assert not lab["native_path"] + assert not lab["has_wrapper"] + assert native["native_path"] + assert native["has_wrapper"] + assert lab["has_nonidentity_ordering"] + assert native["has_nonidentity_ordering"] + assert torch.any(lab["applied_effort"] != 0.0) + assert torch.any(native["applied_effort"] != 0.0) + assert torch.any(lab["final_position"] != lab["initial_position"]) + assert torch.any(native["final_position"] != native["initial_position"]) + torch.testing.assert_close(native["applied_effort"][0], lab["applied_effort"][0], atol=1e-6, rtol=0) + torch.testing.assert_close(lab["applied_effort"][0], torch.ones_like(lab["applied_effort"][0])) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 40b9b80fb33a..f77ad3a06e05 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -3,1300 +3,112 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Launch Isaac Sim Simulator first.""" +"""Minimal real-PhysX integration coverage for rigid objects.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices - -# launch omniverse app -simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app - -"""Rest everything follows.""" -import sys -from typing import Literal +simulation_app = AppLauncher(headless=True, device="cpu").app import pytest import torch import warp as wp -from flaky import flaky from isaaclab_physx.assets import RigidObject import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg from isaaclab.sim import build_simulation_context -from isaaclab.sim.spawners import materials -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, - quat_rotate, - random_orientation, -) - - -def generate_cubes_scene( - num_cubes: int = 1, - height=1.0, - api: Literal["none", "rigid_body", "articulation_root"] = "rigid_body", - kinematic_enabled: bool = False, - device: str = "cuda:0", -) -> tuple[RigidObject, torch.Tensor]: - """Generate a scene with the provided number of cubes. - - Args: - num_cubes: Number of cubes to generate. - height: Height of the cubes. - api: The type of API that the cubes should have. - kinematic_enabled: Whether the cubes are kinematic. - device: Device to use for the simulation. - - Returns: - A tuple containing the rigid object representing the cubes and the origins of the cubes. - """ - origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) - - # Resolve spawn configuration - if api == "none": - # since no rigid body properties defined, this is just a static collider - spawn_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), - collision_props=sim_utils.CollisionPropertiesCfg(), - ) - elif api == "rigid_body": - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), +pytestmark = pytest.mark.integration + + +def _spawn_rigid_objects() -> RigidObject: + """Author two local cuboids without Nucleus dependencies.""" + for env_index in range(2): + sim_utils.create_prim(f"/World/Env_{env_index}", "Xform", translation=(2.0 * env_index, 0.0, 0.0)) + return RigidObject( + RigidObjectCfg( + prim_path="/World/Env_[^/]*/Object", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), ) - elif api == "articulation_root": - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/RigidObject/Cube/dex_cube_instanceable_with_articulation_root.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - else: - raise ValueError(f"Unknown api: {api}") - - # Create rigid object - cube_object_cfg = RigidObjectCfg( - prim_path="/World/Table_[^/]*/Object", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height)), ) - cube_object = RigidObject(cfg=cube_object_cfg) - - return cube_object, origins - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_initialization(num_cubes, device): - """Test initialization for prim with rigid body API at the provided prim path.""" - with build_simulation_context(device=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 - 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.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_initialization_with_kinematic_enabled(num_cubes, device): - """Test that initialization for prim with kinematic flag enabled.""" - with build_simulation_context(device=device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - 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) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - # check that the object is kinematic - default_root_pose = cube_object.data.default_root_pose.torch.clone() - default_root_vel = cube_object.data.default_root_vel.torch.clone() - default_root_pose[:, :3] += origins - torch.testing.assert_close(cube_object.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, default_root_vel) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_initialization_with_no_rigid_body(num_cubes, device): - """Test that initialization fails when no rigid body is found at the provided prim path.""" - with build_simulation_context(device=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, api="none", device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_initialization_with_articulation_root(num_cubes, device): - """Test that initialization fails when an articulation root is found at the provided prim path.""" - with build_simulation_context(device=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, api="articulation_root", device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -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 build_simulation_context(device=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) - - # play the simulator - 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.parametrize("num_cubes", [2, 4]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_external_force_on_single_body(num_cubes, device): - """Test application of external force on the base of the object. - - In this test, 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 validate that this works when we apply the force in the global frame and in the local frame. - """ - # Generate cubes scene - with build_simulation_context(device=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) - - # Play the simulator - 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 - 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 * wp.to_torch(cube_object.root_view.get_masses())[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() - - is_global = False - if i % 2 == 0: - is_global = True - positions = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] - else: - positions = None - - # apply force - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=positions, - 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) - - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - cube_object.data.root_pos_w.torch[0::2, 2], torch.ones(num_cubes // 2, device=sim.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", [2, 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 build_simulation_context(device=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) - - # Play the simulator - 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 - 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] = 500.0 - external_wrench_positions_b[0::2, :, 1] = 1.0 - - # Desired force and torque - desired_force = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) - desired_force[0::2, :, 2] = 1000.0 - desired_torque = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[0::2, :, 0] = 1000.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() - - is_global = False - if i % 2 == 0: - is_global = True - body_com_pos_w = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] - 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 - - # apply force - cube_object.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, - ) - cube_object.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, - ) - torch.testing.assert_close( - cube_object._permanent_wrench_composer.out_force_b.torch[:, 0, :], - desired_force[:, 0, :], - rtol=1e-6, - atol=1e-7, - ) - torch.testing.assert_close( - cube_object._permanent_wrench_composer.out_torque_b.torch[:, 0, :], - desired_torque[:, 0, :], - rtol=1e-6, - atol=1e-7, - ) - # 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) - - # 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) - # 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", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -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 build_simulation_context(device=device, gravity_enabled=False, 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 - 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), - } - - # 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 - torch.testing.assert_close(value, expected_value, rtol=1e-3, atol=1e-3) - - cube_object.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_reset_rigid_object(num_cubes, device): - """Test resetting the state of the rigid object.""" - with build_simulation_context(device=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 - 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) - - if i % 2 == 0: - # reset object - 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 - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_rigid_body_set_material_properties(num_cubes, device): - """Test getting and setting material properties of rigid object.""" - with build_simulation_context( - device=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 sim - sim.reset() - - # Set material properties - static_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) - dynamic_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) - restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) - - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - # Add friction to cube - cube_object.root_view.set_material_properties( - wp.from_torch(materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) +def test_rigid_object_real_physx_seams() -> None: + """Prove partial state/inertial writes and one real external-wrench delivery.""" + with build_simulation_context(device="cpu", gravity_enabled=False) as sim: + rigid_object = _spawn_rigid_objects() + sim.reset() + + assert rigid_object.is_initialized + assert rigid_object.num_instances == 2 + assert rigid_object.data.body_mass.torch.shape == (2, 1) + assert rigid_object.data.body_com_pose_b.torch.shape == (2, 1, 7) + assert rigid_object.data.body_inertia.torch.shape == (2, 1, 9) + + env_ids = torch.tensor([1], dtype=torch.int32) + body_ids = torch.tensor([0], dtype=torch.int32) + initial_pose = rigid_object.data.root_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids].clone() + target_pose[:, :3] += torch.tensor([0.25, -0.1, 0.3]) + target_velocity = torch.tensor([[0.0, 0.2, 0.0, 0.0, 0.0, 0.1]]) + rigid_object.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + rigid_object.write_root_link_velocity_to_sim_index(root_velocity=target_velocity, env_ids=env_ids) + torch.testing.assert_close(rigid_object.data.root_link_pose_w.torch[env_ids], target_pose) + torch.testing.assert_close(rigid_object.data.root_link_vel_w.torch[env_ids], target_velocity) + torch.testing.assert_close(rigid_object.data.root_link_pose_w.torch[:1], initial_pose[:1]) + + masses = torch.tensor([[3.0]]) + rigid_object.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(rigid_object.data.body_mass.torch[env_ids][:, body_ids], masses) + raw_masses = wp.to_torch(rigid_object.root_view.get_masses()).reshape(2, 1) + torch.testing.assert_close(raw_masses[env_ids][:, body_ids], masses) + + coms = rigid_object.data.body_com_pose_b.torch[env_ids][:, body_ids].clone() + coms[..., :3] = torch.tensor([[[0.03, -0.02, 0.01]]]) + rigid_object.set_coms_index(coms=coms, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(rigid_object.data.body_com_pose_b.torch[env_ids][:, body_ids], coms) + raw_coms = wp.to_torch(rigid_object.root_view.get_coms().view(wp.float32)).reshape(2, 1, 7) + torch.testing.assert_close(raw_coms[env_ids][:, body_ids], coms) + + inertias = rigid_object.data.body_inertia.torch[env_ids][:, body_ids].clone() + inertias[..., 0] *= 1.2 + inertias[..., 4] *= 1.3 + inertias[..., 8] *= 1.4 + rigid_object.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(rigid_object.data.body_inertia.torch[env_ids][:, body_ids], inertias) + raw_inertias = wp.to_torch(rigid_object.root_view.get_inertias()).reshape(2, 1, 9) + torch.testing.assert_close(raw_inertias[env_ids][:, body_ids], inertias) + + materials = torch.empty((2, rigid_object.root_view.max_shapes, 3)) + materials[0] = torch.tensor([0.8, 0.4, 0.2]) + materials[1] = torch.tensor([0.7, 0.3, 0.1]) + all_env_ids = torch.arange(2, dtype=torch.int32) + rigid_object.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.from_torch(all_env_ids, dtype=wp.int32) ) + torch.testing.assert_close(wp.to_torch(rigid_object.root_view.get_material_properties()), materials) - # Simulate physics - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Get material properties - materials_to_check = wp.to_torch(cube_object.root_view.get_material_properties()) - - # Check if material properties are set correctly - torch.testing.assert_close(materials_to_check.reshape(num_cubes, 3), materials) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_set_material_properties_via_view(num_cubes, device): - """Test setting material properties via the PhysX view-level API.""" - with build_simulation_context( - device=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 sim - sim.reset() - - # Get number of shapes - max_shapes = cube_object.root_view.max_shapes - - # Generate random material properties: (static_friction, dynamic_friction, restitution) - materials = torch.empty(num_cubes, max_shapes, 3, device="cpu").uniform_(0.0, 1.0) - # Ensure dynamic friction <= static friction - materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) - - # Set material properties via the PhysX view-level API - env_ids = torch.arange(num_cubes, dtype=torch.int32) - cube_object.root_view.set_material_properties( - wp.from_torch(materials, dtype=wp.float32), wp.from_torch(env_ids, dtype=wp.int32) + rigid_object.write_root_link_velocity_to_sim_index( + root_velocity=torch.zeros((2, 6)), env_ids=torch.tensor([0, 1], dtype=torch.int32) ) - - # Simulate physics - sim.step() - cube_object.update(sim.cfg.dt) - - # Get material properties from simulation - materials_check = wp.to_torch(cube_object.root_view.get_material_properties()) - - # Check if material properties are set correctly - torch.testing.assert_close(materials_check, materials) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_rigid_body_no_friction(num_cubes, device): - """Test that a rigid object with no friction maintains its tangential velocity on a plane.""" - with build_simulation_context(device=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, height=0.0, device=device) - - # Create ground plane with no friction - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - static_friction=0.0, - dynamic_friction=0.0, - restitution=0.0, - ) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set material friction properties to be all zero - static_friction = torch.zeros(num_cubes, 1) - dynamic_friction = torch.zeros(num_cubes, 1) - restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - cube_object.root_view.set_material_properties( - wp.from_torch(cube_object_materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) - ) - - # Set initial velocity - # Initial velocity in X to get the block moving - initial_velocity = torch.zeros((num_cubes, 6), device=sim.cfg.device) - initial_velocity[:, 0] = 0.1 - - cube_object.write_root_velocity_to_sim_index(root_velocity=initial_velocity) - - # Simulate physics - for _ in range(5): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Non-deterministic when on GPU, so we use different tolerances - if device.startswith("cuda"): - tolerance = 1e-2 - else: - tolerance = 1e-5 - - torch.testing.assert_close( - cube_object.data.root_lin_vel_w.torch[:, :2], initial_velocity[:, :2], rtol=1e-5, atol=tolerance - ) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_rigid_body_with_static_friction(num_cubes, device): - """Test that static friction applied to rigid object works as expected. - - This test works by applying a force to the object and checking if the object moves or not based on the - mu (coefficient of static friction) value set for the object. We set the static friction to be non-zero and - apply a force to the object. When the force applied is below mu, the object should not move. When the force - applied is above mu, the object should move. - """ - with build_simulation_context(device=device, dt=0.01, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.03125, device=device) - - # Create ground plane - static_friction_coefficient = 0.5 - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - static_friction=static_friction_coefficient, - dynamic_friction=static_friction_coefficient, # This shouldn't be required but is due to a bug in PhysX - ) - ) - cfg.func("/World/GroundPlane", cfg) - - # Play sim - sim.reset() - - # Set static friction to be non-zero - # Dynamic friction also needs to be zero due to a bug in PhysX - static_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) - dynamic_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) - restitution = torch.zeros(num_cubes, 1) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - # Add friction to cube - cube_object.root_view.set_material_properties( - wp.from_torch(cube_object_materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) - ) - - # let everything settle - for _ in range(100): - sim.step() - cube_object.update(sim.cfg.dt) - cube_object.write_root_velocity_to_sim_index(root_velocity=torch.zeros((num_cubes, 6), device=sim.device)) - cube_mass = wp.to_torch(cube_object.root_view.get_masses()) - gravity_magnitude = abs(sim.cfg.gravity[2]) - # 2 cases: force applied is below and above mu - # below mu: block should not move as the force applied is <= mu - # above mu: block should move as the force applied is > mu - for force in "below_mu", "above_mu": - # set initial velocity to zero - cube_object.write_root_velocity_to_sim_index(root_velocity=torch.zeros((num_cubes, 6), device=sim.device)) - - external_wrench_b = torch.zeros((num_cubes, 1, 6), device=sim.device) - if force == "below_mu": - external_wrench_b[..., 0] = static_friction_coefficient * cube_mass * gravity_magnitude * 0.99 - else: - external_wrench_b[..., 0] = static_friction_coefficient * cube_mass * gravity_magnitude * 1.01 - - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - ) - - # Get root state - initial_root_pos = cube_object.data.root_pos_w.torch.clone() - # Simulate physics - for _ in range(200): - # apply the wrench - cube_object.write_data_to_sim() - sim.step() - # update object - cube_object.update(sim.cfg.dt) - if force == "below_mu": - # Assert that the block has not moved - torch.testing.assert_close( - cube_object.data.root_pos_w.torch, initial_root_pos, rtol=2e-3, atol=2e-3 - ) - if force == "above_mu": - assert (cube_object.data.root_pos_w.torch[..., 0] - initial_root_pos[..., 0] > 0.02).all() - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_rigid_body_with_restitution(num_cubes, device): - """Test that restitution when applied to rigid object works as expected. - - This test works by dropping a block from a height and checking if the block bounces or not based on the - restitution value set for the object. We set the restitution to be non-zero and drop the block from a height. - When the restitution is 0, the block should not bounce. When the restitution is between 0 and 1, the block - should bounce with less energy. - """ - for expected_collision_type in "partially_elastic", "inelastic": - with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - # Set static friction to be non-zero - if expected_collision_type == "inelastic": - restitution_coefficient = 0.0 - elif expected_collision_type == "partially_elastic": - restitution_coefficient = 0.5 - - # Create ground plane such that has a restitution of 1.0 (perfectly elastic collision) - cfg = sim_utils.GroundPlaneCfg( - physics_material=materials.RigidBodyMaterialCfg( - restitution=restitution_coefficient, - ) - ) - cfg.func("/World/GroundPlane", cfg) - - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - # Play sim - sim.reset() - - root_pose = torch.zeros(num_cubes, 7, device=sim.device) - root_pose[:, 3] = 1.0 # To make orientation a quaternion - for i in range(num_cubes): - root_pose[i, 1] = 1.0 * i - root_pose[:, 2] = 1.0 # Set an initial drop height - root_vel = torch.zeros(num_cubes, 6, device=sim.device) - root_vel[:, 2] = -1.0 # Set an initial downward velocity - - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - static_friction = torch.zeros(num_cubes, 1) - dynamic_friction = torch.zeros(num_cubes, 1) - restitution = torch.Tensor([[restitution_coefficient]] * num_cubes) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - # Add restitution to cube - cube_object.root_view.set_material_properties( - wp.from_torch(cube_object_materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) - ) - - curr_z_velocity = cube_object.data.root_lin_vel_w.torch[:, 2].clone() - - for _ in range(100): - sim.step() - - # update object - cube_object.update(sim.cfg.dt) - curr_z_velocity = cube_object.data.root_lin_vel_w.torch[:, 2].clone() - - if expected_collision_type == "inelastic": - # Allow a small contact separation velocity while ensuring that the block does not bounce. - assert (curr_z_velocity <= 1e-3).all() - - if torch.all(curr_z_velocity <= 0.0): - # Still in the air - prev_z_velocity = curr_z_velocity - else: - # collision has happened, exit the for loop - break - - if expected_collision_type == "partially_elastic": - # Assert that the block has lost some energy by checking that the z velocity is less - assert torch.all(torch.le(abs(curr_z_velocity), abs(prev_z_velocity))) - assert (curr_z_velocity > 0.0).all() - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.isaacsim_ci -def test_rigid_body_set_mass(num_cubes, device): - """Test getting and setting mass of rigid object.""" - with build_simulation_context( - device=device, gravity_enabled=False, add_ground_plane=True, auto_add_lighting=True - ) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) - - # Play sim - sim.reset() - - # Get masses before increasing - original_masses = wp.to_torch(cube_object.root_view.get_masses()) - - assert original_masses.shape == (num_cubes, 1) - - # Randomize mass of the object - masses = original_masses + torch.FloatTensor(num_cubes, 1).uniform_(4, 8) - - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - # Add friction to cube - cube_object.root_view.set_masses( - wp.from_torch(masses, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) + initial_velocity = rigid_object.data.root_com_lin_vel_w.torch.clone() + rigid_object.permanent_wrench_composer.set_forces_and_torques_index( + forces=torch.tensor([[[6.0, 0.0, 0.0]]]), + torques=torch.zeros((1, 1, 3)), + env_ids=torch.tensor([0], dtype=torch.int32), + body_ids=body_ids, ) - - torch.testing.assert_close(wp.to_torch(cube_object.root_view.get_masses()), masses) - - # Simulate physics - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - masses_to_check = wp.to_torch(cube_object.root_view.get_masses()) - - # Check if mass is set correctly - torch.testing.assert_close(masses, masses_to_check) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [True, False]) -@pytest.mark.isaacsim_ci -def test_gravity_vec_w(num_cubes, device, gravity_enabled): - """Test that gravity vector direction is set correctly for the rigid object.""" - with build_simulation_context(device=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: - gravity_dir = (0.0, 0.0, -1.0) - else: - gravity_dir = (0.0, 0.0, 0.0) - - # Play sim - sim.reset() - - # Check that gravity is set correctly - assert cube_object.data.GRAVITY_VEC_W.torch[0, 0] == gravity_dir[0] - assert cube_object.data.GRAVITY_VEC_W.torch[0, 1] == gravity_dir[1] - assert cube_object.data.GRAVITY_VEC_W.torch[0, 2] == gravity_dir[2] - - # 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.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.isaacsim_ci -@flaky(max_runs=3, min_passes=1) -def test_body_root_state_properties(num_cubes, device, with_offset): - """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" - with build_simulation_context(device=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) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - # Play sim - 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) - - com = wp.to_torch(cube_object.root_view.get_coms()) - com[..., :3] = offset.to("cpu") - cube_object.root_view.set_coms(wp.from_torch(com, dtype=wp.float32), wp.from_torch(env_idx, dtype=wp.int32)) - - # check ceter of mass has been set - torch.testing.assert_close(wp.to_torch(cube_object.root_view.get_coms()), com) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # Simulate physics - for _ in range(100): - # spin the object around Z axis (com) - cube_object.write_root_velocity_to_sim_index(root_velocity=spin_twist.repeat(num_cubes, 1)) - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # get state properties - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - 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_link_pose_w = cube_object.data.body_link_pose_w.torch - body_link_vel_w = cube_object.data.body_link_vel_w.torch - 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, root_link_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) - torch.testing.assert_close(body_link_pose_w, body_link_pose_w) - 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) - torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3]) - torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2)) - # 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) - 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)) - - # 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:]) - torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:]) - - # orientation of link will match root state will always match - torch.testing.assert_close(root_link_pose_w[..., 3:], root_link_pose_w[..., 3:]) - torch.testing.assert_close(body_link_pose_w[..., 3:], body_link_pose_w[..., 3:]) - - # 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]) - torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3]) - # 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, atol=1e-4, rtol=1e-4) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), atol=1e-4, rtol=1e-4) - - # ang_vel will always match - torch.testing.assert_close(root_com_vel_w[..., 3:], root_com_vel_w[..., 3:]) - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_com_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -@pytest.mark.isaacsim_ci -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.""" - with build_simulation_context(device=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) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - # Play sim - 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) - - com = wp.to_torch(cube_object.root_view.get_coms()) - com[..., :3] = offset.to("cpu") - cube_object.root_view.set_coms(wp.from_torch(com, dtype=wp.float32), wp.from_torch(env_idx, dtype=wp.int32)) - - # check center of mass has been set - torch.testing.assert_close(wp.to_torch(cube_object.root_view.get_coms()), com) - - 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) - - env_idx = env_idx.to(device) - for i in range(10): - # perform step - sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - 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) - 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 - ) - - 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) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) -@pytest.mark.parametrize("state_location", ["com", "link", "root"]) -@pytest.mark.isaacsim_ci -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 build_simulation_context(device=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) - env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - # Play sim - 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) - - com = wp.to_torch(cube_object.root_view.get_coms()) - com[..., :3] = offset.to("cpu") - cube_object.root_view.set_coms(wp.from_torch(com, dtype=wp.float32), wp.from_torch(env_idx, dtype=wp.int32)) - - # check ceter of mass has been set - torch.testing.assert_close(wp.to_torch(cube_object.root_view.get_coms()), com) - - 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) - - env_idx = env_idx.to(device) - - # perform step + rigid_object.write_data_to_sim() sim.step() - # update buffers - cube_object.update(sim.cfg.dt) - - 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:]) - - -@pytest.mark.isaacsim_ci -def test_warmup_attach_stage_not_called_for_cpu(): - """Regression test: CPU warmup must force-load without explicitly attaching the stage. + rigid_object.update(sim.cfg.dt) - Bug (commit 0ba9c5cb3b): ``PhysxManager._warmup_and_create_views()`` called - ``_physx_sim.attach_stage()`` unconditionally before ``force_load_physics_from_usd()``. - These are two alternative initialization patterns; combining them causes - double-initialization that corrupts the CPU MBP broadphase, producing - non-deterministic collision failures (objects passing through surfaces). - - The CPU pipeline attaches implicitly via ``force_load_physics_from_usd()`` when - the ``omni.physics.physx`` bridge registers the backend. - - This test verifies that the PhysX backend is registered with the unified physics - API, ``attach_stage`` is not called, and ``force_load_physics_from_usd`` is called - exactly once during CPU warmup. - """ - from unittest.mock import MagicMock, patch - - import omni.kit.app - import omni.physx - - with build_simulation_context(device="cpu", add_ground_plane=True, dt=0.01, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - generate_cubes_scene(num_cubes=1, height=1.0, device="cpu") - - # PhysxManager no longer caches the simulation interface; it resolves it on each use - # via ``omni.physx.get_physx_simulation_interface()`` (the accessor memoizes it). - # The PhysX interfaces are C++ bindings whose attributes are read-only. Patch - # their accessors with wrapping mocks so the real calls still execute. - physx_spy = MagicMock(wraps=omni.physx.get_physx_interface()) - physx_sim_spy = MagicMock(wraps=omni.physx.get_physx_simulation_interface()) - with ( - patch("omni.physx.get_physx_interface", return_value=physx_spy), - patch("omni.physx.get_physx_simulation_interface", return_value=physx_sim_spy), - ): - sim.reset() - - extension_manager = omni.kit.app.get_app().get_extension_manager() - assert extension_manager.is_extension_enabled("omni.physics.physx"), ( - "The omni.physics.physx bridge must register PhysX with the unified physics API." - ) - assert physx_sim_spy.attach_stage.call_count == 0, ( - f"attach_stage() was called {physx_sim_spy.attach_stage.call_count} time(s) during CPU warmup. " - "This indicates the CPU MBP broadphase double-initialization regression is present." + assert rigid_object.data.root_com_lin_vel_w.torch[0, 0] > initial_velocity[0, 0] + torch.testing.assert_close( + rigid_object.data.root_com_lin_vel_w.torch[1], initial_velocity[1], atol=1e-5, rtol=0 ) - physx_spy.force_load_physics_from_usd.assert_called_once_with() diff --git a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py index 6286c017e2e9..1fde086389be 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py @@ -3,21 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Launch Isaac Sim Simulator first.""" +"""Minimal real-PhysX integration coverage for rigid-object collections.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices - -# launch omniverse app -simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app -"""Rest everything follows.""" - -import sys +simulation_app = AppLauncher(headless=True, device="cpu").app import pytest import torch @@ -27,874 +17,102 @@ import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg, RigidObjectCollectionCfg from isaaclab.sim import build_simulation_context -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, - quat_rotate, - random_orientation, - subtract_frame_transforms, -) - - -def generate_cubes_scene( - num_envs: int = 1, - num_cubes: int = 1, - height=1.0, - has_api: bool = True, - kinematic_enabled: bool = False, - device: str = "cuda:0", -) -> tuple[RigidObjectCollection, torch.Tensor]: - """Generate a scene with the provided number of cubes. - Args: - num_envs: Number of envs to generate. - num_cubes: Number of cubes to generate. - height: Height of the cubes. - has_api: Whether the cubes have a rigid body API on them. - kinematic_enabled: Whether the cubes are kinematic. - device: Device to use for the simulation. +pytestmark = pytest.mark.integration - Returns: - A tuple containing the rigid object representing the cubes and the origins of the cubes. - """ - origins = torch.tensor([(i * 3.0, 0, height) for i in range(num_envs)]).to(device) - # Create Top-level Xforms, one for each cube - for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) - - # Resolve spawn configuration - if has_api: - spawn_cfg = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ) - else: - # since no rigid body properties defined, this is just a static collider - spawn_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), +def _cube_cfg(prim_path: str, y: float) -> RigidObjectCfg: + """Create one local collection-body configuration.""" + return RigidObjectCfg( + prim_path=prim_path, + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), collision_props=sim_utils.CollisionPropertiesCfg(), - ) - - # create the rigid object configs - cube_config_dict = {} - for i in range(num_cubes): - cube_object_cfg = RigidObjectCfg( - prim_path=f"/World/Table_[^/]*/Object_{i}", - spawn=spawn_cfg, - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 3 * i, height)), - ) - cube_config_dict[f"cube_{i}"] = cube_object_cfg - # create the rigid object collection - cube_object_collection_cfg = RigidObjectCollectionCfg(rigid_objects=cube_config_dict) - cube_object_colection = RigidObjectCollection(cfg=cube_object_collection_cfg) - - return cube_object_colection, origins - - -@pytest.fixture -def sim(request): - """Create simulation context with the specified device.""" - device = request.getfixturevalue("device") - if "gravity_enabled" in request.fixturenames: - gravity_enabled = request.getfixturevalue("gravity_enabled") - else: - gravity_enabled = True # default to gravity enabled - with build_simulation_context(device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled) as sim: - sim._app_control_on_stop_handle = None - yield sim - - -@pytest.mark.parametrize("num_envs", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 3]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization(sim, num_envs, num_cubes, device): - """Test initialization for prim with rigid body API at the provided prim path.""" - 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 - - # Play sim - sim.reset() - - # Check if object is initialized - assert object_collection.is_initialized - assert len(object_collection.body_names) == num_cubes - - # Check buffers that exist and have correct shapes - 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_id_conversion(sim, device): - """Test environment and object index conversion to physics view indices.""" - object_collection, _ = generate_cubes_scene(num_envs=2, num_cubes=3, device=device) - - # Play sim - sim.reset() - - expected = [ - torch.tensor([4, 5], device=device, dtype=torch.int32), - torch.tensor([4], device=device, dtype=torch.int32), - torch.tensor([0, 2, 4], device=device, dtype=torch.int32), - torch.tensor([1, 3, 5], device=device, dtype=torch.int32), - ] - - torch_all_env_indices = wp.to_torch(object_collection._ALL_ENV_INDICES) - torch_all_body_indices = wp.to_torch(object_collection._ALL_BODY_INDICES) - - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices, torch_all_body_indices[None, 2], device=device + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, y, 1.0)), ) - assert (wp.to_torch(view_ids) == expected[0]).all() - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices[None, 0], torch_all_body_indices[None, 2], device=device - ) - assert (wp.to_torch(view_ids) == expected[1]).all() - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices[None, 0], torch_all_body_indices, device=device - ) - assert (wp.to_torch(view_ids) == expected[2]).all() - view_ids = object_collection._env_body_ids_to_view_ids( - torch_all_env_indices[None, 1], torch_all_body_indices, device=device - ) - assert (wp.to_torch(view_ids) == expected[3]).all() - - -@pytest.mark.parametrize("num_envs", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 3]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_kinematic_enabled(sim, num_envs, num_cubes, device): - """Test that initialization for prim with kinematic flag enabled.""" - object_collection, origins = generate_cubes_scene( - num_envs=num_envs, num_cubes=num_cubes, kinematic_enabled=True, device=device - ) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(object_collection) < 10 - - # Play sim - sim.reset() - - # Check if object is initialized - assert object_collection.is_initialized - assert len(object_collection.body_names) == num_cubes - - # Check buffers that exist and have correct shapes - 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) - - # Simulate physics - for _ in range(2): - sim.step() - object_collection.update(sim.cfg.dt) - # check that the object is kinematic - default_body_pose = object_collection.data.default_body_pose.torch.clone() - default_body_vel = object_collection.data.default_body_vel.torch.clone() - default_body_pose[..., :3] += origins.unsqueeze(1) - torch.testing.assert_close(object_collection.data.body_link_pose_w.torch, default_body_pose) - torch.testing.assert_close(object_collection.data.body_link_vel_w.torch, default_body_vel) - - -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization_with_no_rigid_body(sim, num_cubes, device): - """Test that initialization fails when no rigid body is found at the provided prim path.""" - object_collection, _ = generate_cubes_scene(num_cubes=num_cubes, has_api=False, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(object_collection) < 10 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(sim, device): - """Test if external force buffer correctly updates in the force value is zero case.""" - num_envs = 2 - num_cubes = 1 - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 4]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body(sim, num_envs, num_cubes, device): - """Test application of external force on the base of the object.""" - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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 - 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 - 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() - - is_global = False - if i % 2 == 0: - positions = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] - is_global = True - else: - positions = None - - # apply force - object_collection.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=positions, - body_ids=object_ids, - 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) - # First object should still be at the same Z position (1.0) - torch.testing.assert_close( - object_collection.data.body_link_pos_w.torch[:, 0::2, 2], - torch.ones_like(object_collection.data.body_link_pos_w.torch[:, 0::2, 2]), - ) - # 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", [1, 2]) -@pytest.mark.parametrize("num_cubes", [1, 4]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(sim, 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. - """ - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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 - 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] = 500.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() - - is_global = False - if i % 2 == 0: - body_com_pos_w = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] - 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 - is_global = True - else: - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - - # apply force - object_collection.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=object_ids, - env_ids=None, - is_global=is_global, - ) - object_collection.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=object_ids, - is_global=is_global, +def _spawn_collection() -> RigidObjectCollection: + """Author a two-instance, two-body local collection.""" + for env_index in range(2): + sim_utils.create_prim(f"/World/Env_{env_index}", "Xform", translation=(3.0 * env_index, 0.0, 0.0)) + return RigidObjectCollection( + RigidObjectCollectionCfg( + rigid_objects={ + "left": _cube_cfg("/World/Env_[^/]*/Object_0", 0.0), + "right": _cube_cfg("/World/Env_[^/]*/Object_1", 1.0), + } ) - - 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) - - # 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) - # 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", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_set_object_state(sim, num_envs, num_cubes, device, gravity_enabled): - """Test setting the state of the object. - - .. note:: - Turn off gravity for this test as we don't want any external forces acting on the object - to ensure state remains static - """ - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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 - torch.testing.assert_close(value, expected_value, rtol=1e-5, atol=1e-5) - - object_collection.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_envs", [1, 4]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("gravity_enabled", [False]) -def test_object_state_properties(sim, num_envs, num_cubes, device, with_offset, gravity_enabled): - """Test the object_com_state_w and object_link_state_w properties.""" - cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) - view_ids = torch.tensor([x for x in range(num_cubes * num_envs)], dtype=torch.int32) - - 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) - ) - - com = wp.to_torch(cube_object.reshape_view_to_data_2d(cube_object.root_view.get_coms().view(wp.transformf))) - com[..., :3] = offset.to("cpu") - cube_object.root_view.set_coms( - cube_object.reshape_data_to_view_2d(wp.from_torch(com.clone(), dtype=wp.transformf)).view(wp.float32), - wp.from_torch(view_ids, dtype=wp.int32), - ) - - # check center of mass has been set - torch.testing.assert_close( - wp.to_torch(cube_object.reshape_view_to_data_2d(cube_object.root_view.get_coms().view(wp.transformf))), com - ) - - # random z spin velocity - spin_twist = torch.zeros(6, device=device) - spin_twist[5] = torch.randn(1, device=device) - - # initial spawn point - init_com = cube_object.data.body_com_pose_w.torch[..., :3] - - for i in range(10): - # spin the object around Z axis (com) - cube_object.write_body_com_velocity_to_sim_index(body_velocities=spin_twist.repeat(num_envs, num_cubes, 1)) - sim.step() - cube_object.update(sim.cfg.dt) - - # get state properties - object_link_pose_w = cube_object.data.body_link_pose_w.torch - object_link_vel_w = cube_object.data.body_link_vel_w.torch - 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: - # 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]) - - # 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], - ) - - torch.testing.assert_close(-offset, object_link_state_pos_rel_com) - - # 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:]) - - # orientation of link will match object state will always match - torch.testing.assert_close(object_link_pose_w[..., 3:], object_link_pose_w[..., 3:]) - - # 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], - ) - - # 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, atol=1e-4, rtol=1e-3) - - # ang_vel will always match - torch.testing.assert_close(object_com_vel_w[..., 3:], object_com_vel_w[..., 3:]) - torch.testing.assert_close(object_com_vel_w[..., 3:], object_link_vel_w[..., 3:]) - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 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]) -def test_write_object_state(sim, num_envs, num_cubes, device, with_offset, state_location, gravity_enabled): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - # 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) - view_ids = torch.tensor([x for x in range(num_cubes * num_envs)], dtype=torch.int32) - 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) - - 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) ) - com = wp.to_torch(cube_object.reshape_view_to_data_2d(cube_object.root_view.get_coms().view(wp.transformf))) - com[..., :3] = offset.to("cpu") - cube_object.root_view.set_coms( - cube_object.reshape_data_to_view_2d(wp.from_torch(com.clone(), dtype=wp.transformf)).view(wp.float32), - wp.from_torch(view_ids, dtype=wp.int32), - ) - # check center of mass has been set - torch.testing.assert_close( - wp.to_torch(cube_object.reshape_view_to_data_2d(cube_object.root_view.get_coms().view(wp.transformf))), com - ) - - 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): - 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 - ) - - 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) - 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) - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset_object_collection(sim, num_envs, num_cubes, device): - """Test resetting the state of the rigid object.""" - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - 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", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_set_material_properties(sim, num_envs, num_cubes, device): - """Test getting and setting material properties of rigid object.""" - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - sim.reset() - - # Set material properties - static_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) - dynamic_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) - restitution = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.0, 0.2) - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - # Add friction to cube - indices = torch.tensor(range(num_cubes * num_envs), dtype=torch.int) - object_collection.root_view.set_material_properties( - object_collection.reshape_data_to_view_3d(wp.from_torch(materials, dtype=wp.float32), 3, device="cpu"), - wp.from_torch(indices, dtype=wp.int32), - ) - - # Perform simulation - sim.step() - object_collection.update(sim.cfg.dt) - - # Get material properties - materials_to_check = object_collection.root_view.get_material_properties() - - # Check if material properties are set correctly - torch.testing.assert_close( - wp.to_torch(object_collection.reshape_view_to_data_3d(materials_to_check, 3, device="cpu")), materials - ) - - -@pytest.mark.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [True, False]) -def test_gravity_vec_w(sim, num_envs, num_cubes, device, gravity_enabled): - """Test that gravity vector direction is set correctly for the rigid object.""" - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - - # Obtain gravity direction - gravity_dir = (0.0, 0.0, -1.0) if gravity_enabled else (0.0, 0.0, 0.0) - - sim.reset() - - # Check if gravity vector is set correctly - gravity_vec = object_collection.data.GRAVITY_VEC_W.torch - assert gravity_vec[0, 0, 0] == gravity_dir[0] - assert gravity_vec[0, 0, 1] == gravity_dir[1] - assert gravity_vec[0, 0, 2] == gravity_dir[2] - - # Perform simulation - 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.parametrize("num_envs", [1, 3]) -@pytest.mark.parametrize("num_cubes", [1, 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]) -def test_write_object_state_functions_data_consistency( - sim, num_envs, num_cubes, device, with_offset, state_location, gravity_enabled -): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - # 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) - view_ids = torch.tensor([x for x in range(num_cubes * num_envs)], dtype=torch.int32) - 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) - - 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) - ) - - com = wp.to_torch(cube_object.reshape_view_to_data_2d(cube_object.root_view.get_coms().view(wp.transformf))) - com[..., :3] = offset.to("cpu") - cube_object.root_view.set_coms( - cube_object.reshape_data_to_view_2d(wp.from_torch(com.clone(), dtype=wp.transformf)).view(wp.float32), - wp.from_torch(view_ids, dtype=wp.int32), - ) - - # check center of mass has been set - torch.testing.assert_close( - wp.to_torch(cube_object.reshape_view_to_data_2d(cube_object.root_view.get_coms().view(wp.transformf))), com - ) - - 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 - ) +def test_rigid_object_collection_real_physx_seams() -> None: + """Prove body-major view remapping through nontrivial partial state and inertial writes.""" + with build_simulation_context(device="cpu", gravity_enabled=False) as sim: + collection = _spawn_collection() + sim.reset() - 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:]) + assert collection.is_initialized + assert collection.num_instances == 2 + assert collection.body_names == ["left", "right"] + assert collection.data.body_mass.torch.shape == (2, 2) + assert collection.data.body_com_pose_b.torch.shape == (2, 2, 7) + assert collection.data.body_inertia.torch.shape == (2, 2, 9) + + env_ids = torch.tensor([1, 0], dtype=torch.int32) + body_ids = torch.tensor([1], dtype=torch.int32) + initial_pose = collection.data.body_link_pose_w.torch.clone() + target_pose = initial_pose[env_ids][:, body_ids].clone() + target_pose[0, 0, :3] += torch.tensor([0.2, 0.3, 0.4]) + target_pose[1, 0, :3] += torch.tensor([-0.1, -0.2, 0.1]) + collection.write_body_link_pose_to_sim_index(body_poses=target_pose, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(collection.data.body_link_pose_w.torch[env_ids][:, body_ids], target_pose) + torch.testing.assert_close(collection.data.body_link_pose_w.torch[:, :1], initial_pose[:, :1]) + + initial_masses = collection.data.body_mass.torch.clone() + initial_coms = collection.data.body_com_pose_b.torch.clone() + initial_inertias = collection.data.body_inertia.torch.clone() + masses = torch.tensor([[5.0], [7.0]]) + collection.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(collection.data.body_mass.torch[env_ids][:, body_ids], masses) + raw_mass = wp.to_torch(collection.root_view.get_masses()).reshape(2, 2).T + torch.testing.assert_close(raw_mass[env_ids][:, body_ids], masses) + torch.testing.assert_close(collection.data.body_mass.torch[:, :1], initial_masses[:, :1]) + torch.testing.assert_close(raw_mass[:, :1], initial_masses[:, :1]) + + coms = collection.data.body_com_pose_b.torch[env_ids][:, body_ids].clone() + coms[0, 0, :3] = torch.tensor([0.02, 0.03, 0.04]) + coms[1, 0, :3] = torch.tensor([-0.01, 0.01, 0.02]) + collection.set_coms_index(coms=coms, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(collection.data.body_com_pose_b.torch[env_ids][:, body_ids], coms) + raw_coms = wp.to_torch(collection.root_view.get_coms().view(wp.float32)).reshape(2, 2, 7).transpose(0, 1) + torch.testing.assert_close(raw_coms[env_ids][:, body_ids], coms) + torch.testing.assert_close(collection.data.body_com_pose_b.torch[:, :1], initial_coms[:, :1]) + torch.testing.assert_close(raw_coms[:, :1], initial_coms[:, :1]) + + inertias = collection.data.body_inertia.torch[env_ids][:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[1, 0, 4] *= 1.3 + collection.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) + torch.testing.assert_close(collection.data.body_inertia.torch[env_ids][:, body_ids], inertias) + raw_inertias = wp.to_torch(collection.root_view.get_inertias()).reshape(2, 2, 9).transpose(0, 1) + torch.testing.assert_close(raw_inertias[env_ids][:, body_ids], inertias) + torch.testing.assert_close(collection.data.body_inertia.torch[:, :1], initial_inertias[:, :1]) + torch.testing.assert_close(raw_inertias[:, :1], initial_inertias[:, :1]) + + materials = torch.tensor( + [ + [[0.9, 0.4, 0.1], [0.8, 0.3, 0.2]], + [[0.7, 0.2, 0.3], [0.6, 0.1, 0.4]], + ] + ) + view_materials = collection.reshape_data_to_view_3d(wp.from_torch(materials, dtype=wp.float32), 3, device="cpu") + view_ids = wp.array([0, 1, 2, 3], dtype=wp.int32, device="cpu") + collection.root_view.set_material_properties(view_materials, view_ids) + raw_materials = collection.reshape_view_to_data_3d( + collection.root_view.get_material_properties(), 3, device="cpu" + ) + torch.testing.assert_close(wp.to_torch(raw_materials), materials) diff --git a/source/isaaclab_physx/test/assets/test_surface_gripper.py b/source/isaaclab_physx/test/assets/test_surface_gripper.py index e61c7c22a766..536143eae0dd 100644 --- a/source/isaaclab_physx/test/assets/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/test_surface_gripper.py @@ -3,294 +3,110 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none - - -"""Launch Isaac Sim Simulator first.""" - -import os +"""Local real-PhysX integration coverage for surface grippers.""" from isaaclab.app import AppLauncher -# launch omniverse app -simulation_app = AppLauncher(headless=True).app - -"""Rest everything follows.""" +simulation_app = AppLauncher(headless=True, device="cpu").app import pytest import torch import warp as wp from isaaclab_physx.assets import SurfaceGripper, SurfaceGripperCfg -import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets import ( - Articulation, - ArticulationCfg, - RigidObject, - RigidObjectCfg, -) -from isaaclab.sim import build_simulation_context -from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR -from isaaclab.utils.version import get_isaac_sim_version, has_kit - -# from isaacsim.robot.surface_gripper import GripperView - -_RUNNING_CI = bool( - os.environ.get("CI") == "true" or os.environ.get("GITHUB_ACTIONS") == "true" or os.environ.get("GITLAB_CI") -) - - -def generate_surface_gripper_cfgs( - kinematic_enabled: bool = False, - max_grip_distance: float = 0.1, - coaxial_force_limit: float = 100.0, - shear_force_limit: float = 100.0, - retry_interval: float = 0.1, - reset_xform_op_properties: bool = False, -) -> tuple[SurfaceGripperCfg, ArticulationCfg]: - """Generate a surface gripper cfg and an articulation cfg. - - Args: - max_grip_distance: The maximum grip distance of the surface gripper. - coaxial_force_limit: The coaxial force limit of the surface gripper. - shear_force_limit: The shear force limit of the surface gripper. - retry_interval: The retry interval of the surface gripper. - reset_xform_op_properties: Whether to reset the xform op properties of the surface gripper. - - Returns: - A tuple containing the surface gripper cfg and the articulation cfg. - """ - articulation_cfg = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/SurfaceGripper/test_gripper.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.5), - rot=(0.0, 0.0, 0.0, 1.0), - joint_pos={ - ".*": 0.0, - }, - ), - actuators={ - "dummy": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=0.0, - damping=0.0, - ), - }, - ) - - surface_gripper_cfg = SurfaceGripperCfg( - max_grip_distance=max_grip_distance, - coaxial_force_limit=coaxial_force_limit, - shear_force_limit=shear_force_limit, - retry_interval=retry_interval, - ) - - return surface_gripper_cfg, articulation_cfg - - -def generate_surface_gripper( - surface_gripper_cfg: SurfaceGripperCfg, - articulation_cfg: ArticulationCfg, - num_surface_grippers: int, - device: str, -) -> tuple[SurfaceGripper, Articulation, torch.Tensor]: - """Generate a surface gripper and an articulation. - - Args: - surface_gripper_cfg: The surface gripper cfg. - articulation_cfg: The articulation cfg. - num_surface_grippers: The number of surface grippers to generate. - device: The device to run the test on. - - Returns: - A tuple containing the surface gripper, the articulation, and the translations of the surface grippers. - """ - # Generate translations of 2.5 m in x for each articulation - translations = torch.zeros(num_surface_grippers, 3, device=device) - translations[:, 0] = torch.arange(num_surface_grippers) * 2.5 - - # Create Top-level Xforms, one for each articulation - for i in range(num_surface_grippers): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3]) - articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_[^/]*/Robot")) - surface_gripper_cfg = surface_gripper_cfg.replace(prim_path="/World/Env_[^/]*/Robot/Gripper/SurfaceGripper") - surface_gripper = SurfaceGripper(surface_gripper_cfg) - - return surface_gripper, articulation, translations - - -def generate_grippable_object(sim, num_grippable_objects: int): - object_cfg = RigidObjectCfg( - prim_path="/World/Env_[^/]*/Object", - spawn=sim_utils.CuboidCfg( - size=(1.0, 1.0, 1.0), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)), - ) - grippable_object = RigidObject(object_cfg) +from isaaclab.sim.utils import enable_extension - return grippable_object +enable_extension("isaacsim.robot.surface_gripper") +from usd.schema.isaac import robot_schema -@pytest.fixture -def sim(request): - """Create simulation context with the specified device.""" - device = request.getfixturevalue("device") - if "gravity_enabled" in request.fixturenames: - gravity_enabled = request.getfixturevalue("gravity_enabled") - else: - gravity_enabled = True # default to gravity enabled - if "add_ground_plane" in request.fixturenames: - add_ground_plane = request.getfixturevalue("add_ground_plane") - else: - add_ground_plane = False # default to no ground plane - with build_simulation_context( - device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane - ) as sim: - sim._app_control_on_stop_handle = None - yield sim +from isaacsim.robot.surface_gripper import create_surface_gripper +from pxr import Gf, Sdf, UsdGeom, UsdPhysics +import isaaclab.sim as sim_utils +from isaaclab.sim import build_simulation_context -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci -@pytest.mark.skipif( - _RUNNING_CI, - reason="Isaac Sim SurfaceGripperView initialization can deadlock in CI; keep CUDA fail-fast coverage only.", -) -def test_initialization(sim, num_articulations, device, add_ground_plane) -> None: - """Test initialization for articulation with a surface gripper. - - This test verifies that: - 1. The surface gripper is initialized correctly. - 2. The command and state buffers have the correct shapes. - 3. The command and state are initialized to the correct values. - - Args: - num_articulations: The number of articulations to initialize. - device: The device to run the test on. - add_ground_plane: Whether to add a ground plane to the simulation. - """ - if has_kit() and get_isaac_sim_version().major < 5: - return - surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False) - surface_gripper, articulation, _ = generate_surface_gripper( - surface_gripper_cfg, articulation_cfg, num_articulations, device - ) - - sim.reset() - - assert articulation.is_initialized - assert surface_gripper.is_initialized - - # Check that the command and state buffers have the correct shapes - assert surface_gripper.command.shape == (num_articulations,) - assert surface_gripper.state.shape == (num_articulations,) - - # Check that the command and state are initialized to the correct values - assert wp.to_torch(surface_gripper.command).item() == 0.0 # Idle command after a reset - assert wp.to_torch(surface_gripper.state).item() == -1.0 # Open state after a reset - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - surface_gripper.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci -@pytest.mark.skipif( - _RUNNING_CI, - reason="Isaac Sim SurfaceGripperView initialization can deadlock in CI; keep CUDA fail-fast coverage only.", -) -def test_close_and_open_command(sim, num_articulations, device, add_ground_plane) -> None: - """Test that the close/open commands actually drive the surface gripper status. - - This is a regression test for the command plumbing: a single ``close`` command must move the - gripper out of the *open* state into a *closing* (or *closed*) state, and a subsequent ``open`` - command must bring it back to the *open* state. - - .. note:: - The shared ``test_gripper.usd`` does not contain a separate grippable rigid body (the cube - at the attachment point is a link of the gripper articulation), so the gripper cannot latch - onto anything and will not reach the *closed* state. We therefore only assert that the - command takes effect (the status leaves *open*), which is the deterministic behavior. - - Args: - num_articulations: The number of articulations to initialize. - device: The device to run the test on. - add_ground_plane: Whether to add a ground plane to the simulation. - """ - if has_kit() and get_isaac_sim_version().major < 5: - return - surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False) - surface_gripper, articulation, _ = generate_surface_gripper( - surface_gripper_cfg, articulation_cfg, num_articulations, device +pytestmark = pytest.mark.integration + + +def _create_rigid_cube(path: str, position: tuple[float, float, float]) -> None: + """Author one local rigid collision cube for the gripper attachment.""" + stage = sim_utils.get_current_stage() + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr(0.1) + cube.AddTranslateOp().Set(Gf.Vec3f(*position)) + prim = cube.GetPrim() + UsdPhysics.CollisionAPI.Apply(prim) + UsdPhysics.RigidBodyAPI.Apply(prim) + UsdPhysics.MassAPI.Apply(prim).CreateMassAttr(1.0) + + +def _author_local_surface_gripper() -> SurfaceGripper: + """Build the minimal schema, rigid bodies, and attachment point locally.""" + stage = sim_utils.get_current_stage() + env_path = "/World/Env_0" + UsdGeom.Xform.Define(stage, env_path) + _create_rigid_cube(f"{env_path}/box0", (0.0, 0.0, 0.05)) + _create_rigid_cube(f"{env_path}/box1", (0.0, 0.0, 0.15)) + create_surface_gripper(stage, env_path) + + gripper_prim = stage.GetPrimAtPath(f"{env_path}/SurfaceGripper") + gripper_prim.GetAttribute(robot_schema.Attributes.COAXIAL_FORCE_LIMIT.name).Set(100.0) + gripper_prim.GetAttribute(robot_schema.Attributes.SHEAR_FORCE_LIMIT.name).Set(100.0) + gripper_prim.GetAttribute(robot_schema.Attributes.MAX_GRIP_DISTANCE.name).Set(0.1) + + joint_path = Sdf.Path(f"{env_path}/box1/attachment") + joint = UsdPhysics.Joint.Define(stage, joint_path) + robot_schema.ApplyAttachmentPointAPI(joint.GetPrim()) + joint.GetPrim().CreateAttribute( + robot_schema.Attributes.FORWARD_AXIS.name, robot_schema.Attributes.FORWARD_AXIS.type + ).Set(UsdPhysics.Tokens.x) + joint.GetPrim().CreateAttribute( + robot_schema.Attributes.CLEARANCE_OFFSET.name, robot_schema.Attributes.CLEARANCE_OFFSET.type + ).Set(0.0) + for limit in ["rotX", "rotY", "rotZ", "transX", "transY", "transZ"]: + limit_api = UsdPhysics.LimitAPI.Apply(joint.GetPrim(), limit) + limit_api.CreateHighAttr().Set(-1.0) + limit_api.CreateLowAttr().Set(1.0) + joint.CreateBody0Rel().SetTargets([f"{env_path}/box1"]) + joint.CreateLocalPos0Attr().Set(Gf.Vec3f(0.0, 0.0, -0.0499)) + joint.CreateLocalRot0Attr().Set(Gf.Quatf(0.5, -0.5, 0.5, 0.5)) + gripper_prim.GetRelationship(robot_schema.Relations.ATTACHMENT_POINTS.name).SetTargets([joint_path]) + + return SurfaceGripper( + SurfaceGripperCfg( + prim_path="/World/Env_[^/]*/SurfaceGripper", + max_grip_distance=0.1, + coaxial_force_limit=100.0, + shear_force_limit=100.0, + retry_interval=0.1, + ) ) - sim.reset() - - assert surface_gripper.is_initialized - # after a reset the gripper is open (-1.0) - assert torch.all(wp.to_torch(surface_gripper.state) == -1.0) - # send a single close command (the action term is edge-triggered, so commands are sent once) - close_cmd = wp.array([1.0] * num_articulations, dtype=wp.float32, device=device) - surface_gripper.set_grippers_command_index(close_cmd) - surface_gripper.write_data_to_sim() - # step the simulation so the gripper reacts to the command - for _ in range(3): - sim.step() - articulation.update(sim.cfg.dt) - surface_gripper.update(sim.cfg.dt) - # the close command must take effect: status is "closing" (0.0) or "closed" (1.0), never "open" (-1.0) - state_after_close = wp.to_torch(surface_gripper.state) - assert torch.all(state_after_close >= 0.0), f"close command had no effect, state={state_after_close.tolist()}" - - # send a single open command; the gripper must return to the open state - open_cmd = wp.array([-1.0] * num_articulations, dtype=wp.float32, device=device) - surface_gripper.set_grippers_command_index(open_cmd) - surface_gripper.write_data_to_sim() - for _ in range(3): - sim.step() - articulation.update(sim.cfg.dt) - surface_gripper.update(sim.cfg.dt) - # the open command must take effect: status is back to "open" (-1.0) - state_after_open = wp.to_torch(surface_gripper.state) - assert torch.all(state_after_open == -1.0), f"open command had no effect, state={state_after_open.tolist()}" - - -@pytest.mark.parametrize("device", ["cuda:0"]) -@pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.isaacsim_ci -def test_raise_error_if_not_cpu(sim, device, add_ground_plane) -> None: - """Test that the SurfaceGripper raises an error if the device is not CPU.""" - if has_kit() and get_isaac_sim_version().major < 5: - return - num_articulations = 1 - surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False) - surface_gripper, articulation, translations = generate_surface_gripper( - surface_gripper_cfg, articulation_cfg, num_articulations, device - ) - - with pytest.raises(Exception): +def test_initialization_and_open_close_commands() -> None: + """Initialize the local view and prove close/open commands reach the real plugin.""" + with build_simulation_context(device="cpu", gravity_enabled=False) as sim: + gripper = _author_local_surface_gripper() sim.reset() - -if __name__ == "__main__": - pytest.main([__file__, "-v", "--maxfail=1"]) + assert gripper.is_initialized + assert gripper.command.shape == (1,) + assert gripper.state.shape == (1,) + assert wp.to_torch(gripper.command).item() == 0.0 + assert wp.to_torch(gripper.state).item() == -1.0 + + gripper.set_grippers_command_index(wp.array([1.0], dtype=wp.float32, device="cpu")) + gripper.write_data_to_sim() + for _ in range(3): + sim.step() + gripper.update(sim.cfg.dt) + assert torch.all(wp.to_torch(gripper.state) >= 0.0) + + gripper.set_grippers_command_index(wp.array([-1.0], dtype=wp.float32, device="cpu")) + gripper.write_data_to_sim() + for _ in range(3): + sim.step() + gripper.update(sim.cfg.dt) + assert torch.all(wp.to_torch(gripper.state) == -1.0) diff --git a/source/isaaclab_physx/test/assets/unit/_imports.py b/source/isaaclab_physx/test/assets/unit/_imports.py new file mode 100644 index 000000000000..e037649295c1 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/_imports.py @@ -0,0 +1,80 @@ +# 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 + +"""Kitless import helpers for PhysX asset unit tests.""" + +import importlib +import sys +import warnings +from importlib.machinery import ModuleSpec +from types import ModuleType +from unittest.mock import patch + +_MISSING = object() + + +def import_physx_module(module_name: str, *, simulation_manager: type | None = None): + """Import a PhysX asset module while replacing only its unavailable Kit boundary. + + Args: + module_name: Fully qualified production module name. + simulation_manager: Manager class to expose at the import boundary. A lightweight + placeholder is created when omitted. + """ + module_parts = module_name.split(".") + asset_package_name = module_name.rsplit(".", 1)[0] + asset_subtree_snapshot = { + name: module + for name, module in sys.modules.items() + if name == asset_package_name or name.startswith(f"{asset_package_name}.") + } + ancestor_names = [".".join(module_parts[:index]) for index in range(1, len(module_parts))] + ancestor_snapshots = { + name: (sys.modules[name], dict(sys.modules[name].__dict__)) for name in ancestor_names if name in sys.modules + } + missing_ancestors = [name for name in ancestor_names if name not in sys.modules] + for name in asset_subtree_snapshot: + sys.modules.pop(name, None) + cloner = ModuleType("isaaclab_physx.cloner") + cloner.queue_physx_replication = lambda cfg: None + physics = ModuleType("isaaclab_physx.physics") + physics.PhysxManager = simulation_manager or type("PhysxManager", (), {}) + stubs = {"isaaclab_physx.cloner": cloner, "isaaclab_physx.physics": physics} + import omni + + omni_physics = ModuleType("omni.physics") + omni_physics.__path__ = [] + omni_physics.__spec__ = ModuleSpec("omni.physics", loader=None, is_package=True) + omni_tensors = ModuleType("omni.physics.tensors") + omni_tensors.__spec__ = ModuleSpec("omni.physics.tensors", loader=None) + omni_physics.tensors = omni_tensors + stubs.update({"omni.physics": omni_physics, "omni.physics.tensors": omni_tensors}) + stub_snapshots = {name: sys.modules.get(name, _MISSING) for name in stubs} + with warnings.catch_warnings(): + warnings.simplefilter("ignore", DeprecationWarning) + try: + with patch.object(omni, "physics", omni_physics, create=True): + sys.modules.update(stubs) + imported = importlib.import_module(module_name) + finally: + current_asset_modules = [ + name + for name in sys.modules + if name == asset_package_name or name.startswith(f"{asset_package_name}.") + ] + for name in current_asset_modules: + sys.modules.pop(name, None) + sys.modules.update(asset_subtree_snapshot) + for name, previous_module in stub_snapshots.items(): + if previous_module is _MISSING: + sys.modules.pop(name, None) + else: + sys.modules[name] = previous_module + for name in reversed(missing_ancestors): + sys.modules.pop(name, None) + for module, namespace in ancestor_snapshots.values(): + module.__dict__.clear() + module.__dict__.update(namespace) + return imported diff --git a/source/isaaclab_physx/test/assets/unit/test_physx_actuator_control.py b/source/isaaclab_physx/test/assets/unit/test_physx_actuator_control.py new file mode 100644 index 000000000000..6082c734cd96 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_physx_actuator_control.py @@ -0,0 +1,156 @@ +# 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 + +"""Focused PhysX dual actuator-dispatch unit tests.""" + +from types import SimpleNamespace +from unittest.mock import Mock, call + +import pytest +import warp as wp +from isaaclab_physx.assets.articulation.actuator_control import PhysxActuatorControl + +from isaaclab.actuators import IdealPDActuatorCfg + +pytestmark = pytest.mark.unit + + +class _RecordingView: + def __init__(self) -> None: + self.effort = None + self.position = None + self.velocity = None + + def set_dof_actuation_forces(self, values, indices) -> None: + self.effort = values + + def set_dof_position_targets(self, values, indices) -> None: + self.position = values + + def set_dof_velocity_targets(self, values, indices) -> None: + self.velocity = values + + +def _buffer(value: float) -> wp.array: + return wp.full((1, 2), value, dtype=wp.float32, device="cpu") + + +def test_submit_commands_uses_processed_lab_buffers_for_standard_path() -> None: + """The standard path must submit processed effort and implicit drive targets.""" + view = _RecordingView() + articulation = SimpleNamespace( + _has_newton_actuators=False, + _has_implicit_actuators=True, + data=SimpleNamespace(has_joint_ordering=False), + root_view=view, + _ALL_INDICES=wp.array([0], dtype=wp.int32, device="cpu"), + ) + collection = SimpleNamespace( + _joint_effort_target_sim=_buffer(1.0), + _joint_pos_target_sim=_buffer(2.0), + _joint_vel_target_sim=_buffer(3.0), + ) + control = object.__new__(PhysxActuatorControl) + control._articulation = articulation + + control.submit_commands(collection) + + assert view.effort is collection._joint_effort_target_sim + assert view.position is collection._joint_pos_target_sim + assert view.velocity is collection._joint_vel_target_sim + + +def test_submit_commands_uses_native_effort_and_public_targets_for_newton_path() -> None: + """The Newton path must submit wrapper effort while retaining public position and velocity targets.""" + view = _RecordingView() + native_effort = _buffer(4.0) + articulation = SimpleNamespace( + _has_newton_actuators=True, + _has_implicit_actuators=True, + _physx_actuator_wrapper=SimpleNamespace(joint_f_2d=native_effort), + data=SimpleNamespace(has_joint_ordering=False), + root_view=view, + _ALL_INDICES=wp.array([0], dtype=wp.int32, device="cpu"), + ) + collection = SimpleNamespace( + _joint_pos_target=_buffer(5.0), + _joint_vel_target=_buffer(6.0), + ) + control = object.__new__(PhysxActuatorControl) + control._articulation = articulation + + control.submit_commands(collection) + + assert view.effort is native_effort + assert view.position is collection._joint_pos_target + assert view.velocity is collection._joint_vel_target + + +def test_compute_native_actuators_refreshes_nonidentity_public_joint_state() -> None: + """The native controller must observe the latest reordered PhysX joint state.""" + ordered_calls = Mock() + refresh_position = ordered_calls.refresh_position + refresh_velocity = ordered_calls.refresh_velocity + runtime = SimpleNamespace(compute=ordered_calls.compute) + articulation = SimpleNamespace( + data=SimpleNamespace(has_joint_ordering=True), + _data=SimpleNamespace(_refresh_joint_pos=refresh_position, _refresh_joint_vel=refresh_velocity), + ) + collection = SimpleNamespace() + control = object.__new__(PhysxActuatorControl) + control._articulation = articulation + control._native_actuator_path_active = True + control._actuator_runtime = runtime + + assert control.compute_native_actuators(collection, 0.01) + + assert ordered_calls.mock_calls == [ + call.refresh_position(), + call.refresh_velocity(), + call.compute(collection, 0.01), + ] + + +def test_prepare_native_actuators_does_not_overwrite_solver_gains(monkeypatch) -> None: + """Native preparation must leave solver gains for collection construction to resolve.""" + from isaaclab_physx.assets.articulation import actuator_control + + gain_writes = [] + articulation = SimpleNamespace( + _sim_cfg=SimpleNamespace(use_newton_actuators=True), + cfg=SimpleNamespace(prim_path="/World/Robot"), + joint_names=["joint"], + num_instances=1, + num_joints=1, + device="cpu", + write_joint_stiffness_to_sim_index=lambda **_: gain_writes.append("stiffness"), + write_joint_damping_to_sim_index=lambda **_: gain_writes.append("damping"), + ) + wrapper = SimpleNamespace() + adapter = SimpleNamespace() + + class _Runtime: + def __init__(self, owner, *, logger): + self.wrapper = wrapper + self.adapter = adapter + + def prepare(self, collection, **kwargs) -> None: + pass + + monkeypatch.setattr(actuator_control, "_validate_newton_native_actuator_cfgs", lambda cfgs: None) + monkeypatch.setattr(actuator_control, "find_first_matching_prim", lambda path: None) + monkeypatch.setattr(actuator_control, "get_current_stage", lambda: None) + monkeypatch.setattr(actuator_control, "PhysxActuatorRuntime", _Runtime) + control = PhysxActuatorControl(articulation) + + native_groups = control.prepare_native_actuators( + SimpleNamespace(), + {"explicit": IdealPDActuatorCfg(joint_names_expr=["joint"], stiffness=None, damping=None)}, + ) + + assert native_groups == {"explicit"} + assert articulation._physx_actuator_wrapper is wrapper + assert articulation.newton_actuator_adapter is adapter + assert gain_writes == [] diff --git a/source/isaaclab_physx/test/assets/test_articulation_kernels.py b/source/isaaclab_physx/test/assets/unit/test_physx_articulation.py similarity index 58% rename from source/isaaclab_physx/test/assets/test_articulation_kernels.py rename to source/isaaclab_physx/test/assets/unit/test_physx_articulation.py index 93d2368b68ba..b0e8c3618761 100644 --- a/source/isaaclab_physx/test/assets/test_articulation_kernels.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_articulation.py @@ -3,21 +3,24 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for PhysX articulation Warp kernels.""" +"""Focused PhysX articulation staging, cache, friction, and kernel unit tests.""" -import sys -import warnings -from types import ModuleType, SimpleNamespace +from types import SimpleNamespace from unittest.mock import patch import numpy as np import pytest import warp as wp +from _imports import import_physx_module from isaaclab_physx.assets.articulation.kernels import ( + extract_friction_properties, + write_joint_friction_data_to_buffer, write_joint_state_data, write_joint_state_data_kernel, ) +pytestmark = pytest.mark.unit + def _selector(values: list[int], dtype: type) -> wp.array: """Create a CPU Warp selector with the requested integer width.""" @@ -26,32 +29,7 @@ def _selector(values: list[int], dtype: type) -> wp.array: def _articulation_class(): """Import PhysX Articulation while suppressing only unavailable lazy Kit exports.""" - cloner = ModuleType("isaaclab_physx.cloner") - cloner.queue_physx_replication = lambda cfg: None - physics = ModuleType("isaaclab_physx.physics") - physics.PhysxManager = type("PhysxManager", (), {}) - with warnings.catch_warnings(): - warnings.filterwarnings( - "ignore", - message=( - "^'RigidBodyMaterialCfg' is deprecated and will be removed in 5[.]0[.] Use " - "'isaaclab_physx[.]sim[.]spawners[.]materials[.]PhysxRigidBodyMaterialCfg' for PhysX properties, " - "or 'isaaclab[.]sim[.]spawners[.]materials[.]RigidBodyMaterialBaseCfg' for solver-common properties " - "only[.]$" - ), - category=DeprecationWarning, - ) - warnings.filterwarnings( - "ignore", - message=( - "^`torch[.]jit[.]script` is deprecated[.] Please switch to `torch[.]compile` or `torch[.]export`[.]$" - ), - category=DeprecationWarning, - ) - with patch.dict(sys.modules, {"isaaclab_physx.cloner": cloner, "isaaclab_physx.physics": physics}): - from isaaclab_physx.assets.articulation.articulation import Articulation - - return Articulation + return import_physx_module("isaaclab_physx.assets.articulation.articulation").Articulation def _model_writer_articulation(Articulation) -> tuple[SimpleNamespace, SimpleNamespace]: @@ -158,3 +136,91 @@ def test_write_joint_state_data_scatters_nonidentity_selectors(env_dtype: type, np.testing.assert_array_equal(joint_vel.numpy(), expected_velocity) np.testing.assert_array_equal(prev_joint_vel.numpy(), expected_velocity) np.testing.assert_array_equal(joint_acc.numpy(), expected_acceleration) + + +def test_friction_mapping_keeps_static_dynamic_and_viscous_components_distinct() -> None: + """The PhysX friction tuple must map to the three public coefficient buffers without aliasing.""" + source = wp.array( + np.asarray([[[0.1, 0.2, 0.3], [1.1, 1.2, 1.3]]], dtype=np.float32), + device="cpu", + ) + static = wp.empty((1, 2), dtype=wp.float32, device="cpu") + dynamic = wp.empty_like(static) + viscous = wp.empty_like(static) + + wp.launch( + extract_friction_properties, + dim=(1, 2), + inputs=[source], + outputs=[static, dynamic, viscous], + device="cpu", + ) + + np.testing.assert_array_equal(static.numpy(), np.asarray([[0.1, 1.1]], dtype=np.float32)) + np.testing.assert_array_equal(dynamic.numpy(), np.asarray([[0.2, 1.2]], dtype=np.float32)) + np.testing.assert_array_equal(viscous.numpy(), np.asarray([[0.3, 1.3]], dtype=np.float32)) + + +def test_friction_writer_preserves_components_and_nonidentity_selection() -> None: + """Write all three coefficients to their literal TensorAPI slots for selected joints only.""" + static_in = wp.array(np.asarray([[0.1, 0.2], [1.1, 1.2]], dtype=np.float32), device="cpu") + dynamic_in = wp.array(np.asarray([[0.3, 0.4], [1.3, 1.4]], dtype=np.float32), device="cpu") + viscous_in = wp.array(np.asarray([[0.5, 0.6], [1.5, 1.6]], dtype=np.float32), device="cpu") + env_ids = _selector([1, 0], wp.int32) + joint_ids = _selector([2, 0], wp.int32) + static = wp.full((2, 3), -1.0, dtype=wp.float32, device="cpu") + dynamic = wp.full_like(static, -1.0) + viscous = wp.full_like(static, -1.0) + properties = wp.full((2, 3, 3), -1.0, dtype=wp.float32, device="cpu") + sim_env_ids = wp.full((2,), -1, dtype=wp.int32, device="cpu") + + wp.launch( + write_joint_friction_data_to_buffer, + dim=(2, 2), + inputs=[static_in, dynamic_in, viscous_in, env_ids, joint_ids, False], + outputs=[static, dynamic, viscous, properties, sim_env_ids], + device="cpu", + ) + + expected_properties = np.full((2, 3, 3), -1.0, dtype=np.float32) + expected_properties[1, 2] = [0.1, 0.3, 0.5] + expected_properties[1, 0] = [0.2, 0.4, 0.6] + expected_properties[0, 2] = [1.1, 1.3, 1.5] + expected_properties[0, 0] = [1.2, 1.4, 1.6] + np.testing.assert_array_equal(properties.numpy(), expected_properties) + np.testing.assert_array_equal(sim_env_ids.numpy(), [1, 0]) + + +def test_int64_sim_selector_is_narrowed_on_the_articulation_device() -> None: + """PhysX TensorAPI selectors must be int32 even when the public selector is int64.""" + Articulation = _articulation_class() + articulation = object.__new__(Articulation) + articulation._device = "cpu" + env_ids = wp.array([2, 0], dtype=wp.int64, device="cpu") + + result = articulation._get_sim_env_ids(env_ids) + + assert result.dtype == wp.int32 + np.testing.assert_array_equal(result.numpy(), [2, 0]) + + +def test_joint_property_3d_buffer_is_reordered_from_backend_to_public_order() -> None: + """A backend 3-D joint buffer must expose the configured public joint order exactly once.""" + Articulation = _articulation_class() + articulation = object.__new__(Articulation) + articulation._device = "cpu" + articulation._root_view = SimpleNamespace(count=1, shared_metatype=SimpleNamespace(dof_count=3)) + articulation._data = SimpleNamespace( + has_joint_ordering=True, + joint_ordering=SimpleNamespace(user_to_backend=wp.array([2, 0, 1], dtype=wp.int32, device="cpu")), + ) + backend = wp.array( + np.asarray([[[20.0, 21.0], [30.0, 31.0], [40.0, 41.0]]], dtype=np.float32), + device="cpu", + ) + public = wp.zeros_like(backend) + + result = articulation._get_user_ordered_joint_3d_buffer(backend, public, 2) + + assert result is public + np.testing.assert_array_equal(public.numpy(), [[[40.0, 41.0], [20.0, 21.0], [30.0, 31.0]]]) diff --git a/source/isaaclab_physx/test/assets/unit/test_physx_deformable_object.py b/source/isaaclab_physx/test/assets/unit/test_physx_deformable_object.py new file mode 100644 index 000000000000..053a9947a7fe --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_physx_deformable_object.py @@ -0,0 +1,107 @@ +# 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 + +"""Focused PhysX deformable type, material, and kinematic-target unit tests.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import warp as wp +from _imports import import_physx_module +from isaaclab_physx.assets.deformable_object.kernels import ( + compute_mean_vec3f_over_vertices, + set_kinematic_flags_to_one, + write_nodal_vec3f_to_buffer, +) + +pytestmark = pytest.mark.unit + + +def _module(): + return import_physx_module("isaaclab_physx.assets.deformable_object.deformable_object") + + +@pytest.mark.parametrize( + ("material_schemas", "has_tetmesh", "has_mesh", "expected"), + [ + (("PhysxSurfaceDeformableMaterialAPI",), True, False, "surface"), + (("PhysxDeformableMaterialAPI",), False, True, "volume"), + ((), True, False, "volume"), + ((), False, True, "surface"), + ((), False, False, None), + ], +) +def test_deformable_type_prefers_material_schema_then_falls_back_to_topology( + material_schemas: tuple[str, ...], has_tetmesh: bool, has_mesh: bool, expected: str | None +) -> None: + """Material schemas must win, while unbound materials fall back to mesh topology.""" + infer = _module()._infer_deformable_type + + assert infer(material_schemas, has_tetmesh=has_tetmesh, has_mesh=has_mesh) == expected + + +def test_surface_deformable_rejects_kinematic_targets_before_touching_view() -> None: + """Unsupported surface targets must fail before accessing volume-only buffers or TensorAPI views.""" + deformable = object.__new__(_module().DeformableObject) + deformable._deformable_type = "surface" + deformable._root_physx_view = SimpleNamespace() + + with pytest.raises(ValueError, match="volume deformable"): + deformable.write_nodal_kinematic_target_to_sim_index(wp.zeros((1, 1), dtype=wp.vec4f, device="cpu")) + + +def test_deformable_tensor_api_float_view_is_cached_over_stable_storage() -> None: + """Repeated nodal writes must reuse the float wrapper over the stable vector buffer.""" + deformable = object.__new__(_module().DeformableObject) + positions = wp.zeros((2, 3), dtype=wp.vec3f, device="cpu") + deformable._data = SimpleNamespace(_nodal_pos_w=SimpleNamespace(data=positions)) + deformable._nodal_pos_w_f32 = None + + first = deformable._get_nodal_pos_w_f32() + second = deformable._get_nodal_pos_w_f32() + + assert first is second + assert first.ptr == positions.ptr + assert first.shape == (2, 3, 3) + + +def test_nodal_writer_scatter_preserves_unselected_environment() -> None: + """A compact nodal write must update only the literal selected environment and vertex values.""" + source = wp.array([[(1.0, 2.0, 3.0), (4.0, 5.0, 6.0)]], dtype=wp.vec3f, device="cpu") + env_ids = wp.array([1], dtype=wp.int32, device="cpu") + destination = wp.full((2, 2), value=-1.0, dtype=wp.vec3f, device="cpu") + + wp.launch( + write_nodal_vec3f_to_buffer, + dim=(1, 2), + inputs=[source, env_ids, False], + outputs=[destination], + device="cpu", + ) + + np.testing.assert_array_equal( + destination.numpy(), + np.asarray([[[-1.0, -1.0, -1.0], [-1.0, -1.0, -1.0]], [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]]), + ) + + +def test_deformable_reduction_and_kinematic_flag_kernels_use_literal_layout() -> None: + """Tiny kernels must reduce vector vertices and set only the volume free-node flag.""" + vertices = wp.array([[(1.0, 2.0, 3.0), (3.0, 4.0, 5.0)]], dtype=wp.vec3f, device="cpu") + mean = wp.zeros(1, dtype=wp.vec3f, device="cpu") + targets = wp.zeros(2, dtype=wp.vec4f, device="cpu") + + wp.launch( + compute_mean_vec3f_over_vertices, + dim=1, + inputs=[vertices, 2], + outputs=[mean], + device="cpu", + ) + wp.launch(set_kinematic_flags_to_one, dim=2, inputs=[targets], device="cpu") + + np.testing.assert_array_equal(mean.numpy(), [[2.0, 3.0, 4.0]]) + np.testing.assert_array_equal(targets.numpy(), [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]]) diff --git a/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object.py b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object.py new file mode 100644 index 000000000000..a0fe27280a8f --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object.py @@ -0,0 +1,82 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Focused PhysX rigid-object CPU staging and cached-view unit tests.""" + +import sys +from types import SimpleNamespace + +import numpy as np +import pytest +import torch +import warp as wp +from _imports import import_physx_module + +pytestmark = pytest.mark.unit + + +def _rigid_object_class(): + return import_physx_module("isaaclab_physx.assets.rigid_object.rigid_object").RigidObject + + +def test_partial_int64_environment_selector_is_narrowed_and_staged_on_cpu() -> None: + """PhysX CPU property writers must receive int32 indices even from public int64 selectors.""" + rigid_object = object.__new__(_rigid_object_class()) + rigid_object._device = "cuda:0" + rigid_object._ALL_INDICES = wp.array([0, 1, 2], dtype=wp.int32, device="cuda:0") + rigid_object._cpu_env_ids_all = wp.array([0, 1, 2], dtype=wp.int32, device="cpu") + rigid_object._cpu_env_ids = wp.empty(3, dtype=wp.int32, device="cpu", pinned=True) + rigid_object._cpu_env_ids_views = {} + + result = rigid_object._get_cpu_env_ids(torch.tensor([2, 0], dtype=torch.int64, device="cuda:0")) + + assert result.dtype == wp.int32 + assert str(result.device) == "cpu" + np.testing.assert_array_equal(result.numpy(), [2, 0]) + + +def test_tensor_api_float_view_is_cached_over_stable_pose_storage() -> None: + """Repeated root-pose writes must reuse the wrapper over the stable data buffer.""" + rigid_object = object.__new__(_rigid_object_class()) + pose = wp.zeros(2, dtype=wp.transformf, device="cpu") + rigid_object._data = SimpleNamespace(_root_link_pose_w=SimpleNamespace(data=pose)) + rigid_object._root_link_pose_w_f32 = None + + first = rigid_object._get_root_link_pose_w_f32() + second = rigid_object._get_root_link_pose_w_f32() + + assert first is second + assert first.ptr == pose.ptr + assert first.shape == (2, 7) + + +def test_kitless_import_is_evicted_before_fresh_manager_import() -> None: + """A stub-bound unit import must not leak into the next real PhysX module import.""" + module_name = "isaaclab_physx.assets.rigid_object.rigid_object" + package_name = module_name.rsplit(".", 1)[0] + subtree_before = { + name: module + for name, module in sys.modules.items() + if name == package_name or name.startswith(f"{package_name}.") + } + stubbed_module = import_physx_module(module_name) + subtree_after = { + name: module + for name, module in sys.modules.items() + if name == package_name or name.startswith(f"{package_name}.") + } + assert subtree_after == subtree_before + + try: + from isaaclab_physx.physics import PhysxManager + except ModuleNotFoundError as exc: + assert exc.name == "carb" + PhysxManager = type("FreshPhysxManager", (), {}) + + fresh_module = import_physx_module(module_name, simulation_manager=PhysxManager) + + assert fresh_module is not stubbed_module + assert fresh_module.SimulationManager is PhysxManager + assert fresh_module.RigidObjectData.__init__.__globals__["SimulationManager"] is PhysxManager diff --git a/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object_collection.py b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object_collection.py new file mode 100644 index 000000000000..a2e4d88b50e6 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object_collection.py @@ -0,0 +1,100 @@ +# 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 + +"""Focused PhysX rigid-object-collection ordering and selector unit tests.""" + +import numpy as np +import pytest +import torch +import warp as wp +from _imports import import_physx_module +from isaaclab_physx.assets.rigid_object_collection.kernels import resolve_view_ids, resolve_view_ids_kernel + +pytestmark = pytest.mark.unit + + +def test_view_id_kernel_maps_instance_body_grid_to_physx_body_major_order() -> None: + """Nontrivial selectors must produce literal PhysX body-major flat indices.""" + env_ids = wp.array([2, 0], dtype=wp.int64, device="cpu") + body_ids = wp.array([1, 0], dtype=wp.int32, device="cpu") + out = wp.empty(4, dtype=wp.int32, device="cpu") + + wp.launch( + resolve_view_ids_kernel(env_ids, body_ids), + dim=(2, 2), + inputs=[env_ids, body_ids, 2, 3], + outputs=[out], + device="cpu", + ) + + np.testing.assert_array_equal(out.numpy(), [5, 3, 2, 0]) + + +def test_view_id_kernel_supports_canonical_int32_selectors() -> None: + """Canonical selectors must use the supported static kernel with the same ordering.""" + env_ids = wp.array([1, 0], dtype=wp.int32, device="cpu") + body_ids = wp.array([2], dtype=wp.int32, device="cpu") + out = wp.empty(2, dtype=wp.int32, device="cpu") + + wp.launch(resolve_view_ids, dim=(2, 1), inputs=[env_ids, body_ids, 2, 3], outputs=[out], device="cpu") + + np.testing.assert_array_equal(out.numpy(), [7, 6]) + + +def test_collection_data_reshapes_physx_body_major_rows_to_instance_major() -> None: + """PhysX body-major property rows must become public (instance, body, component) order.""" + module = import_physx_module("isaaclab_physx.assets.rigid_object_collection.rigid_object_collection_data") + data = object.__new__(module.RigidObjectCollectionData) + data.num_instances = 3 + data.num_bodies = 2 + data.device = "cpu" + raw = wp.array( + np.asarray( + [[0.0, 1.0], [10.0, 11.0], [20.0, 21.0], [100.0, 101.0], [110.0, 111.0], [120.0, 121.0]], + dtype=np.float32, + ), + device="cpu", + ) + + result = data._reshape_view_to_data_3d(raw, 2) + + np.testing.assert_array_equal( + result.numpy(), + np.asarray( + [ + [[0.0, 1.0], [100.0, 101.0]], + [[10.0, 11.0], [110.0, 111.0]], + [[20.0, 21.0], [120.0, 121.0]], + ], + dtype=np.float32, + ), + ) + + +def test_collection_view_id_conversion_stages_partial_cuda_query_on_cpu(monkeypatch) -> None: + """A partial CUDA selector must return synchronized CPU indices accepted by the TensorAPI.""" + module = import_physx_module("isaaclab_physx.assets.rigid_object_collection.rigid_object_collection") + collection = object.__new__(module.RigidObjectCollection) + collection._device = "cuda:0" + collection._root_view = type("View", (), {"count": 6})() + collection._body_names_list = ["left", "right"] + collection._ALL_ENV_INDICES = wp.array([0, 1, 2], dtype=wp.int32, device="cuda:0") + collection._ALL_BODY_INDICES = wp.array([0, 1], dtype=wp.int32, device="cuda:0") + collection._ALL_VIEW_INDICES = wp.array([0, 1, 2, 3, 4, 5], dtype=wp.int32, device="cuda:0") + collection._cpu_all_view_ids = wp.array([0, 1, 2, 3, 4, 5], dtype=wp.int32, device="cpu") + collection._sim_view_ids = wp.empty(6, dtype=wp.int32, device="cuda:0") + collection._cpu_view_ids = wp.empty(6, dtype=wp.int32, device="cpu", pinned=True) + collection._sim_view_ids_views = {} + collection._cpu_view_ids_views = {} + synchronize_calls = [] + monkeypatch.setattr(wp, "synchronize_stream", lambda device: synchronize_calls.append(device)) + + result = collection._env_body_ids_to_view_ids( + torch.tensor([2, 0], device="cuda:0"), torch.tensor([1], device="cuda:0"), device="cpu" + ) + + np.testing.assert_array_equal(result.numpy(), [5, 3]) + assert str(result.device) == "cpu" + assert synchronize_calls == ["cuda:0"] diff --git a/source/isaaclab_physx/test/assets/unit/test_physx_surface_gripper.py b/source/isaaclab_physx/test/assets/unit/test_physx_surface_gripper.py new file mode 100644 index 000000000000..092a7381b9a0 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_physx_surface_gripper.py @@ -0,0 +1,74 @@ +# 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 + +"""Focused PhysX surface-gripper command, property, and device-guard unit tests.""" + +from types import SimpleNamespace +from unittest.mock import Mock + +import numpy as np +import pytest +import warp as wp +from isaaclab_physx.assets.surface_gripper.surface_gripper import SurfaceGripper + +pytestmark = pytest.mark.unit + + +def _gripper() -> SurfaceGripper: + gripper = object.__new__(SurfaceGripper) + gripper._device = "cpu" + gripper._num_envs = 3 + gripper._ALL_INDICES = wp.array([0, 1, 2], dtype=wp.int32, device="cpu") + gripper._gripper_command = wp.zeros(3, dtype=wp.float32, device="cpu") + gripper._max_grip_distance = wp.zeros(3, dtype=wp.float32, device="cpu") + gripper._coaxial_force_limit = wp.zeros(3, dtype=wp.float32, device="cpu") + gripper._shear_force_limit = wp.zeros(3, dtype=wp.float32, device="cpu") + gripper._retry_interval = wp.zeros(3, dtype=wp.float32, device="cpu") + gripper._gripper_view = SimpleNamespace( + apply_gripper_action=Mock(), + set_surface_gripper_properties=Mock(), + ) + return gripper + + +def test_command_filter_and_partial_property_update_use_literal_view_payloads() -> None: + """Submit only open/close commands and preserve property selector ordering.""" + gripper = _gripper() + gripper.set_grippers_command_index(wp.array([0.5, 0.0, -0.5], dtype=wp.float32, device="cpu")) + + gripper.write_data_to_sim() + + gripper.gripper_view.apply_gripper_action.assert_called_once_with([0.5, 0.0, -0.5], [[0], [2]]) + + env_ids = wp.array([2, 0], dtype=wp.int32, device="cpu") + gripper.update_gripper_properties_index( + max_grip_distance=wp.array([0.2, 0.4], dtype=wp.float32, device="cpu"), + env_ids=env_ids, + ) + + np.testing.assert_array_equal(gripper._max_grip_distance.numpy(), np.asarray([0.4, 0.0, 0.2], dtype=np.float32)) + properties = gripper.gripper_view.set_surface_gripper_properties.call_args.kwargs + np.testing.assert_array_equal(properties.pop("max_grip_distance"), np.asarray([0.4, 0.0, 0.2], dtype=np.float32)) + assert properties == { + "coaxial_force_limit": [0.0, 0.0, 0.0], + "shear_force_limit": [0.0, 0.0, 0.0], + "retry_interval": [0.0, 0.0, 0.0], + "indices": [2, 0], + } + + +def test_initialize_rejects_cuda_with_specific_error() -> None: + """Fail before extension/view creation when the simulation device is not CPU.""" + gripper = object.__new__(SurfaceGripper) + gripper._device = "cuda:0" + + with pytest.raises(Exception) as exc_info: + gripper._initialize_impl() + + assert type(exc_info.value) is Exception + assert str(exc_info.value) == ( + "SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU. Use" + " `--device cpu` to run the simulation on CPU." + )