From 1d54751d1e3ae5cef2bc2ed806082723f415b632 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 11:30:52 +0200 Subject: [PATCH 01/26] Document asset test redesign --- ...-08-21-asset-test-suite-redesign-design.md | 292 ++++++++++++++++++ 1 file changed, 292 insertions(+) create mode 100644 docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md diff --git a/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md b/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md new file mode 100644 index 00000000000..2c7049665c6 --- /dev/null +++ b/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md @@ -0,0 +1,292 @@ + + +# Fast Asset Test Suite Redesign + +**Status:** Approved for implementation + +## Objective + +Make the asset tests in `isaaclab`, `isaaclab_newton`, `isaaclab_physx`, and +`isaaclab_ov` fast and thorough by separating four responsibilities: + +1. shared backend contract conformance; +2. solver-independent unit and kernel behavior; +3. backend-specific unit and kernel behavior; +4. minimal real-solver integration. + +The redesign must preserve meaningful coverage while sharply reducing Kit and +solver startup, scene construction, model construction, redundant parameter +matrices, and duplicate WrenchComposer physics scenarios. Newton tests must not +launch Kit. Cable and MPM assets are excluded from this work. + +## Baseline + +The baseline was collected before implementation in a fresh worktree and local +uv environment: + +- branch: `antoiner/asset-tests-redesign`; +- base: `d7033a5a1a207f1d4284edb60d72d7838984413b` from latest + `origin/develop` on 2026-08-21; +- environment: `env_isaaclab`, created 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, and OVPhysX 0.5.10; +- NVIDIA GeForce RTX 5090, driver 590.48.01, 32607 MiB; +- every test file was launched in its own process through the repository test + orchestrator, matching CI behavior. + +| Scope | Files | Cases | Result | Test time | Wall time | +|---|---:|---:|---:|---:|---:| +| Shared asset and interface tests | 8 | 4,331 | 8/8 files passed | 64.70 s | 84.02 s | +| Newton assets, excluding cable and MPM | 6 | 644 | 6/6 files passed | 590.94 s | 608.44 s | +| PhysX assets | 7 | 486 | 7/7 files passed | 236.80 s | 255.30 s | +| OV assets | 9 | 492 | 9/9 files passed | 196.94 s | 220.05 s | +| WrenchComposer | 3 | 412 | 3/3 files passed | 23.34 s | 29.79 s | +| **Total** | **33** | **6,365** | **33/33 files passed** | **1,112.72 s** | **1,197.60 s** | + +The dominant costs are concentrated rather than evenly distributed: + +| File group | Wall time | +|---|---:| +| Shared articulation interface | 41.53 s | +| Shared articulation ordering interface | 18.73 s | +| Newton articulation | 383.25 s | +| Newton actuators | 87.07 s | +| Newton rigid-object collection | 67.36 s | +| Newton rigid object | 66.88 s | +| PhysX articulation | 103.92 s | +| PhysX rigid-object collection | 56.52 s | +| PhysX rigid object | 43.19 s | +| PhysX actuators | 37.00 s | +| OV articulation | 119.74 s | +| OV rigid-object collection | 41.92 s | +| OV rigid object | 33.34 s | +| WrenchComposer integration and PhysX comparison | 23.47 s | + +The current PhysX deformable module costs 2.43 seconds of process startup but +skips all 12 collected cases. Direct kernel/helper files generally take less +than four seconds wall time and validate useful behavior without real scenes. + +### Reproduction + +Run these commands from the worktree with `OMNI_KIT_ACCEPT_EULA=YES`. Each +command writes its aggregate report under `tests/`. + +```bash +TEST_FILTER_PATTERN=/source/isaaclab/test/assets/ \ +TEST_INCLUDE_FILES=test_articulation_iface.py,test_articulation_ordering.py,test_articulation_ordering_iface.py,test_articulation_ordering_kernels.py,test_asset_selector_cache.py,test_iface_test_utils.py,test_rigid_object_collection_iface.py,test_rigid_object_iface.py \ +TEST_RESULT_FILE=baseline-assets-shared.xml \ +./isaaclab.sh -p -m pytest tools + +TEST_FILTER_PATTERN=/source/isaaclab_newton/test/assets/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_articulation_ordering_kernels.py,test_newton_actuators_newton.py,test_rigid_object.py,test_rigid_object_collection.py,test_wrench_kernels.py \ +TEST_RESULT_FILE=baseline-assets-newton.xml \ +./isaaclab.sh -p -m pytest tools + +TEST_FILTER_PATTERN=/source/isaaclab_physx/test/assets/ \ +TEST_INCLUDE_FILES=test_articulation.py,test_articulation_kernels.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=baseline-assets-physx.xml \ +./isaaclab.sh -p -m pytest tools + +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_object_helpers.py,test_deformable_views.py,test_rigid_object.py,test_rigid_object_collection.py,test_rigid_object_helpers.py \ +TEST_RESULT_FILE=baseline-assets-ov.xml \ +./isaaclab.sh -p -m pytest tools + +TEST_FILTER_PATTERN=/source/isaaclab/test/utils/ \ +TEST_INCLUDE_FILES=test_wrench_composer.py,test_wrench_composer_integration.py,test_wrench_composer_vs_physx.py \ +TEST_RESULT_FILE=baseline-wrench-composer.xml \ +./isaaclab.sh -p -m pytest tools +``` + +## Test Taxonomy + +### Shared contract tests + +Rename the current `iface` concept to `contract`. The old name describes only +the original abstract-interface check, while the current files mix reflection, +data semantics, writer behavior, caching, ordering, and backend behavior. + +Place reusable contract definitions in `isaaclab` and keep thin, package-owned +backend runners. Split the contract into focused modules: + +- `test_asset_contract_api.py`: concrete implementations of abstract members, + public signatures, lifecycle, metadata, names, finders, and explicit + unsupported behavior; +- `test_asset_contract_data.py`: shapes, dtypes, devices, aliases, defaults, + derived frames, acceleration, timestamps, and cache invalidation; +- `test_asset_contract_writes.py`: root, body, and joint writers; full and + partial updates; index and mask selection; delegation; and invalid shapes. + +The shared contract is the single source of truth. Backend runners provide the +fixture or adapter needed to execute it and declare supported capabilities. A +contract case may be skipped only from an explicit capability declaration, +with a reason. Broad import-error catches and silently empty backend matrices +are not acceptable. + +Add a base-surface meta-test that enumerates the public members of the asset +base classes and requires every member to be classified as tested, explicitly +unsupported, or intentionally out of scope. This prevents new abstract or +public members from creating silent contract holes. + +Mocks are contract-test infrastructure, not pretend backend implementations. +Mock tests verify forwarding, selection, cache, validation, and failure +semantics without a solver, but they do not substitute for the thin real +backend integration checks. + +### Shared unit and kernel tests + +Keep solver-independent behavior in `isaaclab`. Unit tests directly exercise +small tensors and focused collaborators. Kernel tests launch kernels with tiny +inputs and no scene. + +This layer owns: + +- selector normalization and selector-cache behavior; +- name matching, ordering, gather, and scatter; +- frame and quaternion transforms; +- WrenchComposer arithmetic, accumulation, reset, and frame conversion; +- shape and dtype validation; +- common timestamp and lazy-cache behavior. + +The canonical matrix is deliberately small: CPU with `N=2`, `B=3`, and `J=4`; +one singleton or empty case where behavior differs; one nontrivial ordering +case; and one CUDA smoke for code that has a genuinely distinct device path. +Do not form Cartesian products over several environment counts, body counts, +devices, and backends. + +### Backend-specific unit and kernel tests + +Each backend owns tests for unique logic under its own `test/assets/unit/` +directory. These tests use direct tensors, light fakes, or backend objects that +do not require a real scene whenever possible. + +Newton owns model-index mapping, FK invalidation, model notification behavior, +joint staging, actuator adaptation, and Newton-specific wrench kernels. PhysX +owns view-index and ordering conversion, staging, cache behavior, PhysX +inertial/friction mapping, and supported deformable behavior. OV owns its view +adapters, staging, derived data, deformable helpers, device configuration, and +manager lifecycle. Cable and MPM logic are explicitly excluded. + +Kernel tests are first-class unit tests. They should use the smallest input +that covers selector/order/scatter/gather/cache/frame/staging behavior, plus one +CUDA smoke only where the kernel has a real device-specific execution path. + +### Integration tests + +Integration tests prove that the adapters connect to a real solver; they do not +repeat the full contract or arithmetic matrix. Consolidate each backend's +integration cases into as few files as practical because the CI orchestrator +starts a fresh process per file. + +The minimum real-solver set per supported backend is: + +1. initialize a small local scene without Nucleus assets; +2. write and read back a partial root/body/joint state; +3. read mass, center of mass, and inertia; +4. deliver one real external wrench through the backend; +5. exercise one joint drive or actuator path; +6. smoke-test Jacobian or mass-matrix access when supported. + +WrenchComposer math remains in shared unit tests. Keep only one real delivery +case per backend integration suite. Remove the redundant broad comparison +matrix after the retained cases demonstrate both the composer's math and each +backend's delivery glue. + +The wrench case does not replace backend-only model-property integration. +PhysX must retain one real case per rigid object, rigid-object collection, and +articulation family that exercises mass, center-of-mass, or inertia setters. +The collection case must use nontrivial environment/body selection so it also +proves the real view-order remapping. Unit tests cover the complete selection +matrix; integration proves the TensorAPI accepts the translated request. + +Treat deformables as a separate capability family rather than forcing them +through the rigid-asset matrix. PhysX must cover surface-versus-volume +detection, material fallback, volume kinematic targets, and rejection of +unsupported surface kinematic targets through focused unit tests plus the +smallest stable real GPU probes. Replace the currently all-skipped module with +working coverage or remove its empty startup cost; do not count collected but +unconditionally skipped cases as coverage. + +## Scene and Model Reuse + +Cache immutable authored scene descriptions, small local USD fixtures, asset +configuration, and topology. Reuse a live scene within a consolidated test +module only when reset semantics are part of the fixture contract and every +test restores mutable state. + +Do not globally cache live Newton models or OV views merely to avoid setup. +They contain solver-owned mutable state and lifecycle coupling. Prefer cheap +reconstruction from cached authoring or topology. Where a backend exposes an +explicit safe reset, measure reuse and retain it only if isolation tests prove +that order and repetition do not change results. + +Newton integration must run through its kitless path. Remove `AppLauncher`, +Nucleus checks, and remote assets from Newton asset tests. Use local primitive +or generated fixtures and `SimulationContext` directly. + +## OV CPU/GPU Execution + +OVPhysX 0.5.10 documents that CPU and GPU simulation are selected per scene by +`physxScene:enableGPUDynamics` and `physxScene:broadphaseType`. Its explicit +hard CPU-only mode is process-global and sticky, but that mode is intended for +the no-CUDA-touch guarantee rather than for every CPU scene. + +IsaacLab currently calls `PhysX.set_cpu_mode(True)` for a CPU manager and keeps +a process-lifetime `_locked_device`, so the present `device_split` markers are +an IsaacLab restriction rather than an OVPhysX requirement. A local real-scene +probe against OVPhysX 0.5.10 passed both GPU-CPU-GPU and CPU-GPU-CPU sequences +in one process after bypassing only those two IsaacLab restrictions. Each +sequence created a cuboid and rigid object, produced tensors on the requested +device, authored the expected GPU-dynamics value, and advanced consistently +under gravity. The unmodified manager reproduced its own `_locked_device` +failure on the second context. + +Remove the lock and stop mapping an ordinary CPU scene to sticky hard CPU-only +mode. Author CPU scenes with `physxScene:enableGPUDynamics=false` and +`physxScene:broadphaseType="MBP"`, and author GPU scenes with the corresponding +GPU values. Preserve the wheel's explicit environment/configuration path for +hosts that require the no-CUDA-touch guarantee. Add a tracked lifecycle +regression that writes, reads, and advances CPU-GPU-CPU scenes in one process, +plus focused unit tests for the manager call and scene attributes. Then remove +the `device_split` markers and module-level split workarounds. + +## Coverage Gates + +The fast presubmit gate consists of shared contract tests plus shared and +backend-specific unit/kernel tests. The minimal integration gate runs the +consolidated backend files. Broader solver, controller, or long-horizon +behavior belongs to its owning suite or a scheduled job. + +The redesign is complete only when: + +- contract/unit tests run in under 30 seconds per backend after warmup; +- the selected asset and WrenchComposer wall time is at least three times + faster than the 1,197.60-second baseline on the reference machine; +- the base-surface meta-test has no unclassified public members; +- no retained backend file is entirely skipped; +- Newton asset tests import and run without Kit or Nucleus; +- every supported backend passes the minimal real integration set; +- test-order and repeated-run checks prove fixture isolation; +- the before/after report lists cases moved, consolidated, removed as + redundant, or newly added, so speedups are not obtained by hidden coverage + loss. + +## Migration Sequence + +1. Add the contract inventory/meta-test and focused shared contract modules. +2. Move pure shared behavior and WrenchComposer arithmetic into focused unit + and kernel tests. +3. Add package-local backend unit/kernel tests for unique logic. +4. Convert Newton assets to local, kitless integration fixtures. +5. Validate and, if supported, remove the OV process device split while + retaining explicit hard CPU-only behavior. +6. Consolidate minimal backend integration cases and delete only proven + duplicate matrices. +7. Run the exact baseline scopes, publish the before/after coverage mapping, + and add one test-only changelog fragment per touched package. From 735be0f64c5e105a2e1586d3bc73bd79cbe17c97 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 11:32:40 +0200 Subject: [PATCH 02/26] Plan asset test redesign --- .../2026-08-21-asset-test-suite-redesign.md | 220 ++++++++++++++++++ 1 file changed, 220 insertions(+) create mode 100644 docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md diff --git a/docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md b/docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md new file mode 100644 index 00000000000..9b1c0edb4e2 --- /dev/null +++ b/docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md @@ -0,0 +1,220 @@ + + +# Asset Test Suite Redesign Implementation Plan + +> **Spec:** `docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md` + +## Global Constraints + +- Work only in the `antoiner/asset-tests-redesign` worktree based on commit + `d7033a5a1a207f1d4284edb60d72d7838984413b` or its descendants. +- Use the worktree-local `env_isaaclab` through `./isaaclab.sh -p`. +- Apply strict red-green-refactor for production behavior changes. Record the + failing command and expected failure in each task report. +- Do not modify or add cable or MPM tests. +- Newton asset tests must not import `isaaclab.app`, launch Kit, use Nucleus, + or require remote assets. +- Shared contract tests own solver-common behavior. Backend tests own unique + implementation behavior and the smallest real integration proof. +- Keep one real wrench-delivery case per backend and retain real PhysX + property/view-order glue per asset family. +- Ordinary OV CPU scenes use per-scene CPU attributes, not sticky hard + CPU-only mode. Explicit wheel-level hard CPU-only operation remains valid. +- Preserve all supported public behavior; this project changes tests and one + OV manager lifecycle restriction, not asset public APIs. +- New test files use the 2026 SPDX header. Add one `.skip` changelog fragment + per test-only package and a patch fragment for `isaaclab_ov` if manager + behavior changes. +- Run focused tests after each task, `./isaaclab.sh -f` before every commit, + and the baseline scopes at the end. + +## Task 1: Allow OV CPU/GPU reuse in one process + +**Files:** + +- Modify `source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py`. +- Modify `source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py`. +- Add or modify a real lifecycle test under `source/isaaclab_ov/test/physics/`. +- Remove `device_split` only from the three OV asset modules in this project: + `test_articulation.py`, `test_rigid_object.py`, and + `test_rigid_object_collection.py`. + +**Red:** + +1. Add a unit test proving ordinary CPU construction never calls + `PhysX.set_cpu_mode(True)`. +2. Add unit tests proving CPU scenes author `enableGPUDynamics=false` and + `broadphaseType=MBP`, while GPU scenes author the GPU values. +3. Add a real CPU-CUDA-CPU lifecycle regression that creates a local cuboid + rigid object, writes/reads state, advances it, and asserts tensor placement. +4. Verify the unit tests fail due the current sticky call and attributes, and + the real test fails at `_locked_device`. + +**Green:** remove `_locked_device` and its error path, stop calling sticky CPU +mode for ordinary CPU scenes, author both CPU and GPU attributes explicitly, +and update lifecycle documentation. Remove the asset `device_split` markers +only after the real lifecycle regression passes. + +**Verify:** run the manager lifecycle unit file, the new real lifecycle test, +then the three OV asset files in a single unsplit invocation. + +## Task 2: Establish focused shared contract infrastructure + +**Files:** + +- Rename `_iface_test_boot.py` and the three `_..._iface_test_utils.py` helpers + under `source/isaaclab/test/assets/` to contract terminology. +- Replace the five current `*iface*.py` test modules with focused API, data, + and write contract modules under `source/isaaclab/test/assets/contract/`. +- Add a capability declaration and public-surface classification helper in the + same test-only package. + +**Behavior:** preserve existing meaningful assertions while eliminating the + environment/body/joint/device Cartesian matrix. Use CPU `N=2`, `B=3`, `J=4` + as the canonical case, targeted singleton/empty/order cases, and one CUDA + smoke per distinct device path. Make backend availability and unsupported + behavior explicit. + +**Meta-test:** enumerate public members declared by `AssetBase`, +`BaseRigidObject`, `BaseRigidObjectData`, `BaseRigidObjectCollection`, +`BaseRigidObjectCollectionData`, `BaseArticulation`, and +`BaseArticulationData`. Fail with the unclassified names unless each member is +mapped to an API/data/write contract, explicitly unsupported, or documented as +out of scope. + +**Verify:** first demonstrate the meta-test catches one intentionally omitted +member, restore the classification, then run the complete shared contract +directory and ordering/selector unit files. Compare collected cases and wall +time to the 84.02-second shared baseline. + +## Task 3: Make WrenchComposer coverage focused + +**Files:** + +- Refactor `source/isaaclab/test/utils/test_wrench_composer.py`. +- Consolidate the unique real behavior from + `test_wrench_composer_integration.py` and + `test_wrench_composer_vs_physx.py` into one minimal integration module. +- Update backend integration modules as required to retain one real delivery + case per backend. + +**Behavior:** keep arithmetic, accumulation, clearing, frame conversion, +permanent-versus-instantaneous, selection, and validation in direct unit tests. +Use literal expected wrenches on tiny tensors. Keep one rotated force-at-position +real parity case because it covers both force and induced torque. Delete broad +multi-size/device/long-run duplication only after mapping every removed case to +the retained unit or integration assertion. + +**Verify:** run the focused unit and integration modules, record case mapping, +and compare wall time to the 29.79-second WrenchComposer baseline. + +## Task 4: Convert Newton rigid assets to kitless local fixtures + +**Files:** + +- Refactor `source/isaaclab_newton/test/assets/test_rigid_object.py`. +- Refactor `source/isaaclab_newton/test/assets/test_rigid_object_collection.py`. +- Add focused unit modules under `source/isaaclab_newton/test/assets/unit/` for + model-index mapping, staging, cache invalidation, and wrench kernels. + +**Red:** add a subprocess import/collection test that fails if either target +module loads `isaaclab.app`, `isaacsim`, or a Nucleus asset. Add focused unit +tests before extracting unique logic from real scenes. + +**Green:** use `SimulationContext` and local primitive/generated USD authoring, +remove `AppLauncher` and Nucleus imports, reduce matrices to canonical cases, +and consolidate real tests so a scene is authored once per module where reset +is proven safe. + +**Verify:** run the import/collection guard without IsaacSim modules, the unit +directory, and both real files. Compare to the 134.24-second combined baseline. + +## Task 5: Convert Newton articulation and actuator tests to kitless coverage + +**Files:** + +- Refactor `source/isaaclab_newton/test/assets/test_articulation.py`. +- Refactor `source/isaaclab_newton/test/assets/test_newton_actuators_newton.py`. +- Extend Newton package-local unit/kernel modules for FK invalidation, joint + staging, model notifications, ordering, and actuator adaptation. + +**Red:** extend the kitless collection guard to these modules and add focused +tests for every extracted unique behavior. Verify failures before production +or fixture changes. + +**Green:** replace remote robots with the smallest locally authored +articulation fixtures, remove AppLauncher, move actuator calculations that do +not require a model into unit tests, and keep one real actuator equivalence, +one partial root/joint roundtrip, one property read, one wrench delivery, and +supported Jacobian/mass access. + +**Verify:** run Newton unit/kernel tests and the two real modules; compare to +the 470.32-second combined baseline. Run all Newton in-scope asset tests and +prove no Kit process starts. + +## Task 6: Extract unique PhysX unit coverage and trim integration duplication + +**Files:** + +- Add `source/isaaclab_physx/test/assets/unit/` modules for articulation, + rigid object, rigid-object collection, deformable, and actuator helpers. +- Reduce the existing real asset modules without splitting them into more + solver-starting files. + +**Unit behavior:** view index conversion, ordering reshape, partial CPU +staging, writer cache invalidation, friction/inertial mapping, dual actuator +dispatch, deformable surface/volume detection, material fallback, kinematic +target validation, and supported kernels. + +**Integration behavior:** retain one local scene per asset family. Prove state +write/read, one real wrench, model-property setter acceptance, nontrivial +collection ordering, articulation Jacobian/mass access, and the smallest stable +surface/volume deformable probes. Move controller and termination behavior to +their owning suites rather than retaining them as asset integration. + +**Verify:** run new units first, then real files. Confirm the deformable module +is no longer an all-skipped startup. Compare to the 255.30-second baseline. + +## Task 7: Extract unique OV unit coverage and trim integration duplication + +**Files:** + +- Organize the existing OV helper/kernel modules under + `source/isaaclab_ov/test/assets/unit/` and add missing focused coverage for + view adapters, fused layouts, staging, derived data, deformable helpers, and + cache invalidation. +- Reduce the three main real rigid/articulation modules and the deformable real + module while keeping local solver proofs. + +**Behavior:** run the full contract in one process across CPU and CUDA. Keep +one state/property/wrench integration per supported family and wheel-only +capability probes that mocks cannot validate. Never cache live views across a +manager close; add repeated-run/order tests for fixture isolation. + +**Verify:** run OV unit/kernel tests, the mixed-device lifecycle regression, +and consolidated real asset tests. Compare to the 220.05-second baseline. + +## Task 8: Publish coverage mapping and final performance result + +**Files:** + +- Add one test-only changelog fragment for each test-only touched package and a + patch fragment for the OV manager behavior change. +- Add a before/after report beside the design specification. +- Update test-runner selections or documentation only where needed to expose + the fast contract/unit gate and minimal integration gate. + +**Report:** list every old module and case family as retained, moved, replaced, +removed as redundant, newly covered, or excluded. Include exact commands, +environment, collected/pass/skip counts, test time, wall time, and the ratio to +the 1,197.60-second baseline. Call out unsupported capabilities explicitly. + +**Final verification:** run every focused suite, all five baseline-scope +commands with updated filenames, repeated/order-sensitive fixture checks, +`./isaaclab.sh -f`, and `git diff --check`. The target is at least a threefold +wall-time improvement and under 30 seconds per warmed contract/unit backend. From ff6308bc780cd8d88638d6420c88be42bfaff5fc Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 11:43:33 +0200 Subject: [PATCH 03/26] Allow OV CPU GPU reuse Remove Isaac Lab's process device lock and avoid enabling the wheel's sticky CPU-only mode for ordinary CPU scenes. Explicitly author CPU and GPU scene dynamics settings, and cover lifecycle reuse in one process. --- .../changelog.d/asset-tests-redesign.rst | 5 + .../isaaclab_ov/physics/ovphysx_manager.py | 66 +++++-------- .../test/assets/test_articulation.py | 54 ----------- .../test/assets/test_rigid_object.py | 47 ---------- .../assets/test_rigid_object_collection.py | 36 ------- .../physics/test_ovphysx_manager_lifecycle.py | 93 ++++++++++++++++++- .../test_ovphysx_scene_data_backend.py | 8 +- 7 files changed, 124 insertions(+), 185 deletions(-) create mode 100644 source/isaaclab_ov/changelog.d/asset-tests-redesign.rst 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 00000000000..02d998f6d68 --- /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 03c95355988..dc89142b565 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 63a35ea00ef..8f129e2eb95 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -28,24 +28,6 @@ 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. """ from __future__ import annotations @@ -100,9 +82,6 @@ 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." @@ -203,39 +182,6 @@ def _read_binding_to_torch(articulation: Articulation, tensor_type: int, device: 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." - ) - - def _ovphysx_sim_context(device: str, **kwargs): """Wrapper around :func:`build_simulation_context` that injects OVPhysX cfg. diff --git a/source/isaaclab_ov/test/assets/test_rigid_object.py b/source/isaaclab_ov/test/assets/test_rigid_object.py index d8d330b6025..493b88bd608 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object.py @@ -10,14 +10,6 @@ """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. """ from __future__ import annotations @@ -59,37 +51,9 @@ 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. @@ -1267,18 +1231,7 @@ def test_warmup_attach_stage_not_called_for_cpu(): 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") 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 846636abb85..a630bb0a33d 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object_collection.py @@ -10,14 +10,6 @@ """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. """ from __future__ import annotations @@ -55,34 +47,6 @@ 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. - - 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. 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 1758a9c2a5e..a3ff62031ce 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py @@ -15,6 +15,8 @@ import pytest +from pxr import Usd + pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") @@ -25,9 +27,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 +53,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 +72,34 @@ 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 == [] + + +@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 +423,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 +501,10 @@ 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) + + +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 bad5af8750c..6fa1ce479b0 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)] From 7b9a867cb99b83a578c1ffbfd22d749a1fc66e3c Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 12:09:22 +0200 Subject: [PATCH 04/26] Refactor shared asset contract tests Organize the shared asset contracts by API, data, and write behavior while keeping assertion definitions reusable. Declare backend capabilities explicitly and classify the public base surface so missing coverage cannot disappear silently. --- .../asset-contract-infrastructure.skip | 0 .../isaaclab/test/assets/contract/__init__.py | 6 + .../_articulation_contract_cases.py} | 28 +-- .../_articulation_contract_utils.py} | 38 +--- .../_articulation_ordering_contract_cases.py} | 14 +- .../_contract_boot.py} | 2 +- ...rigid_object_collection_contract_cases.py} | 22 +- ...rigid_object_collection_contract_utils.py} | 39 +--- .../_rigid_object_contract_cases.py} | 17 +- .../_rigid_object_contract_utils.py} | 36 +--- .../test/assets/contract/capabilities.py | 129 ++++++++++++ .../test/assets/contract/public_surface.py | 193 ++++++++++++++++++ .../contract/test_asset_contract_api.py | 135 ++++++++++++ .../contract/test_asset_contract_data.py | 78 +++++++ .../contract/test_asset_contract_writes.py | 40 ++++ .../test/assets/test_iface_test_utils.py | 98 --------- 16 files changed, 654 insertions(+), 221 deletions(-) create mode 100644 source/isaaclab/changelog.d/asset-contract-infrastructure.skip create mode 100644 source/isaaclab/test/assets/contract/__init__.py rename source/isaaclab/test/assets/{test_articulation_iface.py => contract/_articulation_contract_cases.py} (98%) rename source/isaaclab/test/assets/{_articulation_iface_test_utils.py => contract/_articulation_contract_utils.py} (95%) rename source/isaaclab/test/assets/{test_articulation_ordering_iface.py => contract/_articulation_ordering_contract_cases.py} (99%) rename source/isaaclab/test/assets/{_iface_test_boot.py => contract/_contract_boot.py} (93%) rename source/isaaclab/test/assets/{test_rigid_object_collection_iface.py => contract/_rigid_object_collection_contract_cases.py} (98%) rename source/isaaclab/test/assets/{_rigid_object_collection_iface_test_utils.py => contract/_rigid_object_collection_contract_utils.py} (92%) rename source/isaaclab/test/assets/{test_rigid_object_iface.py => contract/_rigid_object_contract_cases.py} (98%) rename source/isaaclab/test/assets/{_rigid_object_iface_test_utils.py => contract/_rigid_object_contract_utils.py} (94%) create mode 100644 source/isaaclab/test/assets/contract/capabilities.py create mode 100644 source/isaaclab/test/assets/contract/public_surface.py create mode 100644 source/isaaclab/test/assets/contract/test_asset_contract_api.py create mode 100644 source/isaaclab/test/assets/contract/test_asset_contract_data.py create mode 100644 source/isaaclab/test/assets/contract/test_asset_contract_writes.py delete mode 100644 source/isaaclab/test/assets/test_iface_test_utils.py 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 00000000000..e69de29bb2d diff --git a/source/isaaclab/test/assets/contract/__init__.py b/source/isaaclab/test/assets/contract/__init__.py new file mode 100644 index 00000000000..2a3b8f1eaf1 --- /dev/null +++ b/source/isaaclab/test/assets/contract/__init__.py @@ -0,0 +1,6 @@ +# 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 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 98% rename from source/isaaclab/test/assets/test_articulation_iface.py rename to source/isaaclab/test/assets/contract/_articulation_contract_cases.py index f4e5b41b1d0..dfc7d9f4943 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,7 +19,8 @@ import pytest import torch import warp as wp -from _articulation_iface_test_utils import BACKENDS, get_articulation +from ._articulation_contract_utils import BACKENDS, get_articulation +from .capabilities import backend_parameters pytestmark = pytest.mark.integration @@ -59,19 +60,19 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name # Common parametrize decorator for all interface tests -_backends = pytest.mark.parametrize("backend", BACKENDS, indirect=False) +_backends = pytest.mark.parametrize("backend", backend_parameters("api"), indirect=False) # 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"]) +_default_devices = pytest.mark.parametrize("device", ["cpu"]) _index_resolution_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton") if backend in BACKENDS], indirect=False + "backend", backend_parameters("index_resolution", names=("physx", "newton")), indirect=False ) _production_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False + "backend", backend_parameters("api"), indirect=False ) @@ -275,7 +276,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 + "backend", backend_parameters("api"), indirect=False ) @@ -1961,15 +1962,14 @@ 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) +# Spatial tendon behavior is selected through the explicit backend capability declaration. +_tendon_backends = pytest.mark.parametrize("backend", backend_parameters("spatial_tendons"), indirect=False) _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 ], ) diff --git a/source/isaaclab/test/assets/_articulation_iface_test_utils.py b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py similarity index 95% rename from source/isaaclab/test/assets/_articulation_iface_test_utils.py rename to source/isaaclab/test/assets/contract/_articulation_contract_utils.py index 0b2caf77639..fb1678f8297 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,31 @@ from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg from isaaclab.utils.wrench_composer import WrenchComposer -BACKENDS: list[str] = [] -BACKEND_UNAVAILABLE_REASONS: dict[str, str] = {} +BACKENDS = available_backends("api") +BACKEND_UNAVAILABLE_REASONS = backend_unavailable_reasons() -try: +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. + + # PhysX data classes need gravity even though contract 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 create_physx_articulation( @@ -358,13 +347,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 + data = NewtonArticulationData(mock_view, device) mock_view._tendon_count = num_fixed_tendons # Create Articulation shell (bypass __init__) 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 85aef24c6d7..ef621f25525 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,7 @@ 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 _pytest.mark.structures import ParameterSet from isaaclab.utils.buffers import TimestampedBufferWarp @@ -955,7 +955,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 +1986,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 +2059,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 +2137,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 +2320,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/_iface_test_boot.py b/source/isaaclab/test/assets/contract/_contract_boot.py similarity index 93% rename from source/isaaclab/test/assets/_iface_test_boot.py rename to source/isaaclab/test/assets/contract/_contract_boot.py index 96d3847b148..1b9dba40366 100644 --- a/source/isaaclab/test/assets/_iface_test_boot.py +++ b/source/isaaclab/test/assets/contract/_contract_boot.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Shared Kit and kitless bootstrap for backend interface tests.""" +"""Shared Kit and kitless bootstrap for backend contract tests.""" import os import sys 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 98% 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 039140c3f22..e6ef78fa004 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,7 +17,8 @@ import pytest import torch import warp as wp -from _rigid_object_collection_iface_test_utils import BACKENDS, get_rigid_object_collection +from ._rigid_object_collection_contract_utils import BACKENDS, get_rigid_object_collection +from .capabilities import backend_parameters pytestmark = pytest.mark.integration @@ -60,20 +60,20 @@ 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 = pytest.mark.parametrize("backend", backend_parameters("api"), indirect=False) +_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"]) +_default_devices = pytest.mark.parametrize("device", ["cpu"]) _index_resolution_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton") if backend in BACKENDS], indirect=False + "backend", backend_parameters("index_resolution", names=("physx", "newton")), indirect=False ) _reshape_3d_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False + "backend", backend_parameters("api"), indirect=False ) _production_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False + "backend", backend_parameters("api"), indirect=False ) 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 92% 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 602e4949729..1f61961a5bf 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,9 @@ from isaaclab.assets.rigid_object_collection.rigid_object_collection_cfg import RigidObjectCollectionCfg from isaaclab.utils.wrench_composer import WrenchComposer -BACKENDS: list[str] = [] +BACKENDS = available_backends("api") -try: +if "physx" in BACKENDS: from isaaclab_physx.assets.rigid_object_collection.rigid_object_collection import ( RigidObjectCollection as PhysXRigidObjectCollection, ) @@ -30,17 +31,13 @@ ) 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. + + # PhysX data classes need gravity even though contract 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 +45,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 +56,6 @@ RigidObjectCollectionData as OvPhysxRigidObjectCollectionData, ) from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet -except ImportError: - pass -else: - if hasattr(OvPhysxRigidObjectCollection, "_create_buffers"): - BACKENDS.append("ovphysx") def create_physx_rigid_object_collection( @@ -170,16 +158,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 + 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 98% rename from source/isaaclab/test/assets/test_rigid_object_iface.py rename to source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py index e8a6147b7b4..79cd8f14dd3 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,7 +17,8 @@ import pytest import torch import warp as wp -from _rigid_object_iface_test_utils import BACKENDS, get_rigid_object +from ._rigid_object_contract_utils import BACKENDS, get_rigid_object +from .capabilities import backend_parameters pytestmark = pytest.mark.integration @@ -45,15 +46,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 = pytest.mark.parametrize("backend", backend_parameters("api"), indirect=False) +_default_dims = pytest.mark.parametrize("num_instances", [2]) -_default_devices = pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +_default_devices = pytest.mark.parametrize("device", ["cpu"]) _index_resolution_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton") if backend in BACKENDS], indirect=False + "backend", backend_parameters("index_resolution", names=("physx", "newton")), indirect=False ) _production_backends = pytest.mark.parametrize( - "backend", [backend for backend in ("physx", "newton", "ovphysx") if backend in BACKENDS], indirect=False + "backend", backend_parameters("api"), indirect=False ) 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 94% 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 94b198da64b..12743d7e7d9 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,30 @@ from isaaclab.assets.rigid_object.rigid_object_cfg import RigidObjectCfg from isaaclab.utils.wrench_composer import WrenchComposer -BACKENDS: list[str] = [] +BACKENDS = available_backends("api") -try: +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. + + # PhysX data classes need gravity even though contract 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 create_physx_rigid_object( @@ -164,13 +153,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 + 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 00000000000..32221603a1c --- /dev/null +++ b/source/isaaclab/test/assets/contract/capabilities.py @@ -0,0 +1,129 @@ +# 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 + +"""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"}) + +BACKEND_DECLARATIONS = ( + BackendDeclaration( + name="physx", + required_modules=("carb", "isaaclab_physx"), + capabilities=_SHARED_CAPABILITIES | {"index_resolution", "spatial_tendons"}, + ), + BackendDeclaration( + name="newton", + required_modules=("isaaclab_newton",), + capabilities=_SHARED_CAPABILITIES | {"index_resolution"}, + unsupported={"spatial_tendons": "Newton does not support spatial tendons"}, + ), + BackendDeclaration( + name="ovphysx", + required_modules=("ovphysx", "isaaclab_ov"), + capabilities=_SHARED_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 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 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/public_surface.py b/source/isaaclab/test/assets/contract/public_surface.py new file mode 100644 index 00000000000..c2ddd6aaf43 --- /dev/null +++ b/source/isaaclab/test/assets/contract/public_surface.py @@ -0,0 +1,193 @@ +# 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 + +"""Public base-surface classification for shared asset contracts.""" + +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 + """, +} + + +PUBLIC_SURFACE_CLASSIFICATIONS = { + f"{class_name}.{member_name}": ( + ContractKind.DATA + if class_name.endswith("Data") + else ContractKind.WRITE + if member_name.startswith(("set_", "write_")) + else ContractKind.API + ) + for class_name, members in _PUBLIC_MEMBER_SNAPSHOT.items() + for member_name in members.split() +} + + +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 00000000000..914b9802f6b --- /dev/null +++ b/source/isaaclab/test/assets/contract/test_asset_contract_api.py @@ -0,0 +1,135 @@ +# 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 + +# ruff: noqa: F401 + +"""Shared asset API contract tests.""" + +from ._articulation_contract_cases import ( # noqa: F401 + TestArticulationFinderReturnModes, + TestArticulationFinders, + TestArticulationIndexResolution, + TestArticulationProperties, + TestArticulationTendonFinders, + TestArticulationTendonProperties, + TestResolveMatchingNamesCache, + articulation_iface, +) +from ._rigid_object_collection_contract_cases import ( # noqa: F401 + TestCollectionFinderReturnModes, + TestCollectionFinders, + TestCollectionIndexResolution, + TestCollectionProperties, + collection_iface, +) +from ._rigid_object_contract_cases import ( # noqa: F401 + TestRigidObjectFinderReturnModes, + TestRigidObjectFinders, + TestRigidObjectIndexResolution, + TestRigidObjectProperties, + rigid_object_iface, +) +from .capabilities import BackendDeclaration, BackendStatus, backend_parameters, evaluate_backend +from .public_surface import ( + BASE_SURFACE_CLASSES, + PUBLIC_SURFACE_CLASSIFICATIONS, + ContractKind, + unclassified_public_members, +) + + +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_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_base_public_surface_has_an_explicit_contract_classification() -> None: + """Require every public member declared by the shared asset bases to have an explicit classification.""" + unclassified = unclassified_public_members(BASE_SURFACE_CLASSES, PUBLIC_SURFACE_CLASSIFICATIONS) + + assert unclassified == set(), f"Unclassified public asset members: {sorted(unclassified)}" 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 00000000000..4b84f2c7494 --- /dev/null +++ b/source/isaaclab/test/assets/contract/test_asset_contract_data.py @@ -0,0 +1,78 @@ +# 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 + +# ruff: noqa: F401 + +"""Shared asset data contract tests.""" + +import pytest + +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 00000000000..f5ffebdf1ed --- /dev/null +++ b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py @@ -0,0 +1,40 @@ +# 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 + +# ruff: noqa: F401 + +"""Shared asset write contract tests.""" + +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 883105ee4d0..00000000000 --- 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 From 86a91f4864937020c3bc0a8fc93f402252b12523 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 12:41:07 +0200 Subject: [PATCH 05/26] Fix shared asset contract coverage Exercise installed PhysX through kitless boundary stubs and classify each contract family with its actual backend capability. Make public-surface coverage an exact, reasoned map to concrete contract targets. --- .../contract/_articulation_contract_cases.py | 70 +++-- .../contract/_articulation_contract_utils.py | 13 +- .../_articulation_ordering_contract_cases.py | 18 +- .../test/assets/contract/_contract_boot.py | 50 +++- ..._rigid_object_collection_contract_cases.py | 28 +- ..._rigid_object_collection_contract_utils.py | 6 +- .../contract/_rigid_object_contract_cases.py | 23 +- .../contract/_rigid_object_contract_utils.py | 4 +- .../test/assets/contract/capabilities.py | 12 + .../test/assets/contract/public_surface.py | 272 +++++++++++++++++- .../contract/test_asset_contract_api.py | 119 +++++++- 11 files changed, 532 insertions(+), 83 deletions(-) diff --git a/source/isaaclab/test/assets/contract/_articulation_contract_cases.py b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py index dfc7d9f4943..5ddc57f0017 100644 --- a/source/isaaclab/test/assets/contract/_articulation_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py @@ -20,7 +20,7 @@ import torch import warp as wp from ._articulation_contract_utils import BACKENDS, get_articulation -from .capabilities import backend_parameters +from .capabilities import backend_parameters, contract_backend pytestmark = pytest.mark.integration @@ -59,8 +59,8 @@ 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", backend_parameters("api"), 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", @@ -68,12 +68,8 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name ) _default_devices = pytest.mark.parametrize("device", ["cpu"]) -_index_resolution_backends = pytest.mark.parametrize( - "backend", backend_parameters("index_resolution", names=("physx", "newton")), indirect=False -) -_production_backends = pytest.mark.parametrize( - "backend", backend_parameters("api"), indirect=False -) +_index_resolution_backends = contract_backend("index_resolution", names=("physx", "newton")) +_production_backends = contract_backend("api") # --------------------------------------------------------------------------- @@ -275,9 +271,7 @@ def test_finder_returns_legacy_list_or_cached_proxy(self, backend, finder_name): # --------------------------------------------------------------------------- -_non_mock_backends = pytest.mark.parametrize( - "backend", backend_parameters("api"), indirect=False -) +_non_mock_backends = contract_backend("api") class TestResolveMatchingNamesCache: @@ -342,6 +336,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.""" @@ -1331,6 +1327,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"] @@ -1908,6 +1907,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.""" @@ -1962,8 +1963,9 @@ def test_joint_aliases(self, backend, num_instances, num_joints, num_bodies, dev # Tendon tests — parametrize, properties, finders, data, writers # --------------------------------------------------------------------------- -# Spatial tendon behavior is selected through the explicit backend capability declaration. -_tendon_backends = pytest.mark.parametrize("backend", backend_parameters("spatial_tendons"), 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", @@ -1994,7 +1996,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 +2033,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 +2103,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 +2125,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 +2213,8 @@ def test_fixed_tendon_limit_stiffness( device, articulation_iface, ): + if backend == "newton": + pytest.xfail("Newton does not implement fixed-tendon limit stiffness") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2234,6 +2238,8 @@ def test_fixed_tendon_rest_length( device, articulation_iface, ): + if backend == "newton": + pytest.xfail("Newton does not implement fixed-tendon rest length") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2257,6 +2263,8 @@ def test_fixed_tendon_offset( device, articulation_iface, ): + if backend == "newton": + pytest.xfail("Newton does not implement fixed-tendon offset") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2280,6 +2288,8 @@ def test_fixed_tendon_pos_limits( device, articulation_iface, ): + if backend == "newton": + pytest.xfail("Newton does not expose fixed-tendon position limits") art, _ = articulation_iface art.data.update(dt=0.01) from isaaclab.utils.warp import ProxyArray @@ -2297,7 +2307,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 +2332,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 +2357,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 +2382,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 +2451,8 @@ def test_fixed_tendon_writer_index( wp_dtype, accepts_float, ): + if backend == "newton" and method_base not in {"set_fixed_tendon_stiffness", "set_fixed_tendon_damping"}: + pytest.xfail(f"Newton does not implement {method_base}") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2506,6 +2518,8 @@ def test_fixed_tendon_writer_mask( wp_dtype, accepts_float, ): + if backend == "newton": + pytest.xfail("Newton fixed-tendon mask writers are not implemented") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2559,7 +2573,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 +2634,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 +2717,8 @@ def test_write_fixed_tendon_properties_to_sim_index( device, articulation_iface, ): + if backend == "newton": + pytest.xfail("Newton fixed-tendon write-to-sim does not resolve a default environment selector") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2727,6 +2743,8 @@ def test_write_fixed_tendon_properties_to_sim_mask( device, articulation_iface, ): + if backend == "newton": + pytest.xfail("Newton fixed-tendon mask write-to-sim is not implemented") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2737,7 +2755,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 +2781,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/contract/_articulation_contract_utils.py b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py index fb1678f8297..4b5238cd7b4 100644 --- a/source/isaaclab/test/assets/contract/_articulation_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py @@ -177,7 +177,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")) @@ -311,6 +313,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, @@ -319,6 +322,12 @@ def create_newton_articulation( ) mock_view.set_random_mock_data() mock_view._noop_setters = True + 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() @@ -349,8 +358,6 @@ def create_newton_articulation( # Patch SimulationManager in the Newton data module newton_data_module.SimulationManager = mock_manager data = NewtonArticulationData(mock_view, device) - mock_view._tendon_count = num_fixed_tendons - # Create Articulation shell (bypass __init__) articulation = object.__new__(NewtonArticulation) diff --git a/source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py b/source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py index ef621f25525..623f32d9bd2 100644 --- a/source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_articulation_ordering_contract_cases.py @@ -14,6 +14,7 @@ import torch import warp as wp 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 diff --git a/source/isaaclab/test/assets/contract/_contract_boot.py b/source/isaaclab/test/assets/contract/_contract_boot.py index 1b9dba40366..f2d66e5af64 100644 --- a/source/isaaclab/test/assets/contract/_contract_boot.py +++ b/source/isaaclab/test/assets/contract/_contract_boot.py @@ -7,6 +7,7 @@ import os import sys +from importlib.machinery import ModuleSpec from unittest.mock import MagicMock _kitless = "ovphysx" in os.environ.get("LD_PRELOAD", "") or ( @@ -19,13 +20,46 @@ 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. + + 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 _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()) + 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/_rigid_object_collection_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py index e6ef78fa004..ab3663c14ab 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py @@ -18,7 +18,7 @@ import torch import warp as wp from ._rigid_object_collection_contract_utils import BACKENDS, get_rigid_object_collection -from .capabilities import backend_parameters +from .capabilities import contract_backend pytestmark = pytest.mark.integration @@ -60,21 +60,15 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name # Common parametrize decorators -_backends = pytest.mark.parametrize("backend", backend_parameters("api"), indirect=False) +_backends = contract_backend("api") _default_dims = pytest.mark.parametrize("num_instances", [2]) _default_bodies = pytest.mark.parametrize("num_bodies", [3]) _default_devices = pytest.mark.parametrize("device", ["cpu"]) -_index_resolution_backends = pytest.mark.parametrize( - "backend", backend_parameters("index_resolution", names=("physx", "newton")), indirect=False -) -_reshape_3d_backends = pytest.mark.parametrize( - "backend", backend_parameters("api"), indirect=False -) -_production_backends = pytest.mark.parametrize( - "backend", backend_parameters("api"), indirect=False -) +_index_resolution_backends = contract_backend("index_resolution", names=("physx", "newton")) +_reshape_3d_backends = contract_backend("data") +_production_backends = contract_backend("api") # --------------------------------------------------------------------------- @@ -395,6 +389,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 +826,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("data") + + class TestCollectionCacheInvalidation: @_production_backends def test_pose_write_invalidates_pose_dependent_caches(self, backend): @@ -909,6 +909,9 @@ 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.""" @@ -1252,6 +1255,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/contract/_rigid_object_collection_contract_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py index 1f61961a5bf..13086a4ea69 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py @@ -110,10 +110,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 diff --git a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py index 79cd8f14dd3..8d81f879586 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py @@ -18,7 +18,7 @@ import torch import warp as wp from ._rigid_object_contract_utils import BACKENDS, get_rigid_object -from .capabilities import backend_parameters +from .capabilities import contract_backend pytestmark = pytest.mark.integration @@ -46,16 +46,12 @@ def _check_proxy_array(arr, *, expected_shape: tuple, expected_dtype: type, name # Common parametrize decorators -_backends = pytest.mark.parametrize("backend", backend_parameters("api"), indirect=False) +_backends = contract_backend("api") _default_dims = pytest.mark.parametrize("num_instances", [2]) _default_devices = pytest.mark.parametrize("device", ["cpu"]) -_index_resolution_backends = pytest.mark.parametrize( - "backend", backend_parameters("index_resolution", names=("physx", "newton")), indirect=False -) -_production_backends = pytest.mark.parametrize( - "backend", backend_parameters("api"), indirect=False -) +_index_resolution_backends = contract_backend("index_resolution", names=("physx", "newton")) +_production_backends = contract_backend("api") # --------------------------------------------------------------------------- @@ -173,6 +169,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.""" @@ -744,6 +742,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("data") + + class TestRigidObjectCacheInvalidation: @_production_backends def test_pose_write_invalidates_pose_dependent_caches(self, backend): @@ -824,6 +825,9 @@ 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.""" @@ -1096,6 +1100,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/contract/_rigid_object_contract_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py index 12743d7e7d9..35937f8fbdb 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py @@ -104,7 +104,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")) diff --git a/source/isaaclab/test/assets/contract/capabilities.py b/source/isaaclab/test/assets/contract/capabilities.py index 32221603a1c..bb62a60d38a 100644 --- a/source/isaaclab/test/assets/contract/capabilities.py +++ b/source/isaaclab/test/assets/contract/capabilities.py @@ -101,6 +101,18 @@ def backend_parameters( 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 [ diff --git a/source/isaaclab/test/assets/contract/public_surface.py b/source/isaaclab/test/assets/contract/public_surface.py index c2ddd6aaf43..458b2fa801b 100644 --- a/source/isaaclab/test/assets/contract/public_surface.py +++ b/source/isaaclab/test/assets/contract/public_surface.py @@ -5,7 +5,9 @@ """Public base-surface classification for shared asset contracts.""" +from dataclasses import dataclass from enum import StrEnum +from importlib import import_module from isaaclab.assets.articulation.base_articulation import BaseArticulation from isaaclab.assets.articulation.base_articulation_data import BaseArticulationData @@ -169,19 +171,271 @@ class ContractKind(StrEnum): } +@dataclass(frozen=True) +class PublicMemberContract: + """Map one public member to a concrete contract target or justified exclusion.""" + + kind: ContractKind + target: str | None = None + 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] + untargeted: 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 or self.untargeted) + + 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), + ("covered members without targets", self.untargeted), + ) + if values + ) + + +def _mapping( + class_name: str, + members: str, + kind: ContractKind, + *, + target: str | None = None, + 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, target=target, reason=reason) + for member_name in members.split() + } + + +_ARTICULATION_API_CONTRACT = "contract._articulation_contract_cases.TestArticulationProperties" +_ARTICULATION_DATA_CONTRACT = "contract._articulation_contract_cases.TestArticulationDataRootState" +_ARTICULATION_WRITE_CONTRACT = "contract._articulation_contract_cases.TestArticulationWritersRoot" +_COLLECTION_API_CONTRACT = "contract._rigid_object_collection_contract_cases.TestCollectionProperties" +_COLLECTION_DATA_CONTRACT = "contract._rigid_object_collection_contract_cases.TestCollectionDataBodyState" +_COLLECTION_WRITE_CONTRACT = "contract._rigid_object_collection_contract_cases.TestCollectionWritersPose" +_RIGID_OBJECT_API_CONTRACT = "contract._rigid_object_contract_cases.TestRigidObjectProperties" +_RIGID_OBJECT_DATA_CONTRACT = "contract._rigid_object_contract_cases.TestRigidObjectDataRootState" +_RIGID_OBJECT_WRITE_CONTRACT = "contract._rigid_object_contract_cases.TestRigidObjectWritersRoot" + +PUBLIC_SURFACE_CONTRACTS = { + **_mapping( + "AssetBase", + "data device is_initialized num_instances", + ContractKind.API, + target=_ARTICULATION_API_CONTRACT, + ), + **_mapping( + "AssetBase", + "assert_shape_and_dtype assert_shape_and_dtype_mask reset update write_data_to_sim", + ContractKind.WRITE, + target=_ARTICULATION_WRITE_CONTRACT, + ), + **_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, + target=_RIGID_OBJECT_API_CONTRACT, + ), + **_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, + target=_RIGID_OBJECT_WRITE_CONTRACT, + ), + **_mapping( + "BaseRigidObjectData", + _PUBLIC_MEMBER_SNAPSHOT["BaseRigidObjectData"], + ContractKind.DATA, + target=_RIGID_OBJECT_DATA_CONTRACT, + ), + **_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, + target=_COLLECTION_API_CONTRACT, + ), + **_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, + target=_COLLECTION_WRITE_CONTRACT, + ), + **_mapping( + "BaseRigidObjectCollectionData", + _PUBLIC_MEMBER_SNAPSHOT["BaseRigidObjectCollectionData"], + ContractKind.DATA, + target=_COLLECTION_DATA_CONTRACT, + ), + **_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, + target=_ARTICULATION_API_CONTRACT, + ), + **_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, + target=_ARTICULATION_WRITE_CONTRACT, + ), + **_mapping( + "BaseArticulationData", + _PUBLIC_MEMBER_SNAPSHOT["BaseArticulationData"], + ContractKind.DATA, + target=_ARTICULATION_DATA_CONTRACT, + ), +} + PUBLIC_SURFACE_CLASSIFICATIONS = { - f"{class_name}.{member_name}": ( - ContractKind.DATA - if class_name.endswith("Data") - else ContractKind.WRITE - if member_name.startswith(("set_", "write_")) - else ContractKind.API - ) - for class_name, members in _PUBLIC_MEMBER_SNAPSHOT.items() - for member_name in members.split() + 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} + covered_kinds = {ContractKind.API, ContractKind.DATA, ContractKind.WRITE} + 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 + ), + untargeted=frozenset( + member_name + for member_name, contract in mappings.items() + if contract.kind in covered_kinds and not contract.target + ), + ) + + +def unresolved_contract_targets(mappings: dict[str, PublicMemberContract]) -> frozenset[str]: + """Return concrete covered-contract targets that cannot be imported.""" + unresolved = set() + for contract in mappings.values(): + if contract.target is None: + continue + try: + import_module(contract.target) + except ModuleNotFoundError: + module_name, _, attribute = contract.target.rpartition(".") + try: + module = import_module(module_name) + except ModuleNotFoundError: + unresolved.add(contract.target) + else: + if not hasattr(module, attribute): + unresolved.add(contract.target) + return frozenset(unresolved) + + def unclassified_public_members(classes: tuple[type, ...], classifications: dict[str, ContractKind]) -> set[str]: """Return declared public members that have no contract classification.""" declared_members = { diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_api.py b/source/isaaclab/test/assets/contract/test_asset_contract_api.py index 914b9802f6b..f05c8267988 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_api.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_api.py @@ -7,6 +7,13 @@ """Shared asset API contract tests.""" +from importlib.util import find_spec + +import pytest + +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, @@ -17,6 +24,8 @@ 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 ( # noqa: F401 TestCollectionFinderReturnModes, TestCollectionFinders, @@ -31,7 +40,13 @@ TestRigidObjectProperties, rigid_object_iface, ) -from .capabilities import BackendDeclaration, BackendStatus, backend_parameters, evaluate_backend +from .capabilities import ( + BACKEND_STATUSES, + BackendDeclaration, + BackendStatus, + backend_parameters, + evaluate_backend, +) from .public_surface import ( BASE_SURFACE_CLASSES, PUBLIC_SURFACE_CLASSIFICATIONS, @@ -102,6 +117,46 @@ def test_backend_parameters_keep_unsupported_behavior_as_explicit_skip() -> None 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"), + (_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) + + def test_public_surface_reports_an_omitted_declared_member() -> None: """Report a newly declared public member until a contract explicitly classifies it.""" @@ -128,8 +183,64 @@ def added_member(self) -> int: 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, + target="contract._articulation_contract_cases.TestArticulationProperties", + ), + "SyntheticAsset.removed_member": public_surface.PublicMemberContract( + kind=ContractKind.API, + target="contract._articulation_contract_cases.TestArticulationProperties", + ), + } + + 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_public_surface_contract_targets_resolve_to_real_test_classes() -> None: + """Require every covered public member to name a concrete importable contract class.""" + assert public_surface.unresolved_contract_targets(public_surface.PUBLIC_SURFACE_CONTRACTS) == frozenset() + + def test_base_public_surface_has_an_explicit_contract_classification() -> None: - """Require every public member declared by the shared asset bases to have an explicit classification.""" - unclassified = unclassified_public_members(BASE_SURFACE_CLASSES, PUBLIC_SURFACE_CLASSIFICATIONS) + """Require the mapping and current public inventory to match exactly.""" + audit = public_surface.audit_public_surface(BASE_SURFACE_CLASSES, public_surface.PUBLIC_SURFACE_CONTRACTS) - assert unclassified == set(), f"Unclassified public asset members: {sorted(unclassified)}" + assert audit.is_valid, audit.format_errors() From 0fe09659febd271a22501ff4ae71374b2a008000 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 12:46:53 +0200 Subject: [PATCH 06/26] Fix write contract capability selectors Classify rigid-object and collection cache invalidation under the writes capability. Cover all three asset families in the selector regression. --- .../contract/_rigid_object_collection_contract_cases.py | 2 +- .../test/assets/contract/_rigid_object_contract_cases.py | 2 +- .../test/assets/contract/test_asset_contract_api.py | 6 ++++++ 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py index ab3663c14ab..18bebf0c0c8 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py @@ -826,7 +826,7 @@ 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("data") +_production_backends = contract_backend("writes") class TestCollectionCacheInvalidation: diff --git a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py index 8d81f879586..ae3477f0981 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py @@ -742,7 +742,7 @@ 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("data") +_production_backends = contract_backend("writes") class TestRigidObjectCacheInvalidation: diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_api.py b/source/isaaclab/test/assets/contract/test_asset_contract_api.py index f05c8267988..9a8ebb863a7 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_api.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_api.py @@ -26,6 +26,9 @@ ) 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, @@ -33,6 +36,7 @@ TestCollectionProperties, collection_iface, ) +from ._rigid_object_contract_cases import TestRigidObjectCacheInvalidation as _RigidObjectCacheInvalidationContract from ._rigid_object_contract_cases import ( # noqa: F401 TestRigidObjectFinderReturnModes, TestRigidObjectFinders, @@ -135,6 +139,8 @@ def test_installed_backend_packages_cannot_remain_silently_unavailable() -> None [ (_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"), From 30dcb80f567c7fcdd670a4dac781e84310039a5f Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 13:02:11 +0200 Subject: [PATCH 07/26] Focus wrench composer tests Replace randomized size matrices and duplicated PhysX scenarios with literal unit coverage and one rotated force-at-position parity case. --- .../test/utils/test_wrench_composer.py | 1858 ++--------------- .../utils/test_wrench_composer_integration.py | 854 +------- .../utils/test_wrench_composer_vs_physx.py | 849 -------- 3 files changed, 228 insertions(+), 3333 deletions(-) delete mode 100644 source/isaaclab/test/utils/test_wrench_composer_vs_physx.py diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py index b0a346eaa6c..fe476bafe4a 100644 --- a/source/isaaclab/test/utils/test_wrench_composer.py +++ b/source/isaaclab/test/utils/test_wrench_composer.py @@ -3,1775 +3,267 @@ # # 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) +class _AssetData: + """Minimal asset data consumed by :class:`WrenchComposer`.""" - # Apply global forces - wrench_composer.add_forces_and_torques_index(forces=forces_global, 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)) - # 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}" +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 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}" + asset = SimpleNamespace( + num_instances=2, + num_bodies=2, + device="cpu", + data=_AssetData(com_pos_w, link_quat_w), ) + return WrenchComposer(asset) -@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 +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) - # 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) +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) - # 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) +def test_local_force_and_torque_at_position_compose_to_literal_wrench() -> None: + composer = _make_composer() - # 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) - - # Apply global forces at link origin - wrench_composer.add_forces_and_torques_index(forces=forces_global, positions=positions_at_link, is_global=True) - - # 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), - ) - - # 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, - ) - - # 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) - - 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) - - -@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) - - # 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, + forces=_vectors([[[4.0, -1.0, 0.0]]]), torques=_vectors([[[2.0, 0.0, -3.0]]]), body_ids=[1], env_ids=[0] ) - # 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, - ) + 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]]]) - 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" - ) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) + torch.testing.assert_close(composer.out_torque_b.torch, expected_torque) -# ============================================================================ -# Partial Reset Tests -# ============================================================================ - - -@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([[[2.0, 0.0, 0.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 - - -@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 - - 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 -# ============================================================================ + 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]]]) + expected_torque = torch.tensor([[[0.0, 0.0, -4.0], [0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]]) + 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) -@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) +def test_index_and_mask_selection_change_only_selected_cells() -> None: + composer = _make_composer() - 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), + 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]), ) - with pytest.warns(DeprecationWarning, match="composed_force.*is deprecated"): - result = composer.composed_force + 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) - # 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) +def test_set_clears_only_targeted_environment_before_writing() -> None: + composer = _make_composer() 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), + 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]], + ] ) - - 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.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" - ) + composer.set_forces_and_torques_index(forces=_vectors([[[9.0, 0.0, 0.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]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) -@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), - 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) +def test_mask_set_clears_only_masked_environments_before_writing() -> None: + composer = _make_composer() 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}" + 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]], + ] ) - 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), - 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, - ) - - 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) - - 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" - ) - - -@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) - composer.add_forces_and_torques_index( - torques=wp.from_numpy(torques_global_np, dtype=wp.vec3f, device=device), - is_global=True, - ) - - # 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" - ) - - -@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) - composer.add_forces_and_torques_index( - forces=wp.from_numpy(forces_np, dtype=wp.vec3f, device=device), - ) - 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), - ) - 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) - - 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) - - 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), - ), - 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.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}" - ) - - # Force should be unchanged (identity rotation) - 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_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 - - # 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 - - 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) - - composer = WrenchComposer(mock_asset) - - # 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), - 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" - ) - - -@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), - ) - 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), + 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]], + ] ), - dim=-1, + env_mask=_mask([False, True]), + body_mask=_mask([False, True]), ) - mock_asset.data.set_body_com_pose_w(com_pose) - composer = WrenchComposer(mock_asset) + 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]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_force) - # 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) +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), - positions=wp.from_numpy(positions_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]], + ] + ) ) - composer.compose_to_body_frame() + 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]]]) + torch.testing.assert_close(composer.out_force_b.torch, expected_after_partial) + assert composer.active - # 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) + 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))) + assert not composer.active - assert np.allclose(composer.out_force_b.warp.numpy(), expected_force_b, atol=1e-3, rtol=1e-4), ( - "Force mismatch with CoM offset + rotation" - ) - 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()}" - ) +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]) -# ============================================================================ -# Deprecated set_forces_and_torques Tests -# ============================================================================ + instantaneous.reset() + 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))) -@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) +def test_raw_buffer_merge_accumulates_and_ignores_inactive_source() -> None: + destination = _make_composer() + source = _make_composer() + inactive_source = _make_composer() + destination.add_forces_and_torques_index(forces=_vectors([[[1.0, 0.0, 0.0]]]), body_ids=[0], env_ids=[0]) + source.add_forces_and_torques_index(forces=_vectors([[[0.0, 2.0, 0.0]]]), body_ids=[0], env_ids=[0]) - forces_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32) + destination.add_raw_buffers_from(source) + destination.add_raw_buffers_from(inactive_source) - 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), - ) + expected_force = torch.tensor([[[1.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(destination.out_force_b.torch, expected_force) - composer.compose_to_body_frame() - assert np.allclose(composer.out_force_b.warp.numpy(), forces_np, atol=1e-4, rtol=1e-5) +def test_invalid_selection_and_empty_wrench_input_are_reported() -> None: + composer = _make_composer() -@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 5f5e0ccb1ee..09404a172cf 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 +simulation_app = AppLauncher(headless=True, device="cpu").app -"""Rest everything follows.""" +import math import pytest import torch @@ -28,818 +22,76 @@ 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( +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)) + + spawn = sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg(), ) - - 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 - - -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, + 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), ) - 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, - ) - - -@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([[[10.0, 0.0, 0.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) - - # 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}" + composer.update(sim.cfg.dt) + raw.update(sim.cfg.dt) - # 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}" - ), - ) - - -@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." + composer.data.root_ang_vel_w.torch, raw.data.root_ang_vel_w.torch, rtol=1.0e-4, atol=1.0e-4 ) - - # 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]).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 f186993d3b8..00000000000 --- 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" - ) From 3e6c1e37cd34428b1f735750f34be602600256c1 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 13:16:35 +0200 Subject: [PATCH 08/26] Strengthen wrench composer coverage Cover distinct global, torque, reset, and raw-buffer paths with literal tests. Make the retained PhysX parity geometry detect rotated induced torque. --- .../test/utils/test_wrench_composer.py | 180 ++++++++++++++++-- .../utils/test_wrench_composer_integration.py | 4 +- 2 files changed, 171 insertions(+), 13 deletions(-) diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py index fe476bafe4a..06910268e4e 100644 --- a/source/isaaclab/test/utils/test_wrench_composer.py +++ b/source/isaaclab/test/utils/test_wrench_composer.py @@ -104,20 +104,100 @@ def test_global_force_at_position_rotates_force_and_induced_torque() -> None: ) composer.add_forces_and_torques_index( - forces=_vectors([[[2.0, 0.0, 0.0]]]), + 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, ) - 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]]]) - expected_torque = torch.tensor([[[0.0, 0.0, -4.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, 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]]]) 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) +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.add_forces_and_torques_index( + forces=_vectors([[[2.0, 0.0, 0.0]]]), body_ids=[0], env_ids=[0], is_global=True + ) + + 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))) + + +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, + ) + composer.set_forces_and_torques_mask( + 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, + ) + + 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) + + def test_index_and_mask_selection_change_only_selected_cells() -> None: composer = _make_composer() @@ -147,13 +227,23 @@ def test_set_clears_only_targeted_environment_before_writing() -> None: [[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]], + ] + ), ) - composer.set_forces_and_torques_index(forces=_vectors([[[9.0, 0.0, 0.0]]]), body_ids=[1], env_ids=[0]) + 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) def test_mask_set_clears_only_masked_environments_before_writing() -> None: @@ -164,7 +254,13 @@ def test_mask_set_clears_only_masked_environments_before_writing() -> None: [[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]], + ] + ), ) composer.set_forces_and_torques_mask( @@ -174,12 +270,20 @@ def test_mask_set_clears_only_masked_environments_before_writing() -> None: [[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]), ) 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) def test_partial_and_full_reset_clear_their_documented_scope() -> None: @@ -190,17 +294,28 @@ def test_partial_and_full_reset_clear_their_documented_scope() -> None: [[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]], + ] + ), ) 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.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 @@ -217,18 +332,61 @@ def test_permanent_and_instantaneous_composers_remain_independent() -> None: torch.testing.assert_close(instantaneous.out_force_b.torch, torch.zeros((2, 2, 3))) -def test_raw_buffer_merge_accumulates_and_ignores_inactive_source() -> None: +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]) + + 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 + + +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([[[1.0, 0.0, 0.0]]]), body_ids=[0], env_ids=[0]) - source.add_forces_and_torques_index(forces=_vectors([[[0.0, 2.0, 0.0]]]), body_ids=[0], env_ids=[0]) + 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, + ) destination.add_raw_buffers_from(source) destination.add_raw_buffers_from(inactive_source) - expected_force = torch.tensor([[[1.0, 2.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([[[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]]]), + ) + 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]]]), + ) + 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]]]), + ) + 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]]]), + ) + 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) def test_invalid_selection_and_empty_wrench_input_are_reported() -> None: diff --git a/source/isaaclab/test/utils/test_wrench_composer_integration.py b/source/isaaclab/test/utils/test_wrench_composer_integration.py index 09404a172cf..89c8ade52c2 100644 --- a/source/isaaclab/test/utils/test_wrench_composer_integration.py +++ b/source/isaaclab/test/utils/test_wrench_composer_integration.py @@ -61,7 +61,7 @@ def test_rotated_global_force_at_position_matches_physx_delivery() -> None: sim.reset() body_ids, _ = composer.find_bodies(".*") - force = torch.tensor([[[10.0, 0.0, 0.0]]], device=device) + 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 @@ -94,4 +94,4 @@ def test_rotated_global_force_at_position_matches_physx_delivery() -> None: torch.testing.assert_close( composer.data.root_ang_vel_w.torch, raw.data.root_ang_vel_w.torch, rtol=1.0e-4, atol=1.0e-4 ) - assert torch.abs(composer.data.root_ang_vel_w.torch[0, 2]).item() > 0.01 + assert torch.abs(composer.data.root_ang_vel_w.torch[0, :2]).max().item() > 0.01 From 6089a7e0982b83a4d9266b98e54afdf02cdbb40a Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 13:20:02 +0200 Subject: [PATCH 09/26] Cover inactive wrench merge guard Seed an inactive composer with test-only raw data so the merge guard remains observable. --- source/isaaclab/test/utils/test_wrench_composer.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py index 06910268e4e..cb0b7dcbfcf 100644 --- a/source/isaaclab/test/utils/test_wrench_composer.py +++ b/source/isaaclab/test/utils/test_wrench_composer.py @@ -359,6 +359,16 @@ def test_raw_buffer_merge_accumulates_all_five_buffers_and_ignores_inactive_sour env_ids=[0], is_global=True, ) + 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) From a73f00c440d4b55110662cd192a3abbbbc3fd10b Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 13:43:59 +0200 Subject: [PATCH 10/26] Focus Newton rigid asset tests --- .../changelog.d/asset-tests-redesign.skip | 0 .../test/assets/test_rigid_object.py | 1424 +---------------- .../assets/test_rigid_object_collection.py | 1123 +------------ .../test/assets/unit/__init__.py | 4 + .../assets/unit/test_rigid_assets_import.py | 79 + ...t_rigid_object_collection_model_indices.py | 24 + .../assets/unit/test_rigid_object_fk_cache.py | 74 + .../test_rigid_object_inertial_staging.py | 72 + .../assets/{ => unit}/test_wrench_kernels.py | 2 +- 9 files changed, 404 insertions(+), 2398 deletions(-) create mode 100644 source/isaaclab_newton/changelog.d/asset-tests-redesign.skip create mode 100644 source/isaaclab_newton/test/assets/unit/__init__.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_rigid_object_collection_model_indices.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_rigid_object_fk_cache.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_rigid_object_inertial_staging.py rename source/isaaclab_newton/test/assets/{ => unit}/test_wrench_kernels.py (97%) 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 00000000000..e69de29bb2d diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index f00e5a6011a..2ca926870cd 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -3,27 +3,11 @@ # # 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 @@ -32,1360 +16,104 @@ 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() - - -@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 - - # Play sim - with pytest.raises(RuntimeError): - sim.reset() - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(device): - """Test if external force buffer correctly updates in the force value is zero case. - - In this test, we apply a non-zero force, then a zero force, then finally a non-zero force - to an object. We check if the force buffer is properly updated at each step. - """ - - # Generate cubes scene - with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=1, device=device) - - # 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 - 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, - ) - ) - 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, - ) - ) - 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, +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)), ) + ) - # 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(), - ), - ) - ) - # Play sim +@pytest.mark.parametrize("device", _DEVICES) +def test_rigid_object_real_newton_seams(device: str) -> 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() - # 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() - - assert original_masses.shape == (num_cubes, 1) + 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) + 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) + 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) - # 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])) + 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]) - # 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()) + 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() ) - # 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 = 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) + 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, + ) - # 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. + 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() - - # 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 + rigid_object.update(sim.cfg.dt) - 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] 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 b833777766d..25f3738b059 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,100 @@ 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() -> 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) + 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) - # 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 + 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, ) - # 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) - - -@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() + 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]) - 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) - - 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) - - -@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() - - # Check if gravity vector is set correctly + 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() ) - # 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 = 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) + 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) diff --git a/source/isaaclab_newton/test/assets/unit/__init__.py b/source/isaaclab_newton/test/assets/unit/__init__.py new file mode 100644 index 00000000000..460a3056908 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_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 00000000000..cf18c6dd602 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -0,0 +1,79 @@ +# 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 + +"""Collection guard for the kitless Newton rigid-asset suites.""" + +import os +import subprocess +import sys +from pathlib import Path + +import pytest + +_ASSET_TEST_DIR = Path(__file__).resolve().parents[1] +_TARGETS = ("test_rigid_object.py", "test_rigid_object_collection.py") + + +@pytest.mark.parametrize("target", _TARGETS) +def test_rigid_asset_module_collects_without_kit_isaacsim_or_nucleus(target: str, tmp_path: Path) -> None: + """Collect each real module while rejecting Kit, IsaacSim, and Nucleus access.""" + 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: + """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 00000000000..c5cb299eabc --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_fk_cache.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 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 + + +@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 00000000000..ac383f78948 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_inertial_staging.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 kernel tests for Newton rigid-body inertial staging.""" + +import numpy as np +import warp as wp +from isaaclab_newton.assets import kernels + + +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/test_wrench_kernels.py b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py similarity index 97% rename from source/isaaclab_newton/test/assets/test_wrench_kernels.py rename to source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py index 4dc272cc941..d341ce2b231 100644 --- a/source/isaaclab_newton/test/assets/test_wrench_kernels.py +++ b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py @@ -44,7 +44,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") From 27dc5c251efbf225061d96fd2b4f1144363ad435 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 14:10:42 +0200 Subject: [PATCH 11/26] Strengthen Newton rigid test seams --- .../test/assets/test_rigid_object.py | 71 +++++++++++- .../assets/test_rigid_object_collection.py | 33 +++++- .../assets/unit/test_rigid_assets_import.py | 42 +++++-- .../test_rigid_object_setter_notifications.py | 105 ++++++++++++++++++ .../test/assets/unit/test_wrench_kernels.py | 75 +++++++++++++ 5 files changed, 317 insertions(+), 9 deletions(-) create mode 100644 source/isaaclab_newton/test/assets/unit/test_rigid_object_setter_notifications.py diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 2ca926870cd..c92ad310f99 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -13,6 +13,8 @@ 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 @@ -58,8 +60,43 @@ def _spawn_cubes() -> RigidObject: ) +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) + + with pytest.raises(RuntimeError): + sim.reset() + assert not rigid_object.is_initialized + + @pytest.mark.parametrize("device", _DEVICES) -def test_rigid_object_real_newton_seams(device: str) -> None: +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() @@ -72,6 +109,15 @@ def test_rigid_object_real_newton_seams(device: str) -> None: assert rigid_object.data.body_com_pos_b.shape == (2, 1) assert rigid_object.data.body_inertia.shape == (2, 1, 9) + model_changes = [] + add_model_change = SimulationManager.add_model_change + + def record_model_change(change: ModelFlags) -> None: + model_changes.append(change) + add_model_change(change) + + 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) @@ -82,14 +128,32 @@ def test_rigid_object_real_newton_seams(device: str) -> None: 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(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] + model_changes.clear() model = SimulationManager.get_model() + 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) @@ -102,7 +166,11 @@ def test_rigid_object_real_newton_seams(device: str) -> None: atol=1e-6, rtol=1e-6, ) + assert model_changes == [ModelFlags.MODEL_PROPERTIES] + 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) @@ -117,3 +185,4 @@ def test_rigid_object_real_newton_seams(device: str) -> None: rigid_object.update(sim.cfg.dt) 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 25f3738b059..454a5e1da02 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -67,7 +67,7 @@ def _spawn_collection() -> RigidObjectCollection: ) -def test_rigid_object_collection_real_newton_seams() -> None: +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: @@ -82,6 +82,15 @@ def test_rigid_object_collection_real_newton_seams() -> None: assert collection.data.body_com_pos_b.shape == (2, 2) assert collection.data.body_inertia.shape == (2, 2, 9) + model_changes = [] + add_model_change = SimulationManager.add_model_change + + def record_model_change(change: ModelFlags) -> None: + model_changes.append(change) + add_model_change(change) + + 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() @@ -95,15 +104,36 @@ def test_rigid_object_collection_real_newton_seams() -> None: 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]) + 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( 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] + model_changes.clear() model = SimulationManager.get_model() + 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) @@ -113,3 +143,4 @@ def test_rigid_object_collection_real_newton_seams() -> None: 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_rigid_assets_import.py b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py index cf18c6dd602..7c4598080dc 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -13,12 +13,14 @@ import pytest _ASSET_TEST_DIR = Path(__file__).resolve().parents[1] -_TARGETS = ("test_rigid_object.py", "test_rigid_object_collection.py") +_TARGETS = ( + ("test_rigid_object.py", "test_rigid_object_real_newton_seams[cpu]"), + ("test_rigid_object_collection.py", "test_rigid_object_collection_real_newton_seams"), +) -@pytest.mark.parametrize("target", _TARGETS) -def test_rigid_asset_module_collects_without_kit_isaacsim_or_nucleus(target: str, tmp_path: Path) -> None: - """Collect each real module while rejecting Kit, IsaacSim, and Nucleus access.""" +def _run_monitored_target(target: Path, node: str, tmp_path: Path) -> subprocess.CompletedProcess[str]: + """Run a target under import and Nucleus sentinels.""" sitecustomize = tmp_path / "sitecustomize.py" sitecustomize.write_text( """ @@ -67,13 +69,39 @@ def __add__(self, other): encoding="utf-8", ) env = os.environ | {"PYTHONPATH": str(tmp_path)} - result = subprocess.run( - [sys.executable, "-m", "pytest", str(_ASSET_TEST_DIR / target), "--collect-only", "-q"], + return subprocess.run( + [sys.executable, "-m", "pytest", f"{target}::{node}", "-q"], cwd=_ASSET_TEST_DIR, env=env, capture_output=True, text=True, - timeout=30, + timeout=60, ) + +@pytest.mark.parametrize(("target", "node"), _TARGETS) +def test_rigid_asset_cpu_seam_runs_without_kit_isaacsim_or_nucleus(target: str, node: str, tmp_path: Path) -> None: + """Run each real CPU seam while rejecting Kit, IsaacSim, and Nucleus access.""" + result = _run_monitored_target(_ASSET_TEST_DIR / target, node, 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_target(mutated_target, "test_rigid_object_real_newton_seams[cpu]", tmp_path) + + assert result.returncode != 0 + assert "forbidden Nucleus asset used by Newton rigid-asset test" in result.stdout + result.stderr 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 00000000000..2c667eef6cb --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_object_setter_notifications.py @@ -0,0 +1,105 @@ +# 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 RigidObject, RigidObjectCollection +from isaaclab_newton.physics import NewtonManager as SimulationManager +from newton import ModelFlags + + +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 is 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( + _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"), [(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"), [(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/unit/test_wrench_kernels.py b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py index d341ce2b231..06f2d4483ce 100644 --- a/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py +++ b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py @@ -71,3 +71,78 @@ def test_update_wrench_array_ordered_rotates_and_scatters_to_backend_order() -> 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) From 1d3f10ddea40d20cb0ceeaa88280aaaab7e242b3 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 14:16:08 +0200 Subject: [PATCH 12/26] Cover runtime AppLauncher guard --- .../assets/unit/test_rigid_assets_import.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) 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 index 7c4598080dc..f111278a6b7 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -105,3 +105,22 @@ def test_runtime_nucleus_access_inside_fixture_is_rejected(tmp_path: Path) -> No assert result.returncode != 0 assert "forbidden Nucleus asset used by Newton rigid-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_target(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 From 0f5c90edc8a098c1168f0251b50718864d4cbda4 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 14:41:56 +0200 Subject: [PATCH 13/26] Focus Newton articulation asset tests --- .../test/assets/test_articulation.py | 4913 +---------------- .../assets/test_newton_actuators_newton.py | 1054 +--- .../assets/unit/test_articulation_fk_cache.py | 69 + .../unit/test_articulation_joint_staging.py | 113 + .../assets/unit/test_articulation_ordering.py | 81 + .../unit/test_newton_actuator_adaptation.py | 161 + .../assets/unit/test_rigid_assets_import.py | 20 +- .../test_rigid_object_setter_notifications.py | 18 +- 8 files changed, 699 insertions(+), 5730 deletions(-) create mode 100644 source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py create mode 100644 source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index e45d1e96b1d..fba8511f718 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3,4784 +3,193 @@ # # SPDX-License-Identifier: BSD-3-Clause -# ignore private usage of variables warning -# pyright: reportPrivateUsage=none +"""Kitless real-solver integration tests for Newton articulations.""" -"""Launch Isaac Sim Simulator first.""" - -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", - ), - 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( +pytestmark = pytest.mark.integration + + +def _newton_sim_context(*, device: str = "cpu", use_newton_actuators: bool = False): + """Create a fresh 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), + use_newton_actuators=use_newton_actuators, + ) + ) + + +def _author_two_link_articulations(*, actuators: dict | None = None) -> Articulation: + """Author two local one-DOF articulations without files or remote assets.""" + 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/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - joint_ordering="physx", - body_ordering="physx", + prim_path="/World/Env_[^/]*/Robot", + articulation_root_prim_path="/Root", + actuators=( + actuators + if actuators is not None + else { + "joint": ImplicitActuatorCfg( + joint_names_expr=["Joint"], + stiffness=20.0, + damping=2.0, + ) + } + ), ) ) - 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") - ) - 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) +def test_articulation_real_newton_seams(monkeypatch) -> None: + """Exercise the minimal real Newton articulation adapter with isolated partial writes.""" + with _newton_sim_context() as sim: + articulation = _author_two_link_articulations() + sim.reset() - with pytest.warns(DeprecationWarning, match="write_actuator_stiffness_to_sim"): - articulation.write_actuator_stiffness_to_sim( - stiffness=stiffness, + assert articulation.is_initialized + 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) + joint_ids = torch.tensor([0], dtype=torch.int32) + body_ids = torch.tensor([1], dtype=torch.int32) + initial_root_pose = articulation.data.root_link_pose_w.torch.clone() + initial_joint_pos = articulation.data.joint_pos.torch.clone() + initial_joint_vel = articulation.data.joint_vel.torch.clone() + target_root_pose = initial_root_pose[env_ids].clone() + target_root_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3]) + target_joint_pos = torch.tensor([[0.25]], dtype=torch.float32) + target_joint_vel = torch.tensor([[-0.5]], dtype=torch.float32) + + articulation.write_root_link_pose_to_sim_index(root_pose=target_root_pose, env_ids=env_ids) + articulation.write_joint_state_to_sim_index( + position=target_joint_pos, + velocity=target_joint_vel, 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, - ) - 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. - - 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() - 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] - ) - np.testing.assert_allclose(data._body_com_vel_w_user.numpy(), data._sim_bind_body_com_vel_w.numpy()[:, body_u2b]) - - -@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() - 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)}, - ) - 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 + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[env_ids], target_root_pose) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_root_pose[:1]) + 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]) + + 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)) + initial_mass = articulation.data.body_mass.torch.clone() + masses = torch.tensor([[3.0]]) + 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]]]) + 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]]])).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] + + jacobians = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobians.shape == (2, 2, 6, 7) + assert mass_matrix.shape == (2, 7, 7) + assert torch.isfinite(jacobians).all() + assert torch.isfinite(mass_matrix).all() + + 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]]]), + torques=torch.zeros((1, 1, 3)), + env_ids=torch.tensor([1], dtype=torch.int32), + body_ids=torch.tensor([0], dtype=torch.int32), ) - np.testing.assert_allclose(articulation.data._sim_bind_joint_position_target.numpy(), expected_source.numpy()) - + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) -@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", + 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 ) - ) - 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) +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") +def test_articulation_cuda_jacobian_and_mass_access() -> None: + """Smoke-test Newton's CUDA-backed Jacobian and mass-matrix adapter access.""" + with _newton_sim_context(device="cuda:0") as sim: + articulation = _author_two_link_articulations() 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"]) + jacobians = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobians.device.type == "cuda" + assert mass_matrix.device.type == "cuda" + assert jacobians.shape == (2, 2, 6, 7) + assert mass_matrix.shape == (2, 7, 7) + assert torch.isfinite(jacobians).all() + assert torch.isfinite(mass_matrix).all() 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 1e99e95edd8..1d603dd421b 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,109 @@ # # 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 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. - 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}", + 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, ) - - 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", - ) - 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 - - 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) - sim.reset() - assert articulation.is_initialized - - model = SimulationManager.get_model() - - actuator_info = [] - for act in model.actuators: - ctrl_type = type(act.controller).__name__ - clamp_types = sorted(type(c).__name__ for c in (act.clamping or [])) - actuator_info.append( - { - "controller_type": ctrl_type, - "clamping_types": clamp_types, - "has_delay": act.delay is not None, - "num_indices": len(act.indices), - } - ) - - init_pos = wp.to_torch(articulation.data.joint_pos).clone() - target_pos = init_pos + TARGET_OFFSET - target_vel = torch.zeros_like(init_pos) - articulation.set_joint_position_target_index(target=target_pos) - articulation.set_joint_velocity_target_index(target=target_vel) - - recorded_pos = [] - for _ in range(NUM_STEPS): - articulation.write_data_to_sim() - sim.step() - articulation.update(DT) - recorded_pos.append(wp.to_torch(articulation.data.joint_pos).clone()) - - return { - "num_actuators": len(model.actuators), - "actuator_info": actuator_info, - "joint_pos": recorded_pos, - } - - -class TestRemotizedPDAuthoring(unittest.TestCase): - """Verify RemotizedPDActuatorCfg is authored as Newton PD + delay + - position-based clamping. - - Uses the Spot knee lookup table on ANYmal's KFE joints, with IdealPD - on HAA and HFE joints. - """ - - @classmethod - def setUpClass(cls): - cls.result = _run_authoring_introspection(_remotized_pd_actuators()) - - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_kfe_controller_is_pd(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - self.assertTrue(len(kfe_acts) > 0, "No actuator with position-based clamping found") - for a in kfe_acts: - self.assertEqual(a["controller_type"], "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", + }, ) + articulation_cfg._post_spawn(stage) + return Articulation(articulation_cfg) - 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], +def _run_actuator_path(*, use_newton_actuators: bool) -> dict[str, list[torch.Tensor] | torch.Tensor]: + """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, ) - - 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() + with build_simulation_context(sim_cfg=sim_cfg) as sim: + articulation = _author_two_link_articulations() + sim.reset() + initial_position = articulation.data.joint_pos.torch.clone() + target_position = initial_position + torch.tensor([[0.10], [-0.15]]) + articulation.set_joint_position_target_index(target=target_position) + articulation.set_joint_velocity_target_index(target=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(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 { + "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) + + 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) 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 00000000000..94d456c469a --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py @@ -0,0 +1,69 @@ +# 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 warp as wp +from isaaclab_newton.assets.articulation.articulation_data import ArticulationData +from isaaclab_newton.physics import NewtonManager as SimulationManager + + +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 00000000000..3514a5e95f8 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py @@ -0,0 +1,113 @@ +# 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 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 + + +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 00000000000..7e6c0c3508b --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py @@ -0,0 +1,81 @@ +# 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 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 + + +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) + callback = lambda: None + 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 00000000000..dd48a143c70 --- /dev/null +++ b/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py @@ -0,0 +1,161 @@ +# 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, 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 + + +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 + + +def test_builder_target_modes_align_sparse_gains_by_joint_name(monkeypatch) -> None: + """Sparse gain dictionaries must select modes by physical joint name before model finalization.""" + cfg = ArticulationCfg( + prim_path="/World/Robot", + actuators={ + "joints": ImplicitActuatorCfg( + joint_names_expr=[".*_joint"], + stiffness={"left_joint": 10.0}, + damping={"right_joint": 2.0}, + ) + }, + ) + monkeypatch.setattr( + "isaaclab_newton.assets.articulation.articulation._resolve_articulation_root_prim_path_expr", + lambda _cfg: "/World/Robot", + ) + builder = _target_mode_builder() + + _configure_builder_joint_target_modes(builder, cfg) + + assert builder.joint_target_mode == [int(JointTargetMode.POSITION), int(JointTargetMode.VELOCITY)] + + +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 index f111278a6b7..d91b03541fe 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Collection guard for the kitless Newton rigid-asset suites.""" +"""Runtime guard for the kitless Newton asset suites.""" import os import subprocess @@ -16,8 +16,12 @@ _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_real_newton_seams"), + ("test_newton_actuators_newton.py", "test_newton_actuator_real_equivalence"), ) +_TARGET_FILENAMES = tuple(target for target, _ in _TARGETS) + def _run_monitored_target(target: Path, node: str, tmp_path: Path) -> subprocess.CompletedProcess[str]: """Run a target under import and Nucleus sentinels.""" @@ -37,9 +41,7 @@ def find_spec(self, fullname, path=None, target=None): or frame.f_code.co_filename == __file__ ): frame = frame.f_back - imported_by_target = frame is not None and frame.f_code.co_filename.endswith( - ("test_rigid_object.py", "test_rigid_object_collection.py") - ) + imported_by_target = frame is not None and frame.f_code.co_filename.endswith(__TARGET_FILENAMES__) if fullname.startswith("isaacsim") or ( fullname.startswith("isaaclab.app") and (imported_by_target or fullname == "isaaclab.app.app_launcher") ): @@ -50,8 +52,8 @@ def find_spec(self, fullname, path=None, target=None): class _ForbiddenNucleusPath(str): def _guard(self): caller = sys._getframe(2).f_code.co_filename - if caller.endswith(("test_rigid_object.py", "test_rigid_object_collection.py")): - raise RuntimeError("forbidden Nucleus asset used by Newton rigid-asset test") + if caller.endswith(__TARGET_FILENAMES__): + raise RuntimeError("forbidden Nucleus asset used by Newton asset test") def __format__(self, format_spec): self._guard() @@ -65,7 +67,7 @@ def __add__(self, other): sys.meta_path.insert(0, _ForbiddenFinder()) assets.ISAAC_NUCLEUS_DIR = _ForbiddenNucleusPath(assets.ISAAC_NUCLEUS_DIR) assets.ISAACLAB_NUCLEUS_DIR = _ForbiddenNucleusPath(assets.ISAACLAB_NUCLEUS_DIR) -""", +""".replace("__TARGET_FILENAMES__", repr(_TARGET_FILENAMES)), encoding="utf-8", ) env = os.environ | {"PYTHONPATH": str(tmp_path)} @@ -80,7 +82,7 @@ def __add__(self, other): @pytest.mark.parametrize(("target", "node"), _TARGETS) -def test_rigid_asset_cpu_seam_runs_without_kit_isaacsim_or_nucleus(target: str, node: str, tmp_path: Path) -> None: +def test_newton_asset_cpu_seam_runs_without_kit_isaacsim_or_nucleus(target: str, node: str, tmp_path: Path) -> None: """Run each real CPU seam while rejecting Kit, IsaacSim, and Nucleus access.""" result = _run_monitored_target(_ASSET_TEST_DIR / target, node, tmp_path) @@ -104,7 +106,7 @@ def test_runtime_nucleus_access_inside_fixture_is_rejected(tmp_path: Path) -> No result = _run_monitored_target(mutated_target, "test_rigid_object_real_newton_seams[cpu]", tmp_path) assert result.returncode != 0 - assert "forbidden Nucleus asset used by Newton rigid-asset test" in result.stdout + result.stderr + 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: 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 index 2c667eef6cb..13eb4db5f25 100644 --- 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 @@ -10,7 +10,7 @@ import numpy as np import pytest import warp as wp -from isaaclab_newton.assets import RigidObject, RigidObjectCollection +from isaaclab_newton.assets import Articulation, RigidObject, RigidObjectCollection from isaaclab_newton.physics import NewtonManager as SimulationManager from newton import ModelFlags @@ -27,12 +27,16 @@ def _minimal_asset(asset_type, num_bodies: int): 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 is RigidObject: + 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"), @@ -43,7 +47,10 @@ def _minimal_asset(asset_type, num_bodies: int): return asset, staged_inertia -@pytest.mark.parametrize(("asset_type", "num_bodies"), [(RigidObject, 1), (RigidObjectCollection, 2)]) +@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: @@ -73,7 +80,10 @@ def test_mass_setter_stages_only_selection_and_notifies_inertial_change( assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] -@pytest.mark.parametrize(("asset_type", "num_bodies"), [(RigidObject, 1), (RigidObjectCollection, 2)]) +@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: From 2867052d0e01529050473dbc66c70726d47aab34 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 15:14:04 +0200 Subject: [PATCH 14/26] Restore Newton controller bridge coverage --- .../test/articulation_test_utils.py | 85 +++++++++ .../test/assets/test_articulation.py | 33 ++++ .../assets/test_newton_actuators_newton.py | 46 ++++- .../assets/unit/test_articulation_ordering.py | 5 +- .../unit/test_newton_actuator_adaptation.py | 106 +++++++++-- .../assets/unit/test_rigid_assets_import.py | 11 +- .../test_newton_task_space_controllers.py | 173 ++++++++++++++++++ 7 files changed, 441 insertions(+), 18 deletions(-) create mode 100644 source/isaaclab_newton/test/articulation_test_utils.py create mode 100644 source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py 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 00000000000..2c71c5e2914 --- /dev/null +++ b/source/isaaclab_newton/test/articulation_test_utils.py @@ -0,0 +1,85 @@ +# 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) -> Articulation: + """Author the smallest fixed chain with a full-rank spatial Jacobian.""" + 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() + robot_path = "/World/Robot" + 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]) + + axes: Sequence[str] = ("X", "Y", "Z", "X", "Y", "Z") + 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=robot_path, + 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 fba8511f718..debdcd6b422 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -19,6 +19,8 @@ from isaaclab.assets import ArticulationCfg from isaaclab.sim import SimulationCfg, build_simulation_context +from source.isaaclab_newton.test.articulation_test_utils import author_fixed_spatial_chain, build_newton_context + pytestmark = pytest.mark.integration @@ -178,6 +180,37 @@ def record_model_change(change: ModelFlags) -> None: ) +def test_fixed_base_articulation_real_newton_seams() -> None: + """Exercise fixed-root state, moving-link velocity, and dynamics data on a local chain.""" + with build_newton_context() as sim: + articulation = author_fixed_spatial_chain() + sim.reset() + + assert articulation.is_initialized + assert articulation.is_fixed_base + assert articulation.num_instances == 1 + assert articulation.num_bodies == 7 + assert articulation.num_joints == 6 + initial_root_pose = articulation.data.root_link_pose_w.torch.clone() + target = articulation.data.joint_pos.torch.clone() + target[:, 0] = 0.05 + articulation.actuators.target_command.set_position_index(value=target) + + for _ in range(12): + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, initial_root_pose, atol=1e-6, rtol=0) + assert torch.linalg.vector_norm(articulation.data.body_link_vel_w.torch[0, -1]) > 1e-3 + jacobians = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobians.shape == (1, 6, 6, 6) + assert mass_matrix.shape == (1, 6, 6) + assert torch.isfinite(jacobians).all() + assert torch.isfinite(mass_matrix).all() + + @pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") def test_articulation_cuda_jacobian_and_mass_access() -> None: """Smoke-test Newton's CUDA-backed Jacobian and mass-matrix adapter access.""" 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 1d603dd421b..daaed45bbf4 100644 --- a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py +++ b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py @@ -8,6 +8,7 @@ import torch 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 @@ -61,7 +62,7 @@ def _author_two_link_articulations() -> Articulation: return Articulation(articulation_cfg) -def _run_actuator_path(*, use_newton_actuators: bool) -> dict[str, list[torch.Tensor] | torch.Tensor]: +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", @@ -73,10 +74,20 @@ def _run_actuator_path(*, use_newton_actuators: bool) -> dict[str, list[torch.Te with build_simulation_context(sim_cfg=sim_cfg) as sim: articulation = _author_two_link_articulations() sim.reset() + 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.set_joint_position_target_index(target=target_position) - articulation.set_joint_velocity_target_index(target=torch.zeros_like(target_position)) + 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 = [] @@ -92,6 +103,8 @@ def _run_actuator_path(*, use_newton_actuators: bool) -> dict[str, list[torch.Te 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, @@ -105,7 +118,34 @@ def test_newton_actuator_real_equivalence() -> None: 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, + } + 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/unit/test_articulation_ordering.py b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py index 7e6c0c3508b..36066bee528 100644 --- a/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py @@ -63,7 +63,10 @@ def test_post_step_publish_refreshes_joint_and_body_shadows_in_public_order() -> 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) - callback = lambda: None + + def callback() -> None: + pass + articulation._model_init_handle = None articulation._physics_ready_handle = None articulation._post_step_callback = callback 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 index dd48a143c70..bf58de678f9 100644 --- a/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py +++ b/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py @@ -13,7 +13,7 @@ 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, Model, ModelBuilder +from newton import JointTargetMode, JointType, Model, ModelBuilder from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.actuators.newton.adapter import resolve_actuator_component @@ -36,34 +36,112 @@ def _target_mode_builder() -> ModelBuilder: return builder -def test_builder_target_modes_align_sparse_gains_by_joint_name(monkeypatch) -> None: - """Sparse gain dictionaries must select modes by physical joint name before model finalization.""" - cfg = ArticulationCfg( - prim_path="/World/Robot", - actuators={ - "joints": ImplicitActuatorCfg( +@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 == [int(JointTargetMode.POSITION), int(JointTargetMode.VELOCITY)] + 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))) + monkeypatch.setattr( + SimulationManager, "activate_newton_actuator_path", classmethod(lambda cls: activations.append(1)) + ) groups = NewtonActuatorControl(articulation).prepare_native_actuators( collection=None, @@ -112,7 +190,9 @@ def test_native_actuator_reset_delegates_selected_environments_to_adapter(monkey 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))) + monkeypatch.setattr( + SimulationManager, "_adapter", SimpleNamespace(reset=lambda env_ids: reset_calls.append(env_ids)) + ) env_ids = [1] control.reset_native_actuators(env_ids) 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 index d91b03541fe..b866875692f 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -18,9 +18,18 @@ ("test_rigid_object_collection.py", "test_rigid_object_collection_real_newton_seams"), ("test_articulation.py", "test_articulation_real_newton_seams"), ("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(target for target, _ in _TARGETS) +_TARGET_FILENAMES = tuple(Path(target).name for target, _ in _TARGETS) + ("articulation_test_utils.py",) def _run_monitored_target(target: Path, node: str, tmp_path: Path) -> subprocess.CompletedProcess[str]: 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 00000000000..a6a5d3bfefa --- /dev/null +++ b/source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py @@ -0,0 +1,173 @@ +# 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 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 + + +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 From ca61466ace0586073fb3922d42aaf40827748220 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 15:19:38 +0200 Subject: [PATCH 15/26] Classify Newton controller tests as integration --- .../test/controllers/test_newton_task_space_controllers.py | 4 ++++ 1 file changed, 4 insertions(+) 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 index a6a5d3bfefa..df72294389c 100644 --- a/source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py +++ b/source/isaaclab_newton/test/controllers/test_newton_task_space_controllers.py @@ -5,6 +5,7 @@ """Kitless controller bridges for Newton articulation dynamics data.""" +import pytest import torch from isaaclab.actuators import ImplicitActuatorCfg @@ -15,8 +16,11 @@ 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.""" From ce495a26b4cfeb4b1a78a029b8b1318673cd82b9 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 15:55:02 +0200 Subject: [PATCH 16/26] Focus PhysX asset tests Split backend-specific logic into fast Kitless units and retain a minimal set of local real-solver seams. Move actuator runtime and termination behavior to their owning suites, and fix collection COM and inertia TensorAPI layouts exposed by the new acceptance coverage. --- .../actuators/test_physx_actuator_runtime.py | 93 + .../test/envs/mdp/test_terminations.py | 33 + .../changelog.d/asset-tests-redesign.rst | 5 + .../deformable_object/deformable_object.py | 32 +- .../rigid_object_collection.py | 3 +- .../test/assets/test_articulation.py | 3141 +---------------- .../test/assets/test_deformable_object.py | 458 +-- .../assets/test_newton_actuators_physx.py | 951 +---- .../test/assets/test_rigid_object.py | 1348 +------ .../assets/test_rigid_object_collection.py | 937 +---- .../test/assets/test_surface_gripper.py | 2 + .../test/assets/unit/__init__.py | 6 + .../test/assets/unit/_imports.py | 35 + .../test/assets/unit/test_actuator_control.py | 105 + .../test_articulation.py} | 92 +- .../assets/unit/test_deformable_object.py | 106 + .../test/assets/unit/test_rigid_object.py | 49 + .../unit/test_rigid_object_collection.py | 98 + 18 files changed, 1105 insertions(+), 6389 deletions(-) create mode 100644 source/isaaclab/test/actuators/test_physx_actuator_runtime.py create mode 100644 source/isaaclab/test/envs/mdp/test_terminations.py create mode 100644 source/isaaclab_physx/changelog.d/asset-tests-redesign.rst create mode 100644 source/isaaclab_physx/test/assets/unit/__init__.py create mode 100644 source/isaaclab_physx/test/assets/unit/_imports.py create mode 100644 source/isaaclab_physx/test/assets/unit/test_actuator_control.py rename source/isaaclab_physx/test/assets/{test_articulation_kernels.py => unit/test_articulation.py} (64%) create mode 100644 source/isaaclab_physx/test/assets/unit/test_deformable_object.py create mode 100644 source/isaaclab_physx/test/assets/unit/test_rigid_object.py create mode 100644 source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py 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 00000000000..f057e218f8d --- /dev/null +++ b/source/isaaclab/test/actuators/test_physx_actuator_runtime.py @@ -0,0 +1,93 @@ +# 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 + +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_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/envs/mdp/test_terminations.py b/source/isaaclab/test/envs/mdp/test_terminations.py new file mode 100644 index 00000000000..5dd9926abab --- /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_physx/changelog.d/asset-tests-redesign.rst b/source/isaaclab_physx/changelog.d/asset-tests-redesign.rst new file mode 100644 index 00000000000..37668041f76 --- /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 58abfdb614e..826fa37b7e0 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 f7b41249097..3973acdb95c 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 6263ce09eb8..7e3e445f10b 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -3,31 +3,12 @@ # # 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 articulations.""" 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).app -"""Rest everything follows.""" - -import sys from pathlib import Path import pytest @@ -38,3047 +19,119 @@ from pxr import 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.assets import ArticulationCfg 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 - -## -# Pre-defined configs -## -from isaaclab_assets import ( # isort:skip - ANYMAL_C_CFG, - FRANKA_PANDA_CFG, - FRANKA_PANDA_HIGH_PD_CFG, - SHADOW_HAND_CFG, -) - - -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_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", - ), - 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 - - -# --------------------------------------------------------------------------- -# 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 - - -@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() - 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", ["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) +pytestmark = pytest.mark.integration - 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 +_FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - 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:]) - ) - 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, - ) - 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) - - torch.testing.assert_close( - _to_device_tensor(ordered.root_view.get_root_transforms(), device), - _to_device_tensor(identity.root_view.get_root_transforms(), device), - ) - torch.testing.assert_close( - _to_device_tensor(ordered.root_view.get_root_velocities(), device), - _to_device_tensor(identity.root_view.get_root_velocities(), device), - ) - 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" +def _spawn_ordered_articulation() -> Articulation: + """Spawn the cached local branching fixture with nonidentity public axes.""" articulation = Articulation( ArticulationCfg( prim_path="/World/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), + spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), actuators={}, joint_ordering="mjwarp", body_ordering="mjwarp", ) ) - 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 + UsdPhysics.FixedJoint.Define(sim_utils.get_current_stage(), "/World/Robot/fixed_root").GetBody1Rel().SetTargets( + ["/World/Robot/base"] + ) + return articulation -@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, - ) - ) - sim.reset() - assert articulation.is_initialized +def test_articulation_real_physx_seams() -> None: + """Prove ordered joint state, model-property writes, Jacobian, and mass access.""" + with build_simulation_context(device="cpu", gravity_enabled=False) as sim: + articulation = _spawn_ordered_articulation() + sim.reset() - if ordering_axis == "joint": + assert articulation.is_initialized + assert articulation.is_fixed_base 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 - 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 + assert articulation.num_instances == 1 + assert articulation.num_joints >= 2 + assert articulation.num_bodies >= 3 + + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32) + target_position = torch.tensor([[0.21, -0.13]]) + target_velocity = torch.tensor([[0.41, -0.23]]) + expected_position = articulation.data.joint_pos.torch.clone() + expected_velocity = articulation.data.joint_vel.torch.clone() + expected_position[:, joint_ids] = target_position + expected_velocity[:, joint_ids] = target_velocity + articulation.write_joint_state_to_sim_index( + position=target_position, velocity=target_velocity, joint_ids=joint_ids + ) + torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) + torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + joint_backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_dof_positions()), expected_position[:, joint_backend_to_user] + ) + + body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32) + masses = torch.tensor([[2.5, 3.5]]) + articulation.set_masses_index(masses=masses, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_mass.torch[:, body_ids], masses) + + coms = articulation.data.body_com_pose_b.torch[:, body_ids].clone() + coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03]) + coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02]) + articulation.set_coms_index(coms=coms, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) + + inertias = articulation.data.body_inertia.torch[:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[0, 1, 4] *= 1.3 + articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) + + body_backend_to_user = list(articulation.body_ordering.backend_to_user_indices) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_masses()), + articulation.data.body_mass.torch[:, body_backend_to_user], + ) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_coms()), + articulation.data.body_com_pose_b.torch[:, body_backend_to_user], + ) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_inertias()), + articulation.data.body_inertia.torch[:, body_backend_to_user], + ) + + sim.step() + articulation.update(sim.cfg.dt) + jacobian = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobian.shape == ( + 1, + articulation.num_bodies - 1, + 6, + articulation.num_joints, + ) + assert mass_matrix.shape == (1, articulation.num_joints, articulation.num_joints) + 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) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") +def test_articulation_cuda_dynamics_access() -> None: + """Smoke-test the distinct CUDA-backed Jacobian and mass-matrix path.""" + with build_simulation_context(device="cuda:0", gravity_enabled=False) as sim: + articulation = _spawn_ordered_articulation() + sim.reset() 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.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_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"]) + assert articulation.data.body_link_jacobian_w.torch.device.type == "cuda" + assert articulation.data.mass_matrix.torch.device.type == "cuda" + assert torch.isfinite(articulation.data.body_link_jacobian_w.torch).all() + assert torch.isfinite(articulation.data.mass_matrix.torch).all() diff --git a/source/isaaclab_physx/test/assets/test_deformable_object.py b/source/isaaclab_physx/test/assets/test_deformable_object.py index d1bd725bd61..f29ad888895 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 06e8e6f9e6f..6755af8dbfd 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 40b9b80fb33..0a51fe36ddd 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -3,1300 +3,96 @@ # # 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).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) + + 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) + + 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) + + 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 - # 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) + 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, ) - - # Simulate physics + rigid_object.write_data_to_sim() sim.step() - cube_object.update(sim.cfg.dt) + rigid_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) - ) - - 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 - 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. - - 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 6286c017e2e..da796ac9730 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).app import pytest import torch @@ -27,874 +17,79 @@ 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]) + + 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) + + 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) + + 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) diff --git a/source/isaaclab_physx/test/assets/test_surface_gripper.py b/source/isaaclab_physx/test/assets/test_surface_gripper.py index e61c7c22a76..6214eca2ade 100644 --- a/source/isaaclab_physx/test/assets/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/test_surface_gripper.py @@ -35,6 +35,8 @@ from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR from isaaclab.utils.version import get_isaac_sim_version, has_kit +pytestmark = pytest.mark.integration + # from isaacsim.robot.surface_gripper import GripperView _RUNNING_CI = bool( diff --git a/source/isaaclab_physx/test/assets/unit/__init__.py b/source/isaaclab_physx/test/assets/unit/__init__.py new file mode 100644 index 00000000000..187a2311706 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/__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 + +"""Focused unit coverage for PhysX asset adapter logic.""" 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 00000000000..fe9e48e777e --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/_imports.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""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 + + +def import_physx_module(module_name: str): + """Import a PhysX asset module while replacing only its unavailable Kit boundary.""" + cloner = ModuleType("isaaclab_physx.cloner") + cloner.queue_physx_replication = lambda cfg: None + physics = ModuleType("isaaclab_physx.physics") + physics.PhysxManager = 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}) + with warnings.catch_warnings(): + warnings.simplefilter("ignore", DeprecationWarning) + with patch.object(omni, "physics", omni_physics, create=True), patch.dict(sys.modules, stubs): + return importlib.import_module(module_name) diff --git a/source/isaaclab_physx/test/assets/unit/test_actuator_control.py b/source/isaaclab_physx/test/assets/unit/test_actuator_control.py new file mode 100644 index 00000000000..0f5bac347fa --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_actuator_control.py @@ -0,0 +1,105 @@ +# 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 tests.""" + +from types import SimpleNamespace +from unittest.mock import Mock + +import warp as wp +from isaaclab_physx.assets.articulation.actuator_control import PhysxActuatorControl + + +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.""" + refresh_position = Mock() + refresh_velocity = Mock() + runtime = SimpleNamespace(compute=Mock()) + 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) + + refresh_position.assert_called_once_with() + refresh_velocity.assert_called_once_with() + runtime.compute.assert_called_once_with(collection, 0.01) diff --git a/source/isaaclab_physx/test/assets/test_articulation_kernels.py b/source/isaaclab_physx/test/assets/unit/test_articulation.py similarity index 64% rename from source/isaaclab_physx/test/assets/test_articulation_kernels.py rename to source/isaaclab_physx/test/assets/unit/test_articulation.py index 93d2368b68b..82bfa483648 100644 --- a/source/isaaclab_physx/test/assets/test_articulation_kernels.py +++ b/source/isaaclab_physx/test/assets/unit/test_articulation.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for PhysX articulation Warp kernels.""" +"""Focused PhysX articulation staging, cache, friction, and kernel tests.""" import sys import warnings @@ -14,6 +14,8 @@ import pytest import warp as wp 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, ) @@ -158,3 +160,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_deformable_object.py b/source/isaaclab_physx/test/assets/unit/test_deformable_object.py new file mode 100644 index 00000000000..e5432f3c457 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_deformable_object.py @@ -0,0 +1,106 @@ +# 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 tests.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import warp as wp +from isaaclab_physx.assets.deformable_object.kernels import ( + compute_mean_vec3f_over_vertices, + set_kinematic_flags_to_one, + write_nodal_vec3f_to_buffer, +) + +from ._imports import import_physx_module + + +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_rigid_object.py b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py new file mode 100644 index 00000000000..0a13482b6bb --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Focused PhysX rigid-object CPU staging and cached-view tests.""" + +from types import SimpleNamespace + +import numpy as np +import torch +import warp as wp + +from ._imports import import_physx_module + + +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) diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py b/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py new file mode 100644 index 00000000000..e265d10f56f --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py @@ -0,0 +1,98 @@ +# 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 tests.""" + +import numpy as np +import torch +import warp as wp +from isaaclab_physx.assets.rigid_object_collection.kernels import resolve_view_ids, resolve_view_ids_kernel + +from ._imports import import_physx_module + + +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"] From 724fc3f92a0bcc347c96fe3952cb7a6ff50032ce Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 16:26:02 +0200 Subject: [PATCH 17/26] Close PhysX asset test coverage gaps Add literal backend readbacks and temporal actuator contracts that catch cached-only or stale-state regressions. Replace the skipped remote surface-gripper cases with local real and fast unit seams, and isolate Kitless imports from later real imports. --- .../actuators/test_physx_actuator_runtime.py | 55 ++- .../test/assets/test_articulation.py | 21 +- .../test/assets/test_rigid_object.py | 18 +- .../assets/test_rigid_object_collection.py | 25 +- .../test/assets/test_surface_gripper.py | 357 +++++------------- .../test/assets/unit/_imports.py | 24 +- .../test/assets/unit/test_actuator_control.py | 62 ++- .../test/assets/unit/test_articulation.py | 33 +- .../test/assets/unit/test_rigid_object.py | 22 ++ .../test/assets/unit/test_surface_gripper.py | 72 ++++ 10 files changed, 371 insertions(+), 318 deletions(-) create mode 100644 source/isaaclab_physx/test/assets/unit/test_surface_gripper.py diff --git a/source/isaaclab/test/actuators/test_physx_actuator_runtime.py b/source/isaaclab/test/actuators/test_physx_actuator_runtime.py index f057e218f8d..2fa1558d01f 100644 --- a/source/isaaclab/test/actuators/test_physx_actuator_runtime.py +++ b/source/isaaclab/test/actuators/test_physx_actuator_runtime.py @@ -6,7 +6,7 @@ """Focused unit tests for the shared host-side PhysX actuator runtime.""" from types import SimpleNamespace -from unittest.mock import Mock +from unittest.mock import Mock, call import pytest import warp as wp @@ -83,6 +83,59 @@ def _swap_adapter_state(*args, **kwargs) -> None: 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() diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 7e3e445f10b..cc647436366 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -7,7 +7,7 @@ from isaaclab.app import AppLauncher -simulation_app = AppLauncher(headless=True).app +simulation_app = AppLauncher(headless=True, device="cpu").app from pathlib import Path @@ -16,7 +16,7 @@ import warp as wp from isaaclab_physx.assets import Articulation -from pxr import UsdPhysics +from pxr import UsdGeom, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg @@ -38,9 +38,11 @@ def _spawn_ordered_articulation() -> Articulation: body_ordering="mjwarp", ) ) - UsdPhysics.FixedJoint.Define(sim_utils.get_current_stage(), "/World/Robot/fixed_root").GetBody1Rel().SetTargets( - ["/World/Robot/base"] - ) + stage = sim_utils.get_current_stage() + collision = UsdGeom.Cube.Define(stage, "/World/Robot/base/collision") + collision.CreateSizeAttr(0.1) + UsdPhysics.CollisionAPI.Apply(collision.GetPrim()) + UsdPhysics.FixedJoint.Define(stage, "/World/Robot/fixed_root").GetBody1Rel().SetTargets(["/World/Robot/base"]) return articulation @@ -106,6 +108,15 @@ def test_articulation_real_physx_seams() -> None: articulation.data.body_inertia.torch[:, body_backend_to_user], ) + materials = torch.empty((1, articulation.root_view.max_shapes, 3)) + materials[..., 0] = 0.91 + materials[..., 1] = 0.17 + materials[..., 2] = 0.63 + articulation.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.array([0], dtype=wp.int32, device="cpu") + ) + torch.testing.assert_close(wp.to_torch(articulation.root_view.get_material_properties()), materials) + sim.step() articulation.update(sim.cfg.dt) jacobian = articulation.data.body_link_jacobian_w.torch diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 0a51fe36ddd..f77ad3a06e0 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -7,10 +7,11 @@ from isaaclab.app import AppLauncher -simulation_app = AppLauncher(headless=True).app +simulation_app = AppLauncher(headless=True, device="cpu").app import pytest import torch +import warp as wp from isaaclab_physx.assets import RigidObject import isaaclab.sim as sim_utils @@ -65,11 +66,15 @@ def test_rigid_object_real_physx_seams() -> None: 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 @@ -77,6 +82,17 @@ def test_rigid_object_real_physx_seams() -> None: 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) rigid_object.write_root_link_velocity_to_sim_index( root_velocity=torch.zeros((2, 6)), env_ids=torch.tensor([0, 1], dtype=torch.int32) 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 da796ac9730..1fde086389b 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py @@ -7,7 +7,7 @@ from isaaclab.app import AppLauncher -simulation_app = AppLauncher(headless=True).app +simulation_app = AppLauncher(headless=True, device="cpu").app import pytest import torch @@ -72,11 +72,16 @@ def test_rigid_object_collection_real_physx_seams() -> None: 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]) @@ -85,6 +90,8 @@ def test_rigid_object_collection_real_physx_seams() -> None: 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 @@ -93,3 +100,19 @@ def test_rigid_object_collection_real_physx_seams() -> None: 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 6214eca2ade..8f6bf030718 100644 --- a/source/isaaclab_physx/test/assets/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/test_surface_gripper.py @@ -3,296 +3,109 @@ # # 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 - -pytestmark = pytest.mark.integration - -# 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 +from isaaclab.sim.utils import enable_extension +enable_extension("isaacsim.robot.surface_gripper") -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) - - return grippable_object - - -@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 - - -@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,) +from usd.schema.isaac import robot_schema - # 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 +from isaacsim.robot.surface_gripper import create_surface_gripper +from pxr import Gf, Sdf, UsdGeom, UsdPhysics - # 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. +import isaaclab.sim as sim_utils +from isaaclab.sim import build_simulation_context - 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. +pytestmark = pytest.mark.integration - .. 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 +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 index fe9e48e777e..e5c8e5ec03b 100644 --- a/source/isaaclab_physx/test/assets/unit/_imports.py +++ b/source/isaaclab_physx/test/assets/unit/_imports.py @@ -12,9 +12,18 @@ from types import ModuleType from unittest.mock import patch +_MISSING = object() + def import_physx_module(module_name: str): """Import a PhysX asset module while replacing only its unavailable Kit boundary.""" + module_parts = module_name.split(".") + 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] + previous_target = sys.modules.pop(module_name, _MISSING) cloner = ModuleType("isaaclab_physx.cloner") cloner.queue_physx_replication = lambda cfg: None physics = ModuleType("isaaclab_physx.physics") @@ -31,5 +40,16 @@ def import_physx_module(module_name: str): stubs.update({"omni.physics": omni_physics, "omni.physics.tensors": omni_tensors}) with warnings.catch_warnings(): warnings.simplefilter("ignore", DeprecationWarning) - with patch.object(omni, "physics", omni_physics, create=True), patch.dict(sys.modules, stubs): - return importlib.import_module(module_name) + try: + with patch.object(omni, "physics", omni_physics, create=True), patch.dict(sys.modules, stubs): + imported = importlib.import_module(module_name) + finally: + sys.modules.pop(module_name, None) + if previous_target is not _MISSING: + sys.modules[module_name] = previous_target + 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_actuator_control.py b/source/isaaclab_physx/test/assets/unit/test_actuator_control.py index 0f5bac347fa..d54e4641c74 100644 --- a/source/isaaclab_physx/test/assets/unit/test_actuator_control.py +++ b/source/isaaclab_physx/test/assets/unit/test_actuator_control.py @@ -6,11 +6,13 @@ """Focused PhysX dual actuator-dispatch tests.""" from types import SimpleNamespace -from unittest.mock import Mock +from unittest.mock import Mock, call import warp as wp from isaaclab_physx.assets.articulation.actuator_control import PhysxActuatorControl +from isaaclab.actuators import IdealPDActuatorCfg + class _RecordingView: def __init__(self) -> None: @@ -85,9 +87,10 @@ def test_submit_commands_uses_native_effort_and_public_targets_for_newton_path() def test_compute_native_actuators_refreshes_nonidentity_public_joint_state() -> None: """The native controller must observe the latest reordered PhysX joint state.""" - refresh_position = Mock() - refresh_velocity = Mock() - runtime = SimpleNamespace(compute=Mock()) + 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), @@ -100,6 +103,51 @@ def test_compute_native_actuators_refreshes_nonidentity_public_joint_state() -> assert control.compute_native_actuators(collection, 0.01) - refresh_position.assert_called_once_with() - refresh_velocity.assert_called_once_with() - runtime.compute.assert_called_once_with(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/unit/test_articulation.py b/source/isaaclab_physx/test/assets/unit/test_articulation.py index 82bfa483648..60a88c610c4 100644 --- a/source/isaaclab_physx/test/assets/unit/test_articulation.py +++ b/source/isaaclab_physx/test/assets/unit/test_articulation.py @@ -5,9 +5,7 @@ """Focused PhysX articulation staging, cache, friction, and kernel tests.""" -import sys -import warnings -from types import ModuleType, SimpleNamespace +from types import SimpleNamespace from unittest.mock import patch import numpy as np @@ -20,6 +18,8 @@ write_joint_state_data_kernel, ) +from ._imports import import_physx_module + def _selector(values: list[int], dtype: type) -> wp.array: """Create a CPU Warp selector with the requested integer width.""" @@ -28,32 +28,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]: diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py index 0a13482b6bb..2937a36348f 100644 --- a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py @@ -5,6 +5,8 @@ """Focused PhysX rigid-object CPU staging and cached-view tests.""" +import importlib +import sys from types import SimpleNamespace import numpy as np @@ -47,3 +49,23 @@ def test_tensor_api_float_view_is_cached_over_stable_pose_storage() -> None: 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" + stubbed_module = import_physx_module(module_name) + assert module_name not in sys.modules + + try: + fresh_module = importlib.import_module(module_name) + except ModuleNotFoundError as exc: + # Plain uv Python has no Kit ``carb`` module. Reaching that boundary proves the + # real lazy PhysX manager replaced the unit stub. + assert exc.name == "carb" + assert module_name not in sys.modules + else: + from isaaclab_physx.physics import PhysxManager + + assert fresh_module is not stubbed_module + assert fresh_module.SimulationManager is PhysxManager diff --git a/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py b/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py new file mode 100644 index 00000000000..746c3eed648 --- /dev/null +++ b/source/isaaclab_physx/test/assets/unit/test_surface_gripper.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 surface-gripper command, property, and device-guard 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 + + +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." + ) From 92c2f3054dc6616f40bfca362c57bde55f30ebb9 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 16:36:36 +0200 Subject: [PATCH 18/26] Harden PhysX test import isolation Restore every target-package module imported under lightweight stubs so transitive data classes cannot retain the fake manager. Keep the local SurfaceGripper integration visible to Isaac Sim's short-CI source selector. --- .../test/assets/test_surface_gripper.py | 1 + .../test/assets/unit/_imports.py | 41 +++++++++++++++---- .../test/assets/unit/test_rigid_object.py | 29 ++++++++----- 3 files changed, 53 insertions(+), 18 deletions(-) diff --git a/source/isaaclab_physx/test/assets/test_surface_gripper.py b/source/isaaclab_physx/test/assets/test_surface_gripper.py index 8f6bf030718..536143eae0d 100644 --- a/source/isaaclab_physx/test/assets/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/test_surface_gripper.py @@ -84,6 +84,7 @@ def _author_local_surface_gripper() -> SurfaceGripper: ) +@pytest.mark.isaacsim_ci 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: diff --git a/source/isaaclab_physx/test/assets/unit/_imports.py b/source/isaaclab_physx/test/assets/unit/_imports.py index e5c8e5ec03b..e037649295c 100644 --- a/source/isaaclab_physx/test/assets/unit/_imports.py +++ b/source/isaaclab_physx/test/assets/unit/_imports.py @@ -15,19 +15,32 @@ _MISSING = object() -def import_physx_module(module_name: str): - """Import a PhysX asset module while replacing only its unavailable Kit boundary.""" +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] - previous_target = sys.modules.pop(module_name, _MISSING) + 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 = type("PhysxManager", (), {}) + physics.PhysxManager = simulation_manager or type("PhysxManager", (), {}) stubs = {"isaaclab_physx.cloner": cloner, "isaaclab_physx.physics": physics} import omni @@ -38,15 +51,27 @@ def import_physx_module(module_name: str): 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), patch.dict(sys.modules, stubs): + with patch.object(omni, "physics", omni_physics, create=True): + sys.modules.update(stubs) imported = importlib.import_module(module_name) finally: - sys.modules.pop(module_name, None) - if previous_target is not _MISSING: - sys.modules[module_name] = previous_target + 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(): diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py index 2937a36348f..c26e86a669e 100644 --- a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py @@ -5,7 +5,6 @@ """Focused PhysX rigid-object CPU staging and cached-view tests.""" -import importlib import sys from types import SimpleNamespace @@ -54,18 +53,28 @@ def test_tensor_api_float_view_is_cached_over_stable_pose_storage() -> None: 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) - assert module_name not in sys.modules + 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: - fresh_module = importlib.import_module(module_name) + from isaaclab_physx.physics import PhysxManager except ModuleNotFoundError as exc: - # Plain uv Python has no Kit ``carb`` module. Reaching that boundary proves the - # real lazy PhysX manager replaced the unit stub. assert exc.name == "carb" - assert module_name not in sys.modules - else: - from isaaclab_physx.physics import PhysxManager + 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 is not stubbed_module + assert fresh_module.SimulationManager is PhysxManager + assert fresh_module.RigidObjectData.__init__.__globals__["SimulationManager"] is PhysxManager From 188b6e6523d53fb513869331c738399520a54916 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 17:10:26 +0200 Subject: [PATCH 19/26] Split OVPhysX asset tests by scope Move helper and kernel coverage into fast unit modules and replace remote asset matrices with focused local solver seams. Cover fused collection layouts, native actuator adaptation, deformable lifecycle, and CPU staging while retaining mixed-device behavior. --- .../test/assets/test_articulation.py | 3368 +---------------- .../test/assets/test_deformable_object.py | 793 +--- .../test/assets/test_rigid_object.py | 1292 +------ .../assets/test_rigid_object_collection.py | 985 +---- .../isaaclab_ov/test/assets/unit/__init__.py | 6 + .../test/assets/unit/test_actuator_control.py | 204 + .../test/assets/unit/test_articulation.py | 110 + .../{ => unit}/test_articulation_helpers.py | 21 - .../{ => unit}/test_articulation_kernels.py | 0 .../test_deformable_object.py} | 0 .../{ => unit}/test_deformable_views.py | 0 .../test_rigid_object.py} | 0 .../unit/test_rigid_object_collection.py | 190 + 13 files changed, 831 insertions(+), 6138 deletions(-) create mode 100644 source/isaaclab_ov/test/assets/unit/__init__.py create mode 100644 source/isaaclab_ov/test/assets/unit/test_actuator_control.py create mode 100644 source/isaaclab_ov/test/assets/unit/test_articulation.py rename source/isaaclab_ov/test/assets/{ => unit}/test_articulation_helpers.py (84%) rename source/isaaclab_ov/test/assets/{ => unit}/test_articulation_kernels.py (100%) rename source/isaaclab_ov/test/assets/{test_deformable_object_helpers.py => unit/test_deformable_object.py} (100%) rename source/isaaclab_ov/test/assets/{ => unit}/test_deformable_views.py (100%) rename source/isaaclab_ov/test/assets/{test_rigid_object_helpers.py => unit/test_rigid_object.py} (100%) create mode 100644 source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index 8f129e2eb95..5dd99932912 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -3,3313 +3,153 @@ # # 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``). - -""" +"""Minimal real-OVPhysX integration coverage for articulations.""" from __future__ import annotations -import importlib -import sys from pathlib import Path -from unittest.mock import Mock import pytest import torch import warp as wp -from pxr import Usd, UsdGeom, 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, -) +from pxr import UsdPhysics -# 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.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg # noqa: E402 +from isaaclab.assets import ArticulationCfg # 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() - -_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." -) - - -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, - ) - - 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) - - -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, - ) - - 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", - ], +from isaaclab.test.utils.articulation_ordering import ( # noqa: E402 + BRANCHING_MJWARP_BODY_NAMES, + BRANCHING_MJWARP_JOINT_NAMES, ) -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, - ) - - -@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 - - 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]) -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)) - ) - 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, - 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 - 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, - ) - 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.permanent_wrench_composer.set_forces_and_torques_index( - forces=force_b, torques=torque_b, body_ids=body_ids - ) - - # 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() +pytestmark = pytest.mark.integration - # 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) +_FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" -@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", - ) +def _sim_context(device: str = "cpu", *, use_newton_actuators: bool = False): + """Build a local CPU 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, ) - 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", +def _spawn_ordered_articulation(*, native_actuator: bool = False) -> Articulation: + """Spawn the cached local branching fixture in nonidentity public order.""" + 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) ) - 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 - - # 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), - ) - - # 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)) - torch.testing.assert_close( - data.gravity_compensation_forces.torch, - _read_binding_to_torch(articulation, TT.GRAVITY_FORCE, device), - ) - - # 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={}, + spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), + actuators={"joints": actuator_cfg}, 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], - ) + fixed_joint = UsdPhysics.FixedJoint.Define(sim_utils.get_current_stage(), "/World/Robot/fixed_root") + fixed_joint.GetBody1Rel().SetTargets(["/World/Robot/base"]) + return articulation -@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) +def test_articulation_real_ovphysx_seams() -> None: + """Prove ordering, partial state/properties, drive delivery, and dynamics access.""" + with _sim_context() as sim: + articulation = _spawn_ordered_articulation() + sim.reset() - # 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) + 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 + + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32) + target_position = torch.tensor([[0.21, -0.13]]) + target_velocity = torch.tensor([[0.41, -0.23]]) + expected_position = articulation.data.joint_pos.torch.clone() + expected_velocity = articulation.data.joint_vel.torch.clone() + expected_position[:, joint_ids] = target_position + expected_velocity[:, joint_ids] = target_velocity + articulation.write_joint_state_to_sim_index( + position=target_position, + velocity=target_velocity, + joint_ids=joint_ids, + ) + torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) + torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + + body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32) + masses = torch.tensor([[2.5, 3.5]]) + articulation.set_masses_index(masses=masses, body_ids=body_ids) + coms = articulation.data.body_com_pose_b.torch[:, body_ids].clone() + coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03]) + coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02]) + articulation.set_coms_index(coms=wp.from_torch(coms, dtype=wp.transformf), body_ids=body_ids) + inertias = articulation.data.body_inertia.torch[:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[0, 1, 4] *= 1.3 + articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_mass.torch[:, body_ids], masses) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) + torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) + + drive_target = articulation.data.joint_pos.torch.clone() + drive_target[:, 0] += 0.15 + articulation.actuators.target_command.set_position_index(value=drive_target, full_data=True) + articulation.write_data_to_sim() + backend_target = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)) + backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) + torch.testing.assert_close(backend_target, drive_target[:, backend_to_user]) - # Simulate physics - for _ in range(10): - # perform rendering sim.step() - # update articulation articulation.update(sim.cfg.dt) + jacobian = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobian.shape == (1, articulation.num_bodies - 1, 6, articulation.num_joints) + assert mass_matrix.shape == (1, articulation.num_joints, articulation.num_joints) + 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) -@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. +@pytest.mark.skipif(not torch.cuda.is_available(), reason="Native actuator wheel probe requires CUDA") +def test_articulation_native_actuator_submits_real_ovphysx_effort() -> None: + """Prove the local native controller reaches the real OVPhysX effort binding.""" + with _sim_context(device="cuda:0", use_newton_actuators=True) as sim: + articulation = _spawn_ordered_articulation(native_actuator=True) + sim.reset() - 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, - ) + 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 + articulation.actuators.target_command.set_position_index(value=target) 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"]) + raw_effort = wp.to_torch(articulation._physx_actuator_wrapper.joint_f_2d) + backend_effort = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_ACTUATION_FORCE)) + assert torch.any(raw_effort != 0.0) + torch.testing.assert_close(backend_effort, raw_effort) diff --git a/source/isaaclab_ov/test/assets/test_deformable_object.py b/source/isaaclab_ov/test/assets/test_deformable_object.py index 0bc028a1ee3..7d6fe394650 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 493b88bd608..e895efd6243 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object.py @@ -3,1259 +3,89 @@ # # 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``). -""" +"""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() - -_logger = logging.getLogger(__name__) - -def _ovphysx_sim_context(device: str, **kwargs): - """Wrapper around :func:`build_simulation_context` that injects OVPhysX cfg. +pytestmark = pytest.mark.integration - 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)), +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, ) - 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``.) -# --------------------------------------------------------------------------- - - -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 _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 - 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) - - # 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), - ) - - # 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), - ) - - # 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), - ) - - # 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 +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() + + 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) + + 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]) + + rigid_object.set_masses_index(masses=wp.array([[3.0]], dtype=wp.float32, device="cpu"), env_ids=[1]) + 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]) + 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]) + 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:]) - + rigid_object.update(sim.cfg.dt) -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"``. - - 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`. - - """ - 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 a630bb0a33d..1ad0f9f8d9e 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object_collection.py @@ -3,27 +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``). -""" +"""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 @@ -33,904 +20,96 @@ 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.integration -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 _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 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. - - 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() + 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) + 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]) + + 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) + torch.testing.assert_close(collection.data.body_com_pose_b.torch[env_ids][:, body_ids], coms) + + 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) + 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/unit/__init__.py b/source/isaaclab_ov/test/assets/unit/__init__.py new file mode 100644 index 00000000000..af986054b4f --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/__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 + +"""Focused unit and kernel tests for OVPhysX asset adapters.""" diff --git a/source/isaaclab_ov/test/assets/unit/test_actuator_control.py b/source/isaaclab_ov/test/assets/unit/test_actuator_control.py new file mode 100644 index 00000000000..025db8c9a78 --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/test_actuator_control.py @@ -0,0 +1,204 @@ +# 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 warp as wp + +from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg +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 + + +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_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_articulation.py b/source/isaaclab_ov/test/assets/unit/test_articulation.py new file mode 100644 index 00000000000..3f6ae1c5dbf --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/test_articulation.py @@ -0,0 +1,110 @@ +# 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 torch +import warp as wp +from pxr import Usd, UsdGeom, UsdPhysics + +from isaaclab.assets.articulation import ordering_kernels +from isaaclab.utils.warp.launch_cache import _WarpLaunchCache +from isaaclab_ov import tensor_types as TT +from isaaclab_ov.assets import Articulation +from isaaclab_ov.assets.articulation.articulation_data import ArticulationData + + +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_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 48122a8662c..5ca4fbac33c 100644 --- a/source/isaaclab_ov/test/assets/test_articulation_helpers.py +++ b/source/isaaclab_ov/test/assets/unit/test_articulation_helpers.py @@ -118,27 +118,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 100% rename from source/isaaclab_ov/test/assets/test_articulation_kernels.py rename to source/isaaclab_ov/test/assets/unit/test_articulation_kernels.py diff --git a/source/isaaclab_ov/test/assets/test_deformable_object_helpers.py b/source/isaaclab_ov/test/assets/unit/test_deformable_object.py similarity index 100% rename from source/isaaclab_ov/test/assets/test_deformable_object_helpers.py rename to source/isaaclab_ov/test/assets/unit/test_deformable_object.py 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 100% rename from source/isaaclab_ov/test/assets/test_deformable_views.py rename to source/isaaclab_ov/test/assets/unit/test_deformable_views.py diff --git a/source/isaaclab_ov/test/assets/test_rigid_object_helpers.py b/source/isaaclab_ov/test/assets/unit/test_rigid_object.py similarity index 100% rename from source/isaaclab_ov/test/assets/test_rigid_object_helpers.py rename to source/isaaclab_ov/test/assets/unit/test_rigid_object.py diff --git a/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py b/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py new file mode 100644 index 00000000000..c471bff7e6e --- /dev/null +++ b/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py @@ -0,0 +1,190 @@ +# 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") + +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) From 86a8cba74c8bde244c8baef438bc876af2dbda7d Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 17:28:39 +0200 Subject: [PATCH 20/26] Cover OVPhysX integration boundaries Verify inertial and friction setters against raw backend bindings. Prove implicit and native actuator commands move the real solver, and own CPU warmup and selective reset behavior with focused tests. --- .../test/assets/test_articulation.py | 74 +++++++++++++++++-- .../test/assets/test_rigid_object.py | 21 ++++++ .../assets/test_rigid_object_collection.py | 19 +++++ .../test/assets/unit/test_actuator_control.py | 17 ++++- .../test/assets/unit/test_articulation.py | 7 +- .../unit/test_rigid_object_collection.py | 5 +- .../physics/test_ovphysx_manager_lifecycle.py | 31 ++++++++ 7 files changed, 160 insertions(+), 14 deletions(-) diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index 5dd99932912..f46d166fe17 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -69,7 +69,13 @@ def _spawn_ordered_articulation(*, native_actuator: bool = False) -> Articulatio body_ordering="mjwarp", ) ) - fixed_joint = UsdPhysics.FixedJoint.Define(sim_utils.get_current_stage(), "/World/Robot/fixed_root") + stage = sim_utils.get_current_stage() + for joint_name in ("left_shoulder", "left_elbow", "right_shoulder", "right_elbow"): + drive = UsdPhysics.DriveAPI.Apply(stage.GetPrimAtPath(f"/World/Robot/{joint_name}"), "angular") + drive.CreateStiffnessAttr(5.0) + drive.CreateDampingAttr(0.5) + drive.CreateMaxForceAttr(100.0) + fixed_joint = UsdPhysics.FixedJoint.Define(stage, "/World/Robot/fixed_root") fixed_joint.GetBody1Rel().SetTargets(["/World/Robot/base"]) return articulation @@ -102,31 +108,73 @@ def test_articulation_real_ovphysx_seams() -> None: torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + backend_friction_before = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)).clone() + static_friction = torch.tensor([[0.9, 0.7]]) + dynamic_friction = torch.tensor([[0.4, 0.3]]) + viscous_friction = torch.tensor([[0.11, 0.22]]) + 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, + joint_ids=joint_ids, + ) + backend_joint_ids = torch.as_tensor(articulation.joint_ordering.user_to_backend_indices)[joint_ids] + expected_backend_friction = backend_friction_before.clone() + expected_backend_friction[:, backend_joint_ids, 0] = static_friction + expected_backend_friction[:, backend_joint_ids, 1] = dynamic_friction + expected_backend_friction[:, backend_joint_ids, 2] = viscous_friction + raw_backend_friction = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)) + torch.testing.assert_close(raw_backend_friction, expected_backend_friction) + body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32) + backend_body_ids = torch.as_tensor(articulation.body_ordering.user_to_backend_indices)[body_ids] + raw_mass_before = wp.to_torch(articulation.root_view.get_attribute(TT.BODY_MASS)).clone() masses = torch.tensor([[2.5, 3.5]]) articulation.set_masses_index(masses=masses, body_ids=body_ids) + expected_raw_mass = raw_mass_before.clone() + expected_raw_mass[:, backend_body_ids] = masses + 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[:, body_ids].clone() coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03]) coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02]) articulation.set_coms_index(coms=wp.from_torch(coms, dtype=wp.transformf), body_ids=body_ids) + expected_raw_com = raw_com_before.clone() + expected_raw_com[:, backend_body_ids] = coms + 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[:, body_ids].clone() inertias[0, 0, 0] *= 1.2 inertias[0, 1, 4] *= 1.3 articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) + expected_raw_inertia = raw_inertia_before.clone() + expected_raw_inertia[:, backend_body_ids] = inertias + 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[:, body_ids], masses) torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) + 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() drive_target = articulation.data.joint_pos.torch.clone() - drive_target[:, 0] += 0.15 + drive_target[:, 0] += 0.4 articulation.actuators.target_command.set_position_index(value=drive_target, full_data=True) articulation.write_data_to_sim() backend_target = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)) backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) torch.testing.assert_close(backend_target, drive_target[:, backend_to_user]) - sim.step() - articulation.update(sim.cfg.dt) + for _ in range(8): + sim.step() + articulation.update(sim.cfg.dt) + articulation.write_data_to_sim() + assert torch.any(torch.abs(articulation.data.joint_pos.torch[:, 0] - initial_drive_position) > 1e-6) jacobian = articulation.data.body_link_jacobian_w.torch mass_matrix = articulation.data.mass_matrix.torch assert jacobian.shape == (1, articulation.num_bodies - 1, 6, articulation.num_joints) @@ -146,10 +194,24 @@ def test_articulation_native_actuator_submits_real_ovphysx_effort() -> None: 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() - raw_effort = wp.to_torch(articulation._physx_actuator_wrapper.joint_f_2d) + 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) + torch.testing.assert_close(backend_effort, raw_effort[:, backend_to_user]) + + for _ in range(8): + sim.step() + articulation.update(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( + wp.to_torch(articulation.root_view.get_attribute(TT.DOF_ACTUATION_FORCE)), + recomputed_effort[:, backend_to_user], + ) diff --git a/source/isaaclab_ov/test/assets/test_rigid_object.py b/source/isaaclab_ov/test/assets/test_rigid_object.py index e895efd6243..7a55aeaa80f 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object.py @@ -13,6 +13,7 @@ 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 # noqa: E402 @@ -68,13 +69,33 @@ def test_rigid_object_real_ovphysx_seams() -> None: 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]) + 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 + ) + + 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 + ) + + 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 + ) 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) 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 1ad0f9f8d9e..b7e319cef17 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object_collection.py @@ -83,22 +83,41 @@ def test_rigid_object_collection_real_ovphysx_seams() -> None: 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( diff --git a/source/isaaclab_ov/test/assets/unit/test_actuator_control.py b/source/isaaclab_ov/test/assets/unit/test_actuator_control.py index 025db8c9a78..edb577e99e8 100644 --- a/source/isaaclab_ov/test/assets/unit/test_actuator_control.py +++ b/source/isaaclab_ov/test/assets/unit/test_actuator_control.py @@ -9,12 +9,12 @@ from unittest.mock import Mock, call import warp as wp - -from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg 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 + class _RecordingView: """Capture values written to each OVPhysX tensor type.""" @@ -174,6 +174,19 @@ def test_native_compute_refreshes_owned_state_before_controller() -> None: ] +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() diff --git a/source/isaaclab_ov/test/assets/unit/test_articulation.py b/source/isaaclab_ov/test/assets/unit/test_articulation.py index 3f6ae1c5dbf..d68f4876010 100644 --- a/source/isaaclab_ov/test/assets/unit/test_articulation.py +++ b/source/isaaclab_ov/test/assets/unit/test_articulation.py @@ -9,13 +9,14 @@ 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 -from isaaclab_ov import tensor_types as TT -from isaaclab_ov.assets import Articulation -from isaaclab_ov.assets.articulation.articulation_data import ArticulationData def test_joint_dof_sign_resolution_traverses_instance_proxies() -> None: diff --git a/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py b/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py index c471bff7e6e..ec55159d90f 100644 --- a/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py +++ b/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py @@ -21,6 +21,7 @@ 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 @@ -81,9 +82,7 @@ def test_instance_major_vector_layout_round_trips_through_body_major_binding(lib 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]] - ) + 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) 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 a3ff62031ce..c1eea198168 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py @@ -12,6 +12,7 @@ import textwrap from pathlib import Path from types import ModuleType, SimpleNamespace +from unittest.mock import Mock import pytest @@ -82,6 +83,36 @@ def test_cpu_runtime_construction_does_not_enable_sticky_cpu_mode(manager_module 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")], From d1794aed6f72bad33bc78e921d935c8d230ce3bc Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 18:00:38 +0200 Subject: [PATCH 21/26] Publish asset test redesign results Scope backend manager mocks to tests and classify real solver modules. Keep the fast Newton guard under target and document final speed-ups. --- ...08-21-asset-test-suite-redesign-results.md | 279 ++++++++++++++++++ .../isaaclab/test/assets/contract/__init__.py | 2 +- .../contract/_articulation_contract_cases.py | 3 - .../contract/_articulation_contract_utils.py | 14 +- .../test/assets/contract/_contract_boot.py | 2 +- .../assets/contract/_manager_patch_scope.py | 45 +++ ..._rigid_object_collection_contract_cases.py | 3 - ..._rigid_object_collection_contract_utils.py | 16 +- .../contract/_rigid_object_contract_cases.py | 3 - .../contract/_rigid_object_contract_utils.py | 14 +- .../test/assets/contract/capabilities.py | 2 +- .../isaaclab/test/assets/contract/conftest.py | 19 ++ .../test/assets/contract/public_surface.py | 2 +- .../contract/test_asset_contract_api.py | 80 ++++- .../contract/test_asset_contract_data.py | 2 +- .../contract/test_asset_contract_writes.py | 2 +- .../assets/test_newton_actuators_newton.py | 3 + .../assets/unit/test_rigid_assets_import.py | 20 +- 18 files changed, 465 insertions(+), 46 deletions(-) create mode 100644 docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-results.md create mode 100644 source/isaaclab/test/assets/contract/_manager_patch_scope.py create mode 100644 source/isaaclab/test/assets/contract/conftest.py 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 00000000000..9f70a6ae13f --- /dev/null +++ b/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-results.md @@ -0,0 +1,279 @@ + + +# Asset Test Suite Redesign Results + +## Outcome + +The comparable asset and WrenchComposer scopes now finish in **134.18 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,460 focused cases: 1,355 pass, 69 skip +with an explicit capability reason, and 36 are expected failures for explicit +Newton contract limitations. + +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: `0c676a15c8` 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.18s 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,185 pass, 69 skip, 36 xfail | 64.70 / 84.02 s | 3.80 / 18.26 s | **4.60x** | +| Newton assets, no cable/MPM | 6 / 644 | 15 / 58 pass | 590.94 / 608.44 s | 16.56 / 50.53 s | **12.04x** | +| PhysX assets | 7 / 486 | 12 / 41 pass | 236.80 / 255.30 s | 6.29 / 34.07 s | **7.49x** | +| OV assets | 9 / 492 | 12 / 56 pass | 196.94 / 220.05 s | 3.49 / 24.99 s | **8.81x** | +| WrenchComposer | 3 / 412 | 2 / 15 pass | 23.34 / 29.79 s | 2.50 / 6.33 s | **4.71x** | +| **Aggregate** | **33 / 6,365** | **47 / 1,355 pass, 69 skip, 36 xfail** | **1,112.72 / 1,197.60 s** | **32.64 / 134.18 s** | **8.93x** | + +Focused gate and ownership timings: + +| Gate or owner | Result | Pytest / wall | +|---|---:|---:| +| Shared contract, one process | 1,081 pass, 69 skip, 36 xfail | 5.36 / 6.81 s | +| Shared contract + adjacent units, file-isolated gate | 1,185 pass, 69 skip, 36 xfail | 3.80 / 18.26 s | +| Newton backend units/kernels, including executable kitless guard | 49 pass | 11.72 / 12.78 s | +| PhysX backend units | 33 pass | 1.72 / 2.84 s | +| OV backend units | 50 pass | 1.67 / 2.77 s | +| Newton minimal real integration | 4 files, 9 pass | 5.03 / 16.87 s | +| PhysX minimal real integration | 6 files, 8 pass | 4.43 / 20.62 s | +| OV minimal real integration | 4 files, 6 pass | 2.73 / 10.14 s | +| WrenchComposer real delivery | 1 file, 1 pass | 2.48 / 4.82 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`. This is intentional for the comparable scopes: for +example, `test_articulation.py` selects both the integration file and +`assets/unit/test_articulation.py`. Basenames are not unique identifiers. + +```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_actuator_control.py,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-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_actuator_control.py,test_articulation.py,test_articulation_helpers.py,test_articulation_kernels.py,test_deformable_object.py,test_deformable_views.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. +./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 must exclude `/assets/unit/`; otherwise colliding +basenames pull unit files into the result. + +```bash +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 reduced | Local floating/fixed articulation seams, partial state/property/wrench, drive, Jacobian, and mass matrix. FK, staging, and ordering moved to units; IK/OSC/gravity moved to 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 reduced | One local ordered CPU articulation seam and one CUDA dynamics smoke; property/order conversions moved to units. | +| `test_articulation_kernels.py` | Moved/expanded | `assets/unit/test_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 reduced | Local CPU and CUDA articulation seams with ordered state/properties, native actuation, Jacobian, and mass access. | +| `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_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 | 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 audit classifies every member declared by `AssetBase`, +`BaseRigidObject`, `BaseRigidObjectData`, `BaseRigidObjectCollection`, +`BaseRigidObjectCollectionData`, `BaseArticulation`, and +`BaseArticulationData` as covered, explicitly unsupported, or reasoned out of +scope. 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 are explicitly unsupported. Contract parameters stay + visible as reasoned skips rather than disappearing from collection. +- Contract fixtures without spatial tendons skip spatial-tendon data probes + with `No spatial tendons configured`. +- The 36 expected failures document Newton fixed-tendon writer/property gaps + and its position-only COM representation; they are not silent omissions. +- 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/test/assets/contract/__init__.py b/source/isaaclab/test/assets/contract/__init__.py index 2a3b8f1eaf1..0257e275963 100644 --- a/source/isaaclab/test/assets/contract/__init__.py +++ b/source/isaaclab/test/assets/contract/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/assets/contract/_articulation_contract_cases.py b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py index 5ddc57f0017..1467e56d75a 100644 --- a/source/isaaclab/test/assets/contract/_articulation_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py @@ -22,9 +22,6 @@ from ._articulation_contract_utils import BACKENDS, get_articulation from .capabilities import backend_parameters, contract_backend -pytestmark = pytest.mark.integration - - @pytest.fixture def articulation_iface(request): backend = request.getfixturevalue("backend") diff --git a/source/isaaclab/test/assets/contract/_articulation_contract_utils.py b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py index 4b5238cd7b4..248cbe178c4 100644 --- a/source/isaaclab/test/assets/contract/_articulation_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py @@ -20,6 +20,8 @@ from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg from isaaclab.utils.wrench_composer import WrenchComposer +from ._manager_patch_scope import patch_contract_manager + BACKENDS = available_backends("api") BACKEND_UNAVAILABLE_REASONS = backend_unavailable_reasons() @@ -29,11 +31,6 @@ from isaaclab_physx.physics import PhysxManager as SimulationManager from isaaclab_physx.test.fixtures.views import MockArticulationViewWarp as PhysXMockArticulationViewWarp - # PhysX data classes need gravity even though contract 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) - 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 @@ -59,6 +56,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)] @@ -356,7 +358,7 @@ def create_newton_articulation( mock_manager.get_control.return_value = mock_control # Patch SimulationManager in the Newton data module - newton_data_module.SimulationManager = mock_manager + 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/contract/_contract_boot.py b/source/isaaclab/test/assets/contract/_contract_boot.py index f2d66e5af64..90991c955eb 100644 --- a/source/isaaclab/test/assets/contract/_contract_boot.py +++ b/source/isaaclab/test/assets/contract/_contract_boot.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/assets/contract/_manager_patch_scope.py b/source/isaaclab/test/assets/contract/_manager_patch_scope.py new file mode 100644 index 00000000000..774fc6b4f50 --- /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/contract/_rigid_object_collection_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py index 18bebf0c0c8..24c1fed2e0c 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py @@ -20,9 +20,6 @@ from ._rigid_object_collection_contract_utils import BACKENDS, get_rigid_object_collection from .capabilities import contract_backend -pytestmark = pytest.mark.integration - - @pytest.fixture def collection_iface(request): backend = request.getfixturevalue("backend") diff --git a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py index 13086a4ea69..7ad51281698 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py @@ -20,6 +20,8 @@ from isaaclab.assets.rigid_object_collection.rigid_object_collection_cfg import RigidObjectCollectionCfg from isaaclab.utils.wrench_composer import WrenchComposer +from ._manager_patch_scope import patch_contract_manager + BACKENDS = available_backends("api") if "physx" in BACKENDS: @@ -32,11 +34,6 @@ from isaaclab_physx.physics import PhysxManager as SimulationManager from isaaclab_physx.test.fixtures.views import MockRigidBodyViewWarp as PhysXMockRigidBodyViewWarp - # PhysX data classes need gravity even though contract 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) - if "newton" in BACKENDS: from isaaclab_newton.assets.rigid_object_collection.rigid_object_collection import ( RigidObjectCollection as NewtonRigidObjectCollection, @@ -64,6 +61,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)} @@ -160,8 +162,8 @@ def create_newton_rigid_object_collection( mock_manager.get_control.return_value = mock_control # Patch SimulationManager in both data and collection modules - newton_data_module.SimulationManager = mock_manager - newton_coll_module.SimulationManager = mock_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__) diff --git a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py index ae3477f0981..d4eb9428db3 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py @@ -20,9 +20,6 @@ from ._rigid_object_contract_utils import BACKENDS, get_rigid_object from .capabilities import contract_backend -pytestmark = pytest.mark.integration - - @pytest.fixture def rigid_object_iface(request): backend = request.getfixturevalue("backend") diff --git a/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py index 35937f8fbdb..a97501394c6 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py @@ -19,6 +19,8 @@ from isaaclab.assets.rigid_object.rigid_object_cfg import RigidObjectCfg from isaaclab.utils.wrench_composer import WrenchComposer +from ._manager_patch_scope import patch_contract_manager + BACKENDS = available_backends("api") if "physx" in BACKENDS: @@ -27,11 +29,6 @@ from isaaclab_physx.physics import PhysxManager as SimulationManager from isaaclab_physx.test.fixtures.views import MockRigidBodyViewWarp as PhysXMockRigidBodyViewWarp - # PhysX data classes need gravity even though contract 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) - 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 @@ -50,6 +47,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) @@ -155,7 +157,7 @@ def create_newton_rigid_object( mock_manager.get_control.return_value = mock_control # Patch SimulationManager in the Newton data module - newton_data_module.SimulationManager = mock_manager + patch_contract_manager(newton_data_module, "SimulationManager", mock_manager) data = NewtonRigidObjectData(mock_view, device) # Create RigidObject shell (bypass __init__) diff --git a/source/isaaclab/test/assets/contract/capabilities.py b/source/isaaclab/test/assets/contract/capabilities.py index bb62a60d38a..fc16356d250 100644 --- a/source/isaaclab/test/assets/contract/capabilities.py +++ b/source/isaaclab/test/assets/contract/capabilities.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/assets/contract/conftest.py b/source/isaaclab/test/assets/contract/conftest.py new file mode 100644 index 00000000000..b8dc619426b --- /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 index 458b2fa801b..63f3b22d588 100644 --- a/source/isaaclab/test/assets/contract/public_surface.py +++ b/source/isaaclab/test/assets/contract/public_surface.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_api.py b/source/isaaclab/test/assets/contract/test_asset_contract_api.py index 9a8ebb863a7..44ef3dd1e11 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_api.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_api.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -59,6 +59,84 @@ ) +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( diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_data.py b/source/isaaclab/test/assets/contract/test_asset_contract_data.py index 4b84f2c7494..e90df2dcf93 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_data.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_data.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_writes.py b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py index f5ffebdf1ed..ed46f673a5f 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_writes.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py index daaed45bbf4..5575a80af33 100644 --- a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py +++ b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py @@ -5,6 +5,7 @@ """Kitless real-solver equivalence test for Newton-native actuators.""" +import pytest import torch from isaaclab_newton.assets import Articulation from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg @@ -17,6 +18,8 @@ from isaaclab.assets import ArticulationCfg from isaaclab.sim import SimulationCfg, build_simulation_context +pytestmark = pytest.mark.integration + def _author_two_link_articulations() -> Articulation: """Author two local one-DOF articulations for actuator equivalence.""" 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 index b866875692f..5edf83b7da6 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -10,8 +10,6 @@ import sys from pathlib import Path -import pytest - _ASSET_TEST_DIR = Path(__file__).resolve().parents[1] _TARGETS = ( ("test_rigid_object.py", "test_rigid_object_real_newton_seams[cpu]"), @@ -32,8 +30,8 @@ _TARGET_FILENAMES = tuple(Path(target).name for target, _ in _TARGETS) + ("articulation_test_utils.py",) -def _run_monitored_target(target: Path, node: str, tmp_path: Path) -> subprocess.CompletedProcess[str]: - """Run a target under import and Nucleus sentinels.""" +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( """ @@ -81,7 +79,7 @@ def __add__(self, other): ) env = os.environ | {"PYTHONPATH": str(tmp_path)} return subprocess.run( - [sys.executable, "-m", "pytest", f"{target}::{node}", "-q"], + [sys.executable, "-m", "pytest", *nodes, "-q"], cwd=_ASSET_TEST_DIR, env=env, capture_output=True, @@ -90,10 +88,10 @@ def __add__(self, other): ) -@pytest.mark.parametrize(("target", "node"), _TARGETS) -def test_newton_asset_cpu_seam_runs_without_kit_isaacsim_or_nucleus(target: str, node: str, tmp_path: Path) -> None: - """Run each real CPU seam while rejecting Kit, IsaacSim, and Nucleus access.""" - result = _run_monitored_target(_ASSET_TEST_DIR / target, node, tmp_path) +def test_newton_asset_cpu_seams_run_without_kit_isaacsim_or_nucleus(tmp_path: Path) -> 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 @@ -112,7 +110,7 @@ def test_runtime_nucleus_access_inside_fixture_is_rejected(tmp_path: Path) -> No encoding="utf-8", ) - result = _run_monitored_target(mutated_target, "test_rigid_object_real_newton_seams[cpu]", tmp_path) + 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 @@ -131,7 +129,7 @@ def test_runtime_app_launcher_import_inside_fixture_is_rejected(tmp_path: Path) encoding="utf-8", ) - result = _run_monitored_target(mutated_target, "test_rigid_object_real_newton_seams[cpu]", tmp_path) + 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 From 8327f7cdeb34cccdb6ad5c3bb594aaca8efae424 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 18:05:09 +0200 Subject: [PATCH 22/26] Remove internal asset test plans Keep internal design and execution notes out of the review while retaining the measured results artifact. Make the published gate commands self-contained with their cache and EULA prerequisites. --- .../2026-08-21-asset-test-suite-redesign.md | 220 ------------- ...-08-21-asset-test-suite-redesign-design.md | 292 ------------------ ...08-21-asset-test-suite-redesign-results.md | 6 + 3 files changed, 6 insertions(+), 512 deletions(-) delete mode 100644 docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md delete mode 100644 docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md diff --git a/docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md b/docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md deleted file mode 100644 index 9b1c0edb4e2..00000000000 --- a/docs/superpowers/plans/2026-08-21-asset-test-suite-redesign.md +++ /dev/null @@ -1,220 +0,0 @@ - - -# Asset Test Suite Redesign Implementation Plan - -> **Spec:** `docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md` - -## Global Constraints - -- Work only in the `antoiner/asset-tests-redesign` worktree based on commit - `d7033a5a1a207f1d4284edb60d72d7838984413b` or its descendants. -- Use the worktree-local `env_isaaclab` through `./isaaclab.sh -p`. -- Apply strict red-green-refactor for production behavior changes. Record the - failing command and expected failure in each task report. -- Do not modify or add cable or MPM tests. -- Newton asset tests must not import `isaaclab.app`, launch Kit, use Nucleus, - or require remote assets. -- Shared contract tests own solver-common behavior. Backend tests own unique - implementation behavior and the smallest real integration proof. -- Keep one real wrench-delivery case per backend and retain real PhysX - property/view-order glue per asset family. -- Ordinary OV CPU scenes use per-scene CPU attributes, not sticky hard - CPU-only mode. Explicit wheel-level hard CPU-only operation remains valid. -- Preserve all supported public behavior; this project changes tests and one - OV manager lifecycle restriction, not asset public APIs. -- New test files use the 2026 SPDX header. Add one `.skip` changelog fragment - per test-only package and a patch fragment for `isaaclab_ov` if manager - behavior changes. -- Run focused tests after each task, `./isaaclab.sh -f` before every commit, - and the baseline scopes at the end. - -## Task 1: Allow OV CPU/GPU reuse in one process - -**Files:** - -- Modify `source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py`. -- Modify `source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py`. -- Add or modify a real lifecycle test under `source/isaaclab_ov/test/physics/`. -- Remove `device_split` only from the three OV asset modules in this project: - `test_articulation.py`, `test_rigid_object.py`, and - `test_rigid_object_collection.py`. - -**Red:** - -1. Add a unit test proving ordinary CPU construction never calls - `PhysX.set_cpu_mode(True)`. -2. Add unit tests proving CPU scenes author `enableGPUDynamics=false` and - `broadphaseType=MBP`, while GPU scenes author the GPU values. -3. Add a real CPU-CUDA-CPU lifecycle regression that creates a local cuboid - rigid object, writes/reads state, advances it, and asserts tensor placement. -4. Verify the unit tests fail due the current sticky call and attributes, and - the real test fails at `_locked_device`. - -**Green:** remove `_locked_device` and its error path, stop calling sticky CPU -mode for ordinary CPU scenes, author both CPU and GPU attributes explicitly, -and update lifecycle documentation. Remove the asset `device_split` markers -only after the real lifecycle regression passes. - -**Verify:** run the manager lifecycle unit file, the new real lifecycle test, -then the three OV asset files in a single unsplit invocation. - -## Task 2: Establish focused shared contract infrastructure - -**Files:** - -- Rename `_iface_test_boot.py` and the three `_..._iface_test_utils.py` helpers - under `source/isaaclab/test/assets/` to contract terminology. -- Replace the five current `*iface*.py` test modules with focused API, data, - and write contract modules under `source/isaaclab/test/assets/contract/`. -- Add a capability declaration and public-surface classification helper in the - same test-only package. - -**Behavior:** preserve existing meaningful assertions while eliminating the - environment/body/joint/device Cartesian matrix. Use CPU `N=2`, `B=3`, `J=4` - as the canonical case, targeted singleton/empty/order cases, and one CUDA - smoke per distinct device path. Make backend availability and unsupported - behavior explicit. - -**Meta-test:** enumerate public members declared by `AssetBase`, -`BaseRigidObject`, `BaseRigidObjectData`, `BaseRigidObjectCollection`, -`BaseRigidObjectCollectionData`, `BaseArticulation`, and -`BaseArticulationData`. Fail with the unclassified names unless each member is -mapped to an API/data/write contract, explicitly unsupported, or documented as -out of scope. - -**Verify:** first demonstrate the meta-test catches one intentionally omitted -member, restore the classification, then run the complete shared contract -directory and ordering/selector unit files. Compare collected cases and wall -time to the 84.02-second shared baseline. - -## Task 3: Make WrenchComposer coverage focused - -**Files:** - -- Refactor `source/isaaclab/test/utils/test_wrench_composer.py`. -- Consolidate the unique real behavior from - `test_wrench_composer_integration.py` and - `test_wrench_composer_vs_physx.py` into one minimal integration module. -- Update backend integration modules as required to retain one real delivery - case per backend. - -**Behavior:** keep arithmetic, accumulation, clearing, frame conversion, -permanent-versus-instantaneous, selection, and validation in direct unit tests. -Use literal expected wrenches on tiny tensors. Keep one rotated force-at-position -real parity case because it covers both force and induced torque. Delete broad -multi-size/device/long-run duplication only after mapping every removed case to -the retained unit or integration assertion. - -**Verify:** run the focused unit and integration modules, record case mapping, -and compare wall time to the 29.79-second WrenchComposer baseline. - -## Task 4: Convert Newton rigid assets to kitless local fixtures - -**Files:** - -- Refactor `source/isaaclab_newton/test/assets/test_rigid_object.py`. -- Refactor `source/isaaclab_newton/test/assets/test_rigid_object_collection.py`. -- Add focused unit modules under `source/isaaclab_newton/test/assets/unit/` for - model-index mapping, staging, cache invalidation, and wrench kernels. - -**Red:** add a subprocess import/collection test that fails if either target -module loads `isaaclab.app`, `isaacsim`, or a Nucleus asset. Add focused unit -tests before extracting unique logic from real scenes. - -**Green:** use `SimulationContext` and local primitive/generated USD authoring, -remove `AppLauncher` and Nucleus imports, reduce matrices to canonical cases, -and consolidate real tests so a scene is authored once per module where reset -is proven safe. - -**Verify:** run the import/collection guard without IsaacSim modules, the unit -directory, and both real files. Compare to the 134.24-second combined baseline. - -## Task 5: Convert Newton articulation and actuator tests to kitless coverage - -**Files:** - -- Refactor `source/isaaclab_newton/test/assets/test_articulation.py`. -- Refactor `source/isaaclab_newton/test/assets/test_newton_actuators_newton.py`. -- Extend Newton package-local unit/kernel modules for FK invalidation, joint - staging, model notifications, ordering, and actuator adaptation. - -**Red:** extend the kitless collection guard to these modules and add focused -tests for every extracted unique behavior. Verify failures before production -or fixture changes. - -**Green:** replace remote robots with the smallest locally authored -articulation fixtures, remove AppLauncher, move actuator calculations that do -not require a model into unit tests, and keep one real actuator equivalence, -one partial root/joint roundtrip, one property read, one wrench delivery, and -supported Jacobian/mass access. - -**Verify:** run Newton unit/kernel tests and the two real modules; compare to -the 470.32-second combined baseline. Run all Newton in-scope asset tests and -prove no Kit process starts. - -## Task 6: Extract unique PhysX unit coverage and trim integration duplication - -**Files:** - -- Add `source/isaaclab_physx/test/assets/unit/` modules for articulation, - rigid object, rigid-object collection, deformable, and actuator helpers. -- Reduce the existing real asset modules without splitting them into more - solver-starting files. - -**Unit behavior:** view index conversion, ordering reshape, partial CPU -staging, writer cache invalidation, friction/inertial mapping, dual actuator -dispatch, deformable surface/volume detection, material fallback, kinematic -target validation, and supported kernels. - -**Integration behavior:** retain one local scene per asset family. Prove state -write/read, one real wrench, model-property setter acceptance, nontrivial -collection ordering, articulation Jacobian/mass access, and the smallest stable -surface/volume deformable probes. Move controller and termination behavior to -their owning suites rather than retaining them as asset integration. - -**Verify:** run new units first, then real files. Confirm the deformable module -is no longer an all-skipped startup. Compare to the 255.30-second baseline. - -## Task 7: Extract unique OV unit coverage and trim integration duplication - -**Files:** - -- Organize the existing OV helper/kernel modules under - `source/isaaclab_ov/test/assets/unit/` and add missing focused coverage for - view adapters, fused layouts, staging, derived data, deformable helpers, and - cache invalidation. -- Reduce the three main real rigid/articulation modules and the deformable real - module while keeping local solver proofs. - -**Behavior:** run the full contract in one process across CPU and CUDA. Keep -one state/property/wrench integration per supported family and wheel-only -capability probes that mocks cannot validate. Never cache live views across a -manager close; add repeated-run/order tests for fixture isolation. - -**Verify:** run OV unit/kernel tests, the mixed-device lifecycle regression, -and consolidated real asset tests. Compare to the 220.05-second baseline. - -## Task 8: Publish coverage mapping and final performance result - -**Files:** - -- Add one test-only changelog fragment for each test-only touched package and a - patch fragment for the OV manager behavior change. -- Add a before/after report beside the design specification. -- Update test-runner selections or documentation only where needed to expose - the fast contract/unit gate and minimal integration gate. - -**Report:** list every old module and case family as retained, moved, replaced, -removed as redundant, newly covered, or excluded. Include exact commands, -environment, collected/pass/skip counts, test time, wall time, and the ratio to -the 1,197.60-second baseline. Call out unsupported capabilities explicitly. - -**Final verification:** run every focused suite, all five baseline-scope -commands with updated filenames, repeated/order-sensitive fixture checks, -`./isaaclab.sh -f`, and `git diff --check`. The target is at least a threefold -wall-time improvement and under 30 seconds per warmed contract/unit backend. diff --git a/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md b/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md deleted file mode 100644 index 2c7049665c6..00000000000 --- a/docs/superpowers/specs/2026-08-21-asset-test-suite-redesign-design.md +++ /dev/null @@ -1,292 +0,0 @@ - - -# Fast Asset Test Suite Redesign - -**Status:** Approved for implementation - -## Objective - -Make the asset tests in `isaaclab`, `isaaclab_newton`, `isaaclab_physx`, and -`isaaclab_ov` fast and thorough by separating four responsibilities: - -1. shared backend contract conformance; -2. solver-independent unit and kernel behavior; -3. backend-specific unit and kernel behavior; -4. minimal real-solver integration. - -The redesign must preserve meaningful coverage while sharply reducing Kit and -solver startup, scene construction, model construction, redundant parameter -matrices, and duplicate WrenchComposer physics scenarios. Newton tests must not -launch Kit. Cable and MPM assets are excluded from this work. - -## Baseline - -The baseline was collected before implementation in a fresh worktree and local -uv environment: - -- branch: `antoiner/asset-tests-redesign`; -- base: `d7033a5a1a207f1d4284edb60d72d7838984413b` from latest - `origin/develop` on 2026-08-21; -- environment: `env_isaaclab`, created 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, and OVPhysX 0.5.10; -- NVIDIA GeForce RTX 5090, driver 590.48.01, 32607 MiB; -- every test file was launched in its own process through the repository test - orchestrator, matching CI behavior. - -| Scope | Files | Cases | Result | Test time | Wall time | -|---|---:|---:|---:|---:|---:| -| Shared asset and interface tests | 8 | 4,331 | 8/8 files passed | 64.70 s | 84.02 s | -| Newton assets, excluding cable and MPM | 6 | 644 | 6/6 files passed | 590.94 s | 608.44 s | -| PhysX assets | 7 | 486 | 7/7 files passed | 236.80 s | 255.30 s | -| OV assets | 9 | 492 | 9/9 files passed | 196.94 s | 220.05 s | -| WrenchComposer | 3 | 412 | 3/3 files passed | 23.34 s | 29.79 s | -| **Total** | **33** | **6,365** | **33/33 files passed** | **1,112.72 s** | **1,197.60 s** | - -The dominant costs are concentrated rather than evenly distributed: - -| File group | Wall time | -|---|---:| -| Shared articulation interface | 41.53 s | -| Shared articulation ordering interface | 18.73 s | -| Newton articulation | 383.25 s | -| Newton actuators | 87.07 s | -| Newton rigid-object collection | 67.36 s | -| Newton rigid object | 66.88 s | -| PhysX articulation | 103.92 s | -| PhysX rigid-object collection | 56.52 s | -| PhysX rigid object | 43.19 s | -| PhysX actuators | 37.00 s | -| OV articulation | 119.74 s | -| OV rigid-object collection | 41.92 s | -| OV rigid object | 33.34 s | -| WrenchComposer integration and PhysX comparison | 23.47 s | - -The current PhysX deformable module costs 2.43 seconds of process startup but -skips all 12 collected cases. Direct kernel/helper files generally take less -than four seconds wall time and validate useful behavior without real scenes. - -### Reproduction - -Run these commands from the worktree with `OMNI_KIT_ACCEPT_EULA=YES`. Each -command writes its aggregate report under `tests/`. - -```bash -TEST_FILTER_PATTERN=/source/isaaclab/test/assets/ \ -TEST_INCLUDE_FILES=test_articulation_iface.py,test_articulation_ordering.py,test_articulation_ordering_iface.py,test_articulation_ordering_kernels.py,test_asset_selector_cache.py,test_iface_test_utils.py,test_rigid_object_collection_iface.py,test_rigid_object_iface.py \ -TEST_RESULT_FILE=baseline-assets-shared.xml \ -./isaaclab.sh -p -m pytest tools - -TEST_FILTER_PATTERN=/source/isaaclab_newton/test/assets/ \ -TEST_INCLUDE_FILES=test_articulation.py,test_articulation_ordering_kernels.py,test_newton_actuators_newton.py,test_rigid_object.py,test_rigid_object_collection.py,test_wrench_kernels.py \ -TEST_RESULT_FILE=baseline-assets-newton.xml \ -./isaaclab.sh -p -m pytest tools - -TEST_FILTER_PATTERN=/source/isaaclab_physx/test/assets/ \ -TEST_INCLUDE_FILES=test_articulation.py,test_articulation_kernels.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=baseline-assets-physx.xml \ -./isaaclab.sh -p -m pytest tools - -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_object_helpers.py,test_deformable_views.py,test_rigid_object.py,test_rigid_object_collection.py,test_rigid_object_helpers.py \ -TEST_RESULT_FILE=baseline-assets-ov.xml \ -./isaaclab.sh -p -m pytest tools - -TEST_FILTER_PATTERN=/source/isaaclab/test/utils/ \ -TEST_INCLUDE_FILES=test_wrench_composer.py,test_wrench_composer_integration.py,test_wrench_composer_vs_physx.py \ -TEST_RESULT_FILE=baseline-wrench-composer.xml \ -./isaaclab.sh -p -m pytest tools -``` - -## Test Taxonomy - -### Shared contract tests - -Rename the current `iface` concept to `contract`. The old name describes only -the original abstract-interface check, while the current files mix reflection, -data semantics, writer behavior, caching, ordering, and backend behavior. - -Place reusable contract definitions in `isaaclab` and keep thin, package-owned -backend runners. Split the contract into focused modules: - -- `test_asset_contract_api.py`: concrete implementations of abstract members, - public signatures, lifecycle, metadata, names, finders, and explicit - unsupported behavior; -- `test_asset_contract_data.py`: shapes, dtypes, devices, aliases, defaults, - derived frames, acceleration, timestamps, and cache invalidation; -- `test_asset_contract_writes.py`: root, body, and joint writers; full and - partial updates; index and mask selection; delegation; and invalid shapes. - -The shared contract is the single source of truth. Backend runners provide the -fixture or adapter needed to execute it and declare supported capabilities. A -contract case may be skipped only from an explicit capability declaration, -with a reason. Broad import-error catches and silently empty backend matrices -are not acceptable. - -Add a base-surface meta-test that enumerates the public members of the asset -base classes and requires every member to be classified as tested, explicitly -unsupported, or intentionally out of scope. This prevents new abstract or -public members from creating silent contract holes. - -Mocks are contract-test infrastructure, not pretend backend implementations. -Mock tests verify forwarding, selection, cache, validation, and failure -semantics without a solver, but they do not substitute for the thin real -backend integration checks. - -### Shared unit and kernel tests - -Keep solver-independent behavior in `isaaclab`. Unit tests directly exercise -small tensors and focused collaborators. Kernel tests launch kernels with tiny -inputs and no scene. - -This layer owns: - -- selector normalization and selector-cache behavior; -- name matching, ordering, gather, and scatter; -- frame and quaternion transforms; -- WrenchComposer arithmetic, accumulation, reset, and frame conversion; -- shape and dtype validation; -- common timestamp and lazy-cache behavior. - -The canonical matrix is deliberately small: CPU with `N=2`, `B=3`, and `J=4`; -one singleton or empty case where behavior differs; one nontrivial ordering -case; and one CUDA smoke for code that has a genuinely distinct device path. -Do not form Cartesian products over several environment counts, body counts, -devices, and backends. - -### Backend-specific unit and kernel tests - -Each backend owns tests for unique logic under its own `test/assets/unit/` -directory. These tests use direct tensors, light fakes, or backend objects that -do not require a real scene whenever possible. - -Newton owns model-index mapping, FK invalidation, model notification behavior, -joint staging, actuator adaptation, and Newton-specific wrench kernels. PhysX -owns view-index and ordering conversion, staging, cache behavior, PhysX -inertial/friction mapping, and supported deformable behavior. OV owns its view -adapters, staging, derived data, deformable helpers, device configuration, and -manager lifecycle. Cable and MPM logic are explicitly excluded. - -Kernel tests are first-class unit tests. They should use the smallest input -that covers selector/order/scatter/gather/cache/frame/staging behavior, plus one -CUDA smoke only where the kernel has a real device-specific execution path. - -### Integration tests - -Integration tests prove that the adapters connect to a real solver; they do not -repeat the full contract or arithmetic matrix. Consolidate each backend's -integration cases into as few files as practical because the CI orchestrator -starts a fresh process per file. - -The minimum real-solver set per supported backend is: - -1. initialize a small local scene without Nucleus assets; -2. write and read back a partial root/body/joint state; -3. read mass, center of mass, and inertia; -4. deliver one real external wrench through the backend; -5. exercise one joint drive or actuator path; -6. smoke-test Jacobian or mass-matrix access when supported. - -WrenchComposer math remains in shared unit tests. Keep only one real delivery -case per backend integration suite. Remove the redundant broad comparison -matrix after the retained cases demonstrate both the composer's math and each -backend's delivery glue. - -The wrench case does not replace backend-only model-property integration. -PhysX must retain one real case per rigid object, rigid-object collection, and -articulation family that exercises mass, center-of-mass, or inertia setters. -The collection case must use nontrivial environment/body selection so it also -proves the real view-order remapping. Unit tests cover the complete selection -matrix; integration proves the TensorAPI accepts the translated request. - -Treat deformables as a separate capability family rather than forcing them -through the rigid-asset matrix. PhysX must cover surface-versus-volume -detection, material fallback, volume kinematic targets, and rejection of -unsupported surface kinematic targets through focused unit tests plus the -smallest stable real GPU probes. Replace the currently all-skipped module with -working coverage or remove its empty startup cost; do not count collected but -unconditionally skipped cases as coverage. - -## Scene and Model Reuse - -Cache immutable authored scene descriptions, small local USD fixtures, asset -configuration, and topology. Reuse a live scene within a consolidated test -module only when reset semantics are part of the fixture contract and every -test restores mutable state. - -Do not globally cache live Newton models or OV views merely to avoid setup. -They contain solver-owned mutable state and lifecycle coupling. Prefer cheap -reconstruction from cached authoring or topology. Where a backend exposes an -explicit safe reset, measure reuse and retain it only if isolation tests prove -that order and repetition do not change results. - -Newton integration must run through its kitless path. Remove `AppLauncher`, -Nucleus checks, and remote assets from Newton asset tests. Use local primitive -or generated fixtures and `SimulationContext` directly. - -## OV CPU/GPU Execution - -OVPhysX 0.5.10 documents that CPU and GPU simulation are selected per scene by -`physxScene:enableGPUDynamics` and `physxScene:broadphaseType`. Its explicit -hard CPU-only mode is process-global and sticky, but that mode is intended for -the no-CUDA-touch guarantee rather than for every CPU scene. - -IsaacLab currently calls `PhysX.set_cpu_mode(True)` for a CPU manager and keeps -a process-lifetime `_locked_device`, so the present `device_split` markers are -an IsaacLab restriction rather than an OVPhysX requirement. A local real-scene -probe against OVPhysX 0.5.10 passed both GPU-CPU-GPU and CPU-GPU-CPU sequences -in one process after bypassing only those two IsaacLab restrictions. Each -sequence created a cuboid and rigid object, produced tensors on the requested -device, authored the expected GPU-dynamics value, and advanced consistently -under gravity. The unmodified manager reproduced its own `_locked_device` -failure on the second context. - -Remove the lock and stop mapping an ordinary CPU scene to sticky hard CPU-only -mode. Author CPU scenes with `physxScene:enableGPUDynamics=false` and -`physxScene:broadphaseType="MBP"`, and author GPU scenes with the corresponding -GPU values. Preserve the wheel's explicit environment/configuration path for -hosts that require the no-CUDA-touch guarantee. Add a tracked lifecycle -regression that writes, reads, and advances CPU-GPU-CPU scenes in one process, -plus focused unit tests for the manager call and scene attributes. Then remove -the `device_split` markers and module-level split workarounds. - -## Coverage Gates - -The fast presubmit gate consists of shared contract tests plus shared and -backend-specific unit/kernel tests. The minimal integration gate runs the -consolidated backend files. Broader solver, controller, or long-horizon -behavior belongs to its owning suite or a scheduled job. - -The redesign is complete only when: - -- contract/unit tests run in under 30 seconds per backend after warmup; -- the selected asset and WrenchComposer wall time is at least three times - faster than the 1,197.60-second baseline on the reference machine; -- the base-surface meta-test has no unclassified public members; -- no retained backend file is entirely skipped; -- Newton asset tests import and run without Kit or Nucleus; -- every supported backend passes the minimal real integration set; -- test-order and repeated-run checks prove fixture isolation; -- the before/after report lists cases moved, consolidated, removed as - redundant, or newly added, so speedups are not obtained by hidden coverage - loss. - -## Migration Sequence - -1. Add the contract inventory/meta-test and focused shared contract modules. -2. Move pure shared behavior and WrenchComposer arithmetic into focused unit - and kernel tests. -3. Add package-local backend unit/kernel tests for unique logic. -4. Convert Newton assets to local, kitless integration fixtures. -5. Validate and, if supported, remove the OV process device split while - retaining explicit hard CPU-only behavior. -6. Consolidate minimal backend integration cases and delete only proven - duplicate matrices. -7. Run the exact baseline scopes, publish the before/after coverage mapping, - and add one test-only changelog fragment per touched package. 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 index 9f70a6ae13f..1807d757407 100644 --- 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 @@ -126,6 +126,9 @@ 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 \ @@ -138,6 +141,9 @@ Real-only subprocess gates must exclude `/assets/unit/`; otherwise colliding basenames pull unit files into the result. ```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 \ From 56dc2d150f09e32ce23a0e4521a2f2e327c89d39 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Fri, 21 Aug 2026 18:45:27 +0200 Subject: [PATCH 23/26] Strengthen asset test contracts Make fast suites jointly collectable and marker-selectable, replace no-op writer seams with observable state checks, and declare backend limitations explicitly. Keep retained integration fixtures local and hardware-aware. --- ...08-21-asset-test-suite-redesign-results.md | 65 ++++--- .../contract/_articulation_contract_cases.py | 184 +++++++++++++++--- .../contract/_articulation_contract_utils.py | 56 +++++- ..._rigid_object_collection_contract_cases.py | 91 ++++++++- ..._rigid_object_collection_contract_utils.py | 38 +++- .../contract/_rigid_object_contract_cases.py | 123 +++++++++++- .../contract/_rigid_object_contract_utils.py | 38 +++- .../test/assets/contract/capabilities.py | 39 +++- .../test/assets/contract/public_surface.py | 59 +----- .../contract/test_asset_contract_api.py | 31 ++- .../contract/test_asset_contract_data.py | 2 + .../contract/test_asset_contract_writes.py | 4 + .../utils/test_wrench_composer_integration.py | 11 +- .../test_articulation_ordering_kernels.py | 2 + .../test/assets/unit/__init__.py | 4 - .../assets/unit/test_articulation_fk_cache.py | 3 + .../unit/test_articulation_joint_staging.py | 3 + .../assets/unit/test_articulation_ordering.py | 3 + .../unit/test_newton_actuator_adaptation.py | 2 + .../assets/unit/test_rigid_assets_import.py | 4 + ...t_rigid_object_collection_model_indices.py | 2 + .../assets/unit/test_rigid_object_fk_cache.py | 2 + .../test_rigid_object_inertial_staging.py | 3 + .../test_rigid_object_setter_notifications.py | 2 + .../test/assets/unit/test_wrench_kernels.py | 3 + .../isaaclab_ov/test/assets/unit/__init__.py | 6 - .../assets/unit/test_articulation_helpers.py | 2 + .../assets/unit/test_articulation_kernels.py | 2 + .../test/assets/unit/test_deformable_views.py | 2 + ...ol.py => test_ovphysx_actuator_control.py} | 3 + ...lation.py => test_ovphysx_articulation.py} | 3 + ...t.py => test_ovphysx_deformable_object.py} | 2 + ...object.py => test_ovphysx_rigid_object.py} | 2 + ...> test_ovphysx_rigid_object_collection.py} | 2 + .../physics/test_ovphysx_manager_lifecycle.py | 2 + .../test/assets/unit/__init__.py | 6 - .../test/assets/unit/test_actuator_control.py | 3 + .../test/assets/unit/test_articulation.py | 3 +- .../assets/unit/test_deformable_object.py | 3 +- .../test/assets/unit/test_rigid_object.py | 4 +- .../unit/test_rigid_object_collection.py | 4 +- .../test/assets/unit/test_surface_gripper.py | 2 + 42 files changed, 660 insertions(+), 165 deletions(-) delete mode 100644 source/isaaclab_newton/test/assets/unit/__init__.py delete mode 100644 source/isaaclab_ov/test/assets/unit/__init__.py rename source/isaaclab_ov/test/assets/unit/{test_actuator_control.py => test_ovphysx_actuator_control.py} (99%) rename source/isaaclab_ov/test/assets/unit/{test_articulation.py => test_ovphysx_articulation.py} (98%) rename source/isaaclab_ov/test/assets/unit/{test_deformable_object.py => test_ovphysx_deformable_object.py} (99%) rename source/isaaclab_ov/test/assets/unit/{test_rigid_object.py => test_ovphysx_rigid_object.py} (98%) rename source/isaaclab_ov/test/assets/unit/{test_rigid_object_collection.py => test_ovphysx_rigid_object_collection.py} (99%) delete mode 100644 source/isaaclab_physx/test/assets/unit/__init__.py 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 index 1807d757407..0a38ebbff68 100644 --- 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 @@ -9,12 +9,11 @@ SPDX-License-Identifier: BSD-3-Clause ## Outcome -The comparable asset and WrenchComposer scopes now finish in **134.18 s** of -subprocess wall time, down from **1,197.60 s**: an **8.93x wall-time speed-up**. +The comparable asset and WrenchComposer scopes now finish in **143.67 s** of +subprocess wall time, down from **1,197.60 s**: an **8.34x wall-time speed-up**. The test-runner model is unchanged: every selected file runs in a fresh -subprocess. The final scopes collect 1,460 focused cases: 1,355 pass, 69 skip -with an explicit capability reason, and 36 are expected failures for explicit -Newton contract limitations. +subprocess. The final scopes collect 1,511 focused cases: 1,408 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 @@ -24,7 +23,7 @@ deliberately excluded. ## Measurement environment -- Final branch base: `0c676a15c8` from `origin/develop`, rebased before the +- Final branch base: `0451444c24` from `origin/develop`, rebased before the final measurements. - Original baseline base: `d7033a5a1a207f1d4284edb60d72d7838984413b`. - Worktree-local environment: `env_isaaclab`, installed with @@ -40,33 +39,33 @@ deliberately excluded. ## Copy-ready PR performance section The asset-test redesign reduced the five comparable CI-style scopes from -19m57.60s to 2m14.18s wall time (**8.93x faster**). This comparison uses the +19m57.60s to 2m23.67s wall time (**8.34x 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,185 pass, 69 skip, 36 xfail | 64.70 / 84.02 s | 3.80 / 18.26 s | **4.60x** | -| Newton assets, no cable/MPM | 6 / 644 | 15 / 58 pass | 590.94 / 608.44 s | 16.56 / 50.53 s | **12.04x** | -| PhysX assets | 7 / 486 | 12 / 41 pass | 236.80 / 255.30 s | 6.29 / 34.07 s | **7.49x** | -| OV assets | 9 / 492 | 12 / 56 pass | 196.94 / 220.05 s | 3.49 / 24.99 s | **8.81x** | -| WrenchComposer | 3 / 412 | 2 / 15 pass | 23.34 / 29.79 s | 2.50 / 6.33 s | **4.71x** | -| **Aggregate** | **33 / 6,365** | **47 / 1,355 pass, 69 skip, 36 xfail** | **1,112.72 / 1,197.60 s** | **32.64 / 134.18 s** | **8.93x** | +| Shared assets | 8 / 4,331 | 6 / 1,238 pass, 103 skip | 64.70 / 84.02 s | 7.89 / 25.47 s | **3.30x** | +| Newton assets, no cable/MPM | 6 / 644 | 15 / 58 pass | 590.94 / 608.44 s | 17.00 / 56.02 s | **10.86x** | +| PhysX assets | 7 / 486 | 12 / 41 pass | 236.80 / 255.30 s | 6.13 / 32.32 s | **7.90x** | +| OV assets | 9 / 492 | 12 / 56 pass | 196.94 / 220.05 s | 3.55 / 25.22 s | **8.73x** | +| WrenchComposer | 3 / 412 | 2 / 15 pass | 23.34 / 29.79 s | 0.81 / 4.64 s | **6.42x** | +| **Aggregate** | **33 / 6,365** | **47 / 1,408 pass, 103 skip** | **1,112.72 / 1,197.60 s** | **35.38 / 143.67 s** | **8.34x** | Focused gate and ownership timings: | Gate or owner | Result | Pytest / wall | |---|---:|---:| -| Shared contract, one process | 1,081 pass, 69 skip, 36 xfail | 5.36 / 6.81 s | -| Shared contract + adjacent units, file-isolated gate | 1,185 pass, 69 skip, 36 xfail | 3.80 / 18.26 s | -| Newton backend units/kernels, including executable kitless guard | 49 pass | 11.72 / 12.78 s | -| PhysX backend units | 33 pass | 1.72 / 2.84 s | -| OV backend units | 50 pass | 1.67 / 2.77 s | +| 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 | 7.89 / 25.47 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, 9 pass | 5.03 / 16.87 s | | PhysX minimal real integration | 6 files, 8 pass | 4.43 / 20.62 s | | OV minimal real integration | 4 files, 6 pass | 2.73 / 10.14 s | -| WrenchComposer real delivery | 1 file, 1 pass | 2.48 / 4.82 s | +| WrenchComposer real delivery | 1 file, 1 pass | 0.79 / 3.12 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 | @@ -76,9 +75,9 @@ 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`. This is intentional for the comparable scopes: for -example, `test_articulation.py` selects both the integration file and -`assets/unit/test_articulation.py`. Basenames are not unique identifiers. +`TEST_FILTER_PATTERN`. The comparable commands therefore enumerate every +integration and unit basename explicitly; backend-qualified OV unit filenames +keep a combined multi-backend pytest collection collision-free. ```bash WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ @@ -105,7 +104,7 @@ TEST_RESULT_FILE=task8-final-assets-physx.xml \ WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ OMNI_KIT_ACCEPT_EULA=YES \ TEST_FILTER_PATTERN=/source/isaaclab_ov/test/assets/ \ -TEST_INCLUDE_FILES=test_actuator_control.py,test_articulation.py,test_articulation_helpers.py,test_articulation_kernels.py,test_deformable_object.py,test_deformable_views.py,test_rigid_object.py,test_rigid_object_collection.py \ +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 @@ -215,11 +214,11 @@ absent from every benchmark command. | `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_deformable_object.py`. | +| `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 | Rigid and fused-collection unit modules. | +| `test_rigid_object_helpers.py` | Moved/expanded | Backend-qualified rigid and fused-collection unit modules. | ## Case-family disposition and coverage holes closed @@ -241,22 +240,24 @@ absent from every benchmark command. | 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 audit classifies every member declared by `AssetBase`, +The public-base-surface inventory classifies every member declared by `AssetBase`, `BaseRigidObject`, `BaseRigidObjectData`, `BaseRigidObjectCollection`, `BaseRigidObjectCollectionData`, `BaseArticulation`, and -`BaseArticulationData` as covered, explicitly unsupported, or reasoned out of -scope. Factory manager replacements are scoped to one contract test and exact +`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 are explicitly unsupported. Contract parameters stay - visible as reasoned skips rather than disappearing from collection. +- 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`. -- The 36 expected failures document Newton fixed-tendon writer/property gaps - and its position-only COM representation; they are not silent omissions. - 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 diff --git a/source/isaaclab/test/assets/contract/_articulation_contract_cases.py b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py index 1467e56d75a..6983c2bbb94 100644 --- a/source/isaaclab/test/assets/contract/_articulation_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_cases.py @@ -20,7 +20,7 @@ import torch import warp as wp from ._articulation_contract_utils import BACKENDS, get_articulation -from .capabilities import backend_parameters, contract_backend +from .capabilities import backend_parameters, contract_backend, require_backend_capability @pytest.fixture def articulation_iface(request): @@ -645,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( @@ -926,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( @@ -1201,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 @@ -1315,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 # --------------------------------------------------------------------------- @@ -1334,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", @@ -1606,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 @@ -1739,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 @@ -1760,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") @@ -1844,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") @@ -2210,8 +2351,7 @@ def test_fixed_tendon_limit_stiffness( device, articulation_iface, ): - if backend == "newton": - pytest.xfail("Newton does not implement fixed-tendon limit stiffness") + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2235,8 +2375,7 @@ def test_fixed_tendon_rest_length( device, articulation_iface, ): - if backend == "newton": - pytest.xfail("Newton does not implement fixed-tendon rest length") + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2260,8 +2399,7 @@ def test_fixed_tendon_offset( device, articulation_iface, ): - if backend == "newton": - pytest.xfail("Newton does not implement fixed-tendon offset") + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) _check_proxy_array( @@ -2285,8 +2423,7 @@ def test_fixed_tendon_pos_limits( device, articulation_iface, ): - if backend == "newton": - pytest.xfail("Newton does not expose fixed-tendon position limits") + require_backend_capability(backend, "fixed_tendon_extended_data") art, _ = articulation_iface art.data.update(dt=0.01) from isaaclab.utils.warp import ProxyArray @@ -2448,8 +2585,8 @@ def test_fixed_tendon_writer_index( wp_dtype, accepts_float, ): - if backend == "newton" and method_base not in {"set_fixed_tendon_stiffness", "set_fixed_tendon_damping"}: - pytest.xfail(f"Newton does not implement {method_base}") + 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") @@ -2515,8 +2652,7 @@ def test_fixed_tendon_writer_mask( wp_dtype, accepts_float, ): - if backend == "newton": - pytest.xfail("Newton fixed-tendon mask writers are not implemented") + require_backend_capability(backend, "fixed_tendon_write_mask") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2714,8 +2850,7 @@ def test_write_fixed_tendon_properties_to_sim_index( device, articulation_iface, ): - if backend == "newton": - pytest.xfail("Newton fixed-tendon write-to-sim does not resolve a default environment selector") + require_backend_capability(backend, "fixed_tendon_write_to_sim_index") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") @@ -2740,8 +2875,7 @@ def test_write_fixed_tendon_properties_to_sim_mask( device, articulation_iface, ): - if backend == "newton": - pytest.xfail("Newton fixed-tendon mask write-to-sim is not implemented") + require_backend_capability(backend, "fixed_tendon_write_to_sim_mask") art, _ = articulation_iface if num_fixed_tendons == 0: pytest.skip("No fixed tendons configured") diff --git a/source/isaaclab/test/assets/contract/_articulation_contract_utils.py b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py index 248cbe178c4..2054093fac1 100644 --- a/source/isaaclab/test/assets/contract/_articulation_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_articulation_contract_utils.py @@ -44,6 +44,57 @@ from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet +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( num_instances: int = 2, num_joints: int = 6, @@ -87,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() @@ -323,7 +375,7 @@ 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 ) diff --git a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py index 24c1fed2e0c..2cd4db73b27 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_cases.py @@ -18,7 +18,7 @@ import torch import warp as wp from ._rigid_object_collection_contract_utils import BACKENDS, get_rigid_object_collection -from .capabilities import contract_backend +from .capabilities import contract_backend, require_backend_capability @pytest.fixture def collection_iface(request): @@ -170,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 # --------------------------------------------------------------------------- @@ -912,6 +936,37 @@ def set_coms() -> None: 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") diff --git a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py index 7ad51281698..624a4c9716b 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_collection_contract_utils.py @@ -55,6 +55,39 @@ from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet +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( num_instances: int = 2, num_bodies: int = 3, @@ -77,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) @@ -142,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() diff --git a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py index d4eb9428db3..5474db9028c 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_cases.py @@ -18,7 +18,7 @@ import torch import warp as wp from ._rigid_object_contract_utils import BACKENDS, get_rigid_object -from .capabilities import contract_backend +from .capabilities import contract_backend, require_backend_capability @pytest.fixture def rigid_object_iface(request): @@ -731,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 # --------------------------------------------------------------------------- @@ -828,6 +850,97 @@ def set_coms() -> None: 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 @@ -967,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 @@ -1035,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 diff --git a/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py index a97501394c6..546102d1fbd 100644 --- a/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py +++ b/source/isaaclab/test/assets/contract/_rigid_object_contract_utils.py @@ -42,6 +42,39 @@ from isaaclab_ov.test.fixtures.views import MockOvPhysxBindingSet +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( num_instances: int = 2, device: str = "cuda:0", @@ -64,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) @@ -137,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() diff --git a/source/isaaclab/test/assets/contract/capabilities.py b/source/isaaclab/test/assets/contract/capabilities.py index fc16356d250..60cad30b932 100644 --- a/source/isaaclab/test/assets/contract/capabilities.py +++ b/source/isaaclab/test/assets/contract/capabilities.py @@ -35,23 +35,45 @@ class BackendStatus: _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 | {"index_resolution", "spatial_tendons"}, + capabilities=_SHARED_CAPABILITIES + | _EXTENDED_ARTICULATION_CAPABILITIES + | {"index_resolution", "spatial_tendons"}, ), BackendDeclaration( name="newton", required_modules=("isaaclab_newton",), capabilities=_SHARED_CAPABILITIES | {"index_resolution"}, - unsupported={"spatial_tendons": "Newton does not support spatial tendons"}, + 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 | {"spatial_tendons"}, + 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, @@ -122,6 +144,17 @@ def available_backends(capability: str) -> list[str]: ] +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} diff --git a/source/isaaclab/test/assets/contract/public_surface.py b/source/isaaclab/test/assets/contract/public_surface.py index 63f3b22d588..7e92a03ce6e 100644 --- a/source/isaaclab/test/assets/contract/public_surface.py +++ b/source/isaaclab/test/assets/contract/public_surface.py @@ -7,7 +7,6 @@ from dataclasses import dataclass from enum import StrEnum -from importlib import import_module from isaaclab.assets.articulation.base_articulation import BaseArticulation from isaaclab.assets.articulation.base_articulation_data import BaseArticulationData @@ -173,10 +172,9 @@ class ContractKind(StrEnum): @dataclass(frozen=True) class PublicMemberContract: - """Map one public member to a concrete contract target or justified exclusion.""" + """Classify one public member or record why it is excluded.""" kind: ContractKind - target: str | None = None reason: str | None = None @@ -187,12 +185,11 @@ class PublicSurfaceAudit: missing: frozenset[str] stale: frozenset[str] unreasoned: frozenset[str] - untargeted: 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 or self.untargeted) + return not (self.missing or self.stale or self.unreasoned) def format_errors(self) -> str: """Format all mapping failures for one actionable pytest diagnostic.""" @@ -202,7 +199,6 @@ def format_errors(self) -> str: ("missing", self.missing), ("stale", self.stale), ("unreasoned exclusions", self.unreasoned), - ("covered members without targets", self.untargeted), ) if values ) @@ -213,38 +209,24 @@ def _mapping( members: str, kind: ContractKind, *, - target: str | None = None, 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, target=target, reason=reason) - for member_name in members.split() + f"{class_name}.{member_name}": PublicMemberContract(kind=kind, reason=reason) for member_name in members.split() } -_ARTICULATION_API_CONTRACT = "contract._articulation_contract_cases.TestArticulationProperties" -_ARTICULATION_DATA_CONTRACT = "contract._articulation_contract_cases.TestArticulationDataRootState" -_ARTICULATION_WRITE_CONTRACT = "contract._articulation_contract_cases.TestArticulationWritersRoot" -_COLLECTION_API_CONTRACT = "contract._rigid_object_collection_contract_cases.TestCollectionProperties" -_COLLECTION_DATA_CONTRACT = "contract._rigid_object_collection_contract_cases.TestCollectionDataBodyState" -_COLLECTION_WRITE_CONTRACT = "contract._rigid_object_collection_contract_cases.TestCollectionWritersPose" -_RIGID_OBJECT_API_CONTRACT = "contract._rigid_object_contract_cases.TestRigidObjectProperties" -_RIGID_OBJECT_DATA_CONTRACT = "contract._rigid_object_contract_cases.TestRigidObjectDataRootState" -_RIGID_OBJECT_WRITE_CONTRACT = "contract._rigid_object_contract_cases.TestRigidObjectWritersRoot" - PUBLIC_SURFACE_CONTRACTS = { **_mapping( "AssetBase", "data device is_initialized num_instances", ContractKind.API, - target=_ARTICULATION_API_CONTRACT, ), **_mapping( "AssetBase", "assert_shape_and_dtype assert_shape_and_dtype_mask reset update write_data_to_sim", ContractKind.WRITE, - target=_ARTICULATION_WRITE_CONTRACT, ), **_mapping( "AssetBase", @@ -259,7 +241,6 @@ def _mapping( permanent_wrench_composer root_view """, ContractKind.API, - target=_RIGID_OBJECT_API_CONTRACT, ), **_mapping( "BaseRigidObject", @@ -276,13 +257,11 @@ def _mapping( write_root_velocity_to_sim_index write_root_velocity_to_sim_mask """, ContractKind.WRITE, - target=_RIGID_OBJECT_WRITE_CONTRACT, ), **_mapping( "BaseRigidObjectData", _PUBLIC_MEMBER_SNAPSHOT["BaseRigidObjectData"], ContractKind.DATA, - target=_RIGID_OBJECT_DATA_CONTRACT, ), **_mapping( "BaseRigidObjectCollection", @@ -291,7 +270,6 @@ def _mapping( num_objects object_names permanent_wrench_composer root_view """, ContractKind.API, - target=_COLLECTION_API_CONTRACT, ), **_mapping( "BaseRigidObjectCollection", @@ -311,13 +289,11 @@ def _mapping( write_object_velocity_to_sim """, ContractKind.WRITE, - target=_COLLECTION_WRITE_CONTRACT, ), **_mapping( "BaseRigidObjectCollectionData", _PUBLIC_MEMBER_SNAPSHOT["BaseRigidObjectCollectionData"], ContractKind.DATA, - target=_COLLECTION_DATA_CONTRACT, ), **_mapping( "BaseArticulation", @@ -329,7 +305,6 @@ def _mapping( spatial_tendon_names """, ContractKind.API, - target=_ARTICULATION_API_CONTRACT, ), **_mapping( "BaseArticulation", @@ -374,13 +349,11 @@ def _mapping( write_spatial_tendon_properties_to_sim_mask """, ContractKind.WRITE, - target=_ARTICULATION_WRITE_CONTRACT, ), **_mapping( "BaseArticulationData", _PUBLIC_MEMBER_SNAPSHOT["BaseArticulationData"], ContractKind.DATA, - target=_ARTICULATION_DATA_CONTRACT, ), } @@ -399,7 +372,6 @@ def audit_public_surface(classes: tuple[type, ...], mappings: dict[str, PublicMe } mapped_members = set(mappings) exclusion_kinds = {ContractKind.UNSUPPORTED, ContractKind.OUT_OF_SCOPE} - covered_kinds = {ContractKind.API, ContractKind.DATA, ContractKind.WRITE} return PublicSurfaceAudit( missing=frozenset(declared_members - mapped_members), stale=frozenset(mapped_members - declared_members), @@ -408,34 +380,9 @@ def audit_public_surface(classes: tuple[type, ...], mappings: dict[str, PublicMe for member_name, contract in mappings.items() if contract.kind in exclusion_kinds and not contract.reason ), - untargeted=frozenset( - member_name - for member_name, contract in mappings.items() - if contract.kind in covered_kinds and not contract.target - ), ) -def unresolved_contract_targets(mappings: dict[str, PublicMemberContract]) -> frozenset[str]: - """Return concrete covered-contract targets that cannot be imported.""" - unresolved = set() - for contract in mappings.values(): - if contract.target is None: - continue - try: - import_module(contract.target) - except ModuleNotFoundError: - module_name, _, attribute = contract.target.rpartition(".") - try: - module = import_module(module_name) - except ModuleNotFoundError: - unresolved.add(contract.target) - else: - if not hasattr(module, attribute): - unresolved.add(contract.target) - return frozenset(unresolved) - - def unclassified_public_members(classes: tuple[type, ...], classifications: dict[str, ContractKind]) -> set[str]: """Return declared public members that have no contract classification.""" declared_members = { diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_api.py b/source/isaaclab/test/assets/contract/test_asset_contract_api.py index 44ef3dd1e11..23aeee449a4 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_api.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_api.py @@ -11,6 +11,8 @@ 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 @@ -241,6 +243,26 @@ def test_newton_fixed_tendon_contract_is_not_skipped_as_spatially_unsupported() 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.""" @@ -287,11 +309,9 @@ class SyntheticAsset: mapping = { "SyntheticAsset.added_member": public_surface.PublicMemberContract( kind=ContractKind.API, - target="contract._articulation_contract_cases.TestArticulationProperties", ), "SyntheticAsset.removed_member": public_surface.PublicMemberContract( kind=ContractKind.API, - target="contract._articulation_contract_cases.TestArticulationProperties", ), } @@ -318,13 +338,8 @@ class SyntheticAsset: assert audit.unreasoned == frozenset({"SyntheticAsset.added_member"}) -def test_public_surface_contract_targets_resolve_to_real_test_classes() -> None: - """Require every covered public member to name a concrete importable contract class.""" - assert public_surface.unresolved_contract_targets(public_surface.PUBLIC_SURFACE_CONTRACTS) == frozenset() - - def test_base_public_surface_has_an_explicit_contract_classification() -> None: - """Require the mapping and current public inventory to match exactly.""" + """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 index e90df2dcf93..2bbc469d880 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_data.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_data.py @@ -9,6 +9,8 @@ import pytest +pytestmark = pytest.mark.unit + from ._articulation_contract_cases import ( # noqa: F401 TestArticulationDataAliases, TestArticulationDataBodyState, diff --git a/source/isaaclab/test/assets/contract/test_asset_contract_writes.py b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py index ed46f673a5f..1bfe9a3844c 100644 --- a/source/isaaclab/test/assets/contract/test_asset_contract_writes.py +++ b/source/isaaclab/test/assets/contract/test_asset_contract_writes.py @@ -7,6 +7,10 @@ """Shared asset write contract tests.""" +import pytest + +pytestmark = pytest.mark.unit + from ._articulation_contract_cases import ( # noqa: F401 TestArticulationWritersBody, TestArticulationWritersFixedTendon, diff --git a/source/isaaclab/test/utils/test_wrench_composer_integration.py b/source/isaaclab/test/utils/test_wrench_composer_integration.py index 89c8ade52c2..4b3f130be4b 100644 --- a/source/isaaclab/test/utils/test_wrench_composer_integration.py +++ b/source/isaaclab/test/utils/test_wrench_composer_integration.py @@ -7,7 +7,7 @@ from isaaclab.app import AppLauncher -simulation_app = AppLauncher(headless=True, device="cpu").app +simulation_app = AppLauncher(headless=True).app import math @@ -18,7 +18,6 @@ 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 @@ -30,9 +29,11 @@ def _make_dual_cube_scene(device: str) -> tuple[RigidObject, RigidObject]: 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)) - spawn = sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg(), + 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(), ) composer = RigidObject( cfg=RigidObjectCfg( diff --git a/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py b/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py index 06d516c1213..b3a2c5d5fdd 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/unit/__init__.py b/source/isaaclab_newton/test/assets/unit/__init__.py deleted file mode 100644 index 460a3056908..00000000000 --- a/source/isaaclab_newton/test/assets/unit/__init__.py +++ /dev/null @@ -1,4 +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 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 index 94d456c469a..e2257ddf6e7 100644 --- a/source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_fk_cache.py @@ -7,10 +7,13 @@ 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.""" 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 index 3514a5e95f8..aaa529025f0 100644 --- a/source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_joint_staging.py @@ -8,6 +8,7 @@ from types import SimpleNamespace import numpy as np +import pytest import torch import warp as wp from isaaclab_newton.assets import Articulation @@ -17,6 +18,8 @@ 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.""" diff --git a/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py index 36066bee528..0d3ab3eafa7 100644 --- a/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py +++ b/source/isaaclab_newton/test/assets/unit/test_articulation_ordering.py @@ -8,6 +8,7 @@ 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 @@ -15,6 +16,8 @@ from isaaclab.assets.articulation.base_articulation import BaseArticulation +pytestmark = pytest.mark.unit + class _LaunchCache: def launch(self, _name, kernel, *, dim, inputs, outputs) -> 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 index bf58de678f9..c554412ef14 100644 --- a/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py +++ b/source/isaaclab_newton/test/assets/unit/test_newton_actuator_adaptation.py @@ -20,6 +20,8 @@ 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() 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 index 5edf83b7da6..aea6771fa21 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -10,6 +10,10 @@ 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]"), 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 index 0e746a6485e..b9036dad04b 100644 --- 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 @@ -8,6 +8,8 @@ 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.""" 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 index c5cb299eabc..24a612aa3af 100644 --- 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 @@ -12,6 +12,8 @@ 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: 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 index ac383f78948..65bffe29e52 100644 --- 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 @@ -6,9 +6,12 @@ """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) 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 index 13eb4db5f25..08d2b4b7056 100644 --- 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 @@ -14,6 +14,8 @@ 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) diff --git a/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py b/source/isaaclab_newton/test/assets/unit/test_wrench_kernels.py index 06f2d4483ce..b1c55689b98 100644 --- a/source/isaaclab_newton/test/assets/unit/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)) diff --git a/source/isaaclab_ov/test/assets/unit/__init__.py b/source/isaaclab_ov/test/assets/unit/__init__.py deleted file mode 100644 index af986054b4f..00000000000 --- a/source/isaaclab_ov/test/assets/unit/__init__.py +++ /dev/null @@ -1,6 +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 - -"""Focused unit and kernel tests for OVPhysX asset adapters.""" diff --git a/source/isaaclab_ov/test/assets/unit/test_articulation_helpers.py b/source/isaaclab_ov/test/assets/unit/test_articulation_helpers.py index 5ca4fbac33c..7186eff284e 100644 --- a/source/isaaclab_ov/test/assets/unit/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 diff --git a/source/isaaclab_ov/test/assets/unit/test_articulation_kernels.py b/source/isaaclab_ov/test/assets/unit/test_articulation_kernels.py index a8c6b79f8f5..177eadf5d0b 100644 --- a/source/isaaclab_ov/test/assets/unit/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/unit/test_deformable_views.py b/source/isaaclab_ov/test/assets/unit/test_deformable_views.py index e521784d303..5ba39643502 100644 --- a/source/isaaclab_ov/test/assets/unit/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_actuator_control.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_actuator_control.py similarity index 99% rename from source/isaaclab_ov/test/assets/unit/test_actuator_control.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_actuator_control.py index edb577e99e8..8d8f20b3d5b 100644 --- a/source/isaaclab_ov/test/assets/unit/test_actuator_control.py +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_actuator_control.py @@ -8,6 +8,7 @@ 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 @@ -15,6 +16,8 @@ from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg +pytestmark = pytest.mark.unit + class _RecordingView: """Capture values written to each OVPhysX tensor type.""" diff --git a/source/isaaclab_ov/test/assets/unit/test_articulation.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_articulation.py similarity index 98% rename from source/isaaclab_ov/test/assets/unit/test_articulation.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_articulation.py index d68f4876010..093c56e7d3d 100644 --- a/source/isaaclab_ov/test/assets/unit/test_articulation.py +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_articulation.py @@ -7,6 +7,7 @@ from unittest.mock import Mock +import pytest import torch import warp as wp from isaaclab_ov import tensor_types as TT @@ -18,6 +19,8 @@ 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.""" diff --git a/source/isaaclab_ov/test/assets/unit/test_deformable_object.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_deformable_object.py similarity index 99% rename from source/isaaclab_ov/test/assets/unit/test_deformable_object.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_deformable_object.py index 42e236925d6..a04ce520a67 100644 --- a/source/isaaclab_ov/test/assets/unit/test_deformable_object.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/unit/test_rigid_object.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object.py similarity index 98% rename from source/isaaclab_ov/test/assets/unit/test_rigid_object.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object.py index 7fdb408cdd2..f8e2800e898 100644 --- a/source/isaaclab_ov/test/assets/unit/test_rigid_object.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_rigid_object_collection.py b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object_collection.py similarity index 99% rename from source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py rename to source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object_collection.py index ec55159d90f..1a7760ac832 100644 --- a/source/isaaclab_ov/test/assets/unit/test_rigid_object_collection.py +++ b/source/isaaclab_ov/test/assets/unit/test_ovphysx_rigid_object_collection.py @@ -14,6 +14,8 @@ 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, 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 c1eea198168..958992a44c6 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_manager_lifecycle.py @@ -15,6 +15,7 @@ from unittest.mock import Mock import pytest +import torch from pxr import Usd @@ -534,6 +535,7 @@ def test_retained_binding_preserves_uncaught_failure_exit_status(): _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()) diff --git a/source/isaaclab_physx/test/assets/unit/__init__.py b/source/isaaclab_physx/test/assets/unit/__init__.py deleted file mode 100644 index 187a2311706..00000000000 --- a/source/isaaclab_physx/test/assets/unit/__init__.py +++ /dev/null @@ -1,6 +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 - -"""Focused unit coverage for PhysX asset adapter logic.""" diff --git a/source/isaaclab_physx/test/assets/unit/test_actuator_control.py b/source/isaaclab_physx/test/assets/unit/test_actuator_control.py index d54e4641c74..92d376d3c5d 100644 --- a/source/isaaclab_physx/test/assets/unit/test_actuator_control.py +++ b/source/isaaclab_physx/test/assets/unit/test_actuator_control.py @@ -8,11 +8,14 @@ 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: diff --git a/source/isaaclab_physx/test/assets/unit/test_articulation.py b/source/isaaclab_physx/test/assets/unit/test_articulation.py index 60a88c610c4..f4a616c477b 100644 --- a/source/isaaclab_physx/test/assets/unit/test_articulation.py +++ b/source/isaaclab_physx/test/assets/unit/test_articulation.py @@ -11,6 +11,7 @@ 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, @@ -18,7 +19,7 @@ write_joint_state_data_kernel, ) -from ._imports import import_physx_module +pytestmark = pytest.mark.unit def _selector(values: list[int], dtype: type) -> wp.array: diff --git a/source/isaaclab_physx/test/assets/unit/test_deformable_object.py b/source/isaaclab_physx/test/assets/unit/test_deformable_object.py index e5432f3c457..9f4ceffd1dd 100644 --- a/source/isaaclab_physx/test/assets/unit/test_deformable_object.py +++ b/source/isaaclab_physx/test/assets/unit/test_deformable_object.py @@ -10,13 +10,14 @@ 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, ) -from ._imports import import_physx_module +pytestmark = pytest.mark.unit def _module(): diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py index c26e86a669e..45ef867729f 100644 --- a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/unit/test_rigid_object.py @@ -9,10 +9,12 @@ from types import SimpleNamespace import numpy as np +import pytest import torch import warp as wp +from _imports import import_physx_module -from ._imports import import_physx_module +pytestmark = pytest.mark.unit def _rigid_object_class(): diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py b/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py index e265d10f56f..3185fac92fd 100644 --- a/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py +++ b/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py @@ -6,11 +6,13 @@ """Focused PhysX rigid-object-collection ordering and selector 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 -from ._imports import import_physx_module +pytestmark = pytest.mark.unit def test_view_id_kernel_maps_instance_body_grid_to_physx_body_major_order() -> None: diff --git a/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py b/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py index 746c3eed648..9bedd2b1e2b 100644 --- a/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py @@ -13,6 +13,8 @@ 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) From a4de71e5360e9eeb6f438dcee4bd2211a8075070 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Mon, 24 Aug 2026 10:39:38 +0200 Subject: [PATCH 24/26] Consolidate articulation integration scenes Reuse one backend scene per articulation module so broader real-solver coverage does not repeatedly rebuild simulation state. Qualify PhysX unit module names to keep mixed collection collision-free and refresh the measured performance report. --- ...08-21-asset-test-suite-redesign-results.md | 52 ++- .../test/articulation_test_utils.py | 61 +-- .../test/assets/test_articulation.py | 327 ++++++++------- .../assets/unit/test_rigid_assets_import.py | 5 +- .../test/assets/test_articulation.py | 393 +++++++++++------- .../test/assets/test_articulation.py | 287 ++++++++----- ...trol.py => test_physx_actuator_control.py} | 2 +- ...culation.py => test_physx_articulation.py} | 2 +- ...ect.py => test_physx_deformable_object.py} | 2 +- ...d_object.py => test_physx_rigid_object.py} | 2 +- ... => test_physx_rigid_object_collection.py} | 2 +- ...ipper.py => test_physx_surface_gripper.py} | 2 +- 12 files changed, 680 insertions(+), 457 deletions(-) rename source/isaaclab_physx/test/assets/unit/{test_actuator_control.py => test_physx_actuator_control.py} (99%) rename source/isaaclab_physx/test/assets/unit/{test_articulation.py => test_physx_articulation.py} (99%) rename source/isaaclab_physx/test/assets/unit/{test_deformable_object.py => test_physx_deformable_object.py} (99%) rename source/isaaclab_physx/test/assets/unit/{test_rigid_object.py => test_physx_rigid_object.py} (97%) rename source/isaaclab_physx/test/assets/unit/{test_rigid_object_collection.py => test_physx_rigid_object_collection.py} (98%) rename source/isaaclab_physx/test/assets/unit/{test_surface_gripper.py => test_physx_surface_gripper.py} (97%) 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 index 0a38ebbff68..4361e57422e 100644 --- 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 @@ -9,10 +9,10 @@ SPDX-License-Identifier: BSD-3-Clause ## Outcome -The comparable asset and WrenchComposer scopes now finish in **143.67 s** of -subprocess wall time, down from **1,197.60 s**: an **8.34x wall-time speed-up**. +The comparable asset and WrenchComposer scopes now finish in **140.41 s** of +subprocess wall time, down from **1,197.60 s**: an **8.53x wall-time speed-up**. The test-runner model is unchanged: every selected file runs in a fresh -subprocess. The final scopes collect 1,511 focused cases: 1,408 pass and 103 +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 @@ -39,7 +39,7 @@ deliberately excluded. ## Copy-ready PR performance section The asset-test redesign reduced the five comparable CI-style scopes from -19m57.60s to 2m23.67s wall time (**8.34x faster**). This comparison uses the +19m57.60s to 2m20.41s wall time (**8.53x 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. @@ -47,11 +47,23 @@ 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 | 7.89 / 25.47 s | **3.30x** | -| Newton assets, no cable/MPM | 6 / 644 | 15 / 58 pass | 590.94 / 608.44 s | 17.00 / 56.02 s | **10.86x** | -| PhysX assets | 7 / 486 | 12 / 41 pass | 236.80 / 255.30 s | 6.13 / 32.32 s | **7.90x** | -| OV assets | 9 / 492 | 12 / 56 pass | 196.94 / 220.05 s | 3.55 / 25.22 s | **8.73x** | +| Newton assets, no cable/MPM | 6 / 644 | 15 / 59 pass | 590.94 / 608.44 s | 16.29 / 50.49 s | **12.05x** | +| PhysX assets | 7 / 486 | 12 / 44 pass | 236.80 / 255.30 s | 6.21 / 32.54 s | **7.85x** | +| OV assets | 9 / 492 | 12 / 60 pass | 196.94 / 220.05 s | 5.41 / 27.27 s | **8.07x** | | WrenchComposer | 3 / 412 | 2 / 15 pass | 23.34 / 29.79 s | 0.81 / 4.64 s | **6.42x** | -| **Aggregate** | **33 / 6,365** | **47 / 1,408 pass, 103 skip** | **1,112.72 / 1,197.60 s** | **35.38 / 143.67 s** | **8.34x** | +| **Aggregate** | **33 / 6,365** | **47 / 1,416 pass, 103 skip** | **1,112.72 / 1,197.60 s** | **36.61 / 140.41 s** | **8.53x** | + +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. + +| Articulation backend | Before cases | After cases | Before pytest / wall | After pytest / wall | +|---|---:|---:|---:|---:| +| Newton | 3 | 4 | 3.04 / 4.65 s | 2.54 / 4.11 s | +| PhysX | 2 | 5 | 4.94 / 5.75 s | 3.44 / 4.17 s | +| OVPhysX | 2 | 6 | 2.38 / 3.46 s | 2.53 / 3.66 s | +| **Aggregate** | **7** | **15** | **10.36 / 13.86 s** | **8.51 / 11.94 s** | Focused gate and ownership timings: @@ -62,9 +74,9 @@ Focused gate and ownership timings: | 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, 9 pass | 5.03 / 16.87 s | -| PhysX minimal real integration | 6 files, 8 pass | 4.43 / 20.62 s | -| OV minimal real integration | 4 files, 6 pass | 2.73 / 10.14 s | +| Newton minimal real integration | 4 files, 10 pass | 4.54 / 16.21 s | +| PhysX minimal real integration | 6 files, 11 pass | 4.52 / 21.18 s | +| OV minimal real integration | 4 files, 10 pass | 4.61 / 12.13 s | | WrenchComposer real delivery | 1 file, 1 pass | 0.79 / 3.12 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 | @@ -76,8 +88,8 @@ All warmed contract/backend-unit gates are below the 30-second target. `TEST_INCLUDE_FILES` matches **basenames recursively** below `TEST_FILTER_PATTERN`. The comparable commands therefore enumerate every -integration and unit basename explicitly; backend-qualified OV unit filenames -keep a combined multi-backend pytest collection collision-free. +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 \ @@ -97,7 +109,7 @@ TEST_RESULT_FILE=task8-final-assets-newton.xml \ WARP_CACHE_PATH=/tmp/isaaclab-task8-warp \ OMNI_KIT_ACCEPT_EULA=YES \ TEST_FILTER_PATTERN=/source/isaaclab_physx/test/assets/ \ -TEST_INCLUDE_FILES=test_actuator_control.py,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_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 @@ -136,8 +148,8 @@ export OMNI_KIT_ACCEPT_EULA=YES ./isaaclab.sh -p -m pytest source/isaaclab_ov/test/assets/unit -q ``` -Real-only subprocess gates must exclude `/assets/unit/`; otherwise colliding -basenames pull unit files into the result. +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 @@ -184,7 +196,7 @@ TEST_RESULT_FILE=task8-final-integration-ov.xml \ | Old module | Disposition | Coverage owner | |---|---|---| -| `test_articulation.py` | Retained and reduced | Local floating/fixed articulation seams, partial state/property/wrench, drive, Jacobian, and mass matrix. FK, staging, and ordering moved to units; IK/OSC/gravity moved to the controller owner. | +| `test_articulation.py` | Retained and consolidated | One kitless CPU scene contains duplicated floating and fixed actor islands across identical Newton worlds. Four nodes cover partial state/property/wrench, drive, Jacobian, and mass matrix 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. | @@ -198,8 +210,8 @@ absent from every benchmark command. | Old module | Disposition | Coverage owner | |---|---|---| -| `test_articulation.py` | Retained and reduced | One local ordered CPU articulation seam and one CUDA dynamics smoke; property/order conversions moved to units. | -| `test_articulation_kernels.py` | Moved/expanded | `assets/unit/test_articulation.py`. | +| `test_articulation.py` | Retained and consolidated | One local composite scene contains ordered/fixed, floating, and spatial-tendon islands. Five nodes cover state, raw properties, dynamics, root/COM state, wrench delivery, 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. | @@ -210,7 +222,7 @@ absent from every benchmark command. | Old module | Disposition | Coverage owner | |---|---|---| -| `test_articulation.py` | Retained and reduced | Local CPU and CUDA articulation seams with ordered state/properties, native actuation, Jacobian, and mass access. | +| `test_articulation.py` | Retained and consolidated | One CUDA composite scene contains ordered/fixed, floating, spatial-tendon, and native-actuator islands. Six nodes cover state/properties, drive and dynamics, 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. | diff --git a/source/isaaclab_newton/test/articulation_test_utils.py b/source/isaaclab_newton/test/articulation_test_utils.py index 2c71c5e2914..a4dad0a52f1 100644 --- a/source/isaaclab_newton/test/articulation_test_utils.py +++ b/source/isaaclab_newton/test/articulation_test_utils.py @@ -30,8 +30,13 @@ def build_newton_context(*, gravity: tuple[float, float, float] = (0.0, 0.0, 0.0 ) -def author_fixed_spatial_chain(*, actuators: dict | None = None) -> Articulation: - """Author the smallest fixed chain with a full-rank spatial Jacobian.""" +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), @@ -39,36 +44,36 @@ def author_fixed_spatial_chain(*, actuators: dict | None = None) -> Articulation collision_props=sim_utils.CollisionBaseCfg(collision_enabled=False), ) stage = sim_utils.get_current_stage() - robot_path = "/World/Robot" - 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]) - axes: Sequence[str] = ("X", "Y", "Z", "X", "Y", "Z") - 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 + 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=robot_path, + prim_path=prim_path_expr or prim_paths[0], articulation_root_prim_path="/Root", actuators=( actuators diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index debdcd6b422..8298234155b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3,7 +3,9 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Kitless real-solver integration tests for Newton articulations.""" +"""Kitless real-Newton articulation coverage on one module-scoped scene.""" + +from dataclasses import dataclass import pytest import torch @@ -17,28 +19,37 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg -from isaaclab.sim import SimulationCfg, build_simulation_context +from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context -from source.isaaclab_newton.test.articulation_test_utils import author_fixed_spatial_chain, build_newton_context +from source.isaaclab_newton.test.articulation_test_utils import author_fixed_spatial_chain pytestmark = pytest.mark.integration -def _newton_sim_context(*, device: str = "cpu", use_newton_actuators: bool = False): - """Create a fresh kitless Newton simulation context.""" +@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), - use_newton_actuators=use_newton_actuators, ) ) -def _author_two_link_articulations(*, actuators: dict | None = None) -> Articulation: - """Author two local one-DOF articulations without files or remote assets.""" +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), @@ -69,160 +80,160 @@ def _author_two_link_articulations(*, actuators: dict | None = None) -> Articula ArticulationCfg( prim_path="/World/Env_[^/]*/Robot", articulation_root_prim_path="/Root", - actuators=( - actuators - if actuators is not None - else { - "joint": ImplicitActuatorCfg( - joint_names_expr=["Joint"], - stiffness=20.0, - damping=2.0, - ) - } - ), + actuators={ + "joint": ImplicitActuatorCfg( + joint_names_expr=["Joint"], + stiffness=20.0, + damping=2.0, + ) + }, ) ) -def test_articulation_real_newton_seams(monkeypatch) -> None: - """Exercise the minimal real Newton articulation adapter with isolated partial writes.""" - with _newton_sim_context() as sim: - articulation = _author_two_link_articulations() - sim.reset() - - assert articulation.is_initialized - 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) - joint_ids = torch.tensor([0], dtype=torch.int32) - body_ids = torch.tensor([1], dtype=torch.int32) - initial_root_pose = articulation.data.root_link_pose_w.torch.clone() - initial_joint_pos = articulation.data.joint_pos.torch.clone() - initial_joint_vel = articulation.data.joint_vel.torch.clone() - target_root_pose = initial_root_pose[env_ids].clone() - target_root_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3]) - target_joint_pos = torch.tensor([[0.25]], dtype=torch.float32) - target_joint_vel = torch.tensor([[-0.5]], dtype=torch.float32) - - articulation.write_root_link_pose_to_sim_index(root_pose=target_root_pose, env_ids=env_ids) - 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.root_link_pose_w.torch[env_ids], target_root_pose) - torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_root_pose[:1]) - 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]) - - 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)) - initial_mass = articulation.data.body_mass.torch.clone() - masses = torch.tensor([[3.0]]) - 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]]]) - 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]]])).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] - - jacobians = articulation.data.body_link_jacobian_w.torch - mass_matrix = articulation.data.mass_matrix.torch - assert jacobians.shape == (2, 2, 6, 7) - assert mass_matrix.shape == (2, 7, 7) - assert torch.isfinite(jacobians).all() - assert torch.isfinite(mass_matrix).all() - - 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]]]), - torques=torch.zeros((1, 1, 3)), - env_ids=torch.tensor([1], dtype=torch.int32), - body_ids=torch.tensor([0], dtype=torch.int32), - ) - articulation.write_data_to_sim() - sim.step() - articulation.update(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 +@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", ) - - -def test_fixed_base_articulation_real_newton_seams() -> None: - """Exercise fixed-root state, moving-link velocity, and dynamics data on a local chain.""" - with build_newton_context() as sim: - articulation = author_fixed_spatial_chain() - sim.reset() - - assert articulation.is_initialized - assert articulation.is_fixed_base - assert articulation.num_instances == 1 - assert articulation.num_bodies == 7 - assert articulation.num_joints == 6 - initial_root_pose = articulation.data.root_link_pose_w.torch.clone() - target = articulation.data.joint_pos.torch.clone() - target[:, 0] = 0.05 - articulation.actuators.target_command.set_position_index(value=target) - - for _ in range(12): - articulation.write_data_to_sim() - sim.step() - articulation.update(sim.cfg.dt) - - torch.testing.assert_close(articulation.data.root_link_pose_w.torch, initial_root_pose, atol=1e-6, rtol=0) - assert torch.linalg.vector_norm(articulation.data.body_link_vel_w.torch[0, -1]) > 1e-3 - jacobians = articulation.data.body_link_jacobian_w.torch - mass_matrix = articulation.data.mass_matrix.torch - assert jacobians.shape == (1, 6, 6, 6) - assert mass_matrix.shape == (1, 6, 6) - assert torch.isfinite(jacobians).all() - assert torch.isfinite(mass_matrix).all() - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") -def test_articulation_cuda_jacobian_and_mass_access() -> None: - """Smoke-test Newton's CUDA-backed Jacobian and mass-matrix adapter access.""" - with _newton_sim_context(device="cuda:0") as sim: - articulation = _author_two_link_articulations() sim.reset() + yield _ArticulationScene(sim=sim, floating=floating, fixed=fixed, device=device) + + +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 + 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_root_pose = articulation.data.root_link_pose_w.torch.clone() + initial_joint_pos = articulation.data.joint_pos.torch.clone() + initial_joint_vel = articulation.data.joint_vel.torch.clone() + target_root_pose = initial_root_pose[env_ids].clone() + target_root_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3], device=device) + target_joint_pos = torch.tensor([[0.25]], device=device) + target_joint_vel = torch.tensor([[-0.5]], device=device) + articulation.write_root_link_pose_to_sim_index(root_pose=target_root_pose, env_ids=env_ids) + articulation.write_joint_state_to_sim_index( + position=target_joint_pos, + velocity=target_joint_vel, + env_ids=env_ids, + joint_ids=joint_ids, + ) - jacobians = articulation.data.body_link_jacobian_w.torch - mass_matrix = articulation.data.mass_matrix.torch - assert jacobians.device.type == "cuda" - assert mass_matrix.device.type == "cuda" - assert jacobians.shape == (2, 2, 6, 7) - assert mass_matrix.shape == (2, 7, 7) - assert torch.isfinite(jacobians).all() - assert torch.isfinite(mass_matrix).all() + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[env_ids], target_root_pose) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_root_pose[:1]) + 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_model_properties_notify_newton( + articulation_scene: _ArticulationScene, monkeypatch: pytest.MonkeyPatch +) -> None: + """Prove partial inertial-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)) + 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] + + +def test_articulation_dynamics_and_wrench_response(articulation_scene: _ArticulationScene) -> None: + """Prove live floating-base dynamics data and isolated wrench delivery.""" + articulation = articulation_scene.floating + device = articulation_scene.device + jacobians = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobians.device.type == torch.device(device).type + assert mass_matrix.device.type == torch.device(device).type + assert jacobians.shape == (2, 2, 6, 7) + assert mass_matrix.shape == (2, 7, 7) + assert torch.isfinite(jacobians).all() + assert torch.isfinite(mass_matrix).all() + + 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=torch.tensor([1], dtype=torch.int32, device=device), + body_ids=torch.tensor([0], dtype=torch.int32, device=device), + ) + 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) + + +def test_fixed_articulation_actuation_and_dynamics(articulation_scene: _ArticulationScene) -> None: + """Prove fixed-root actuation, moving-link velocity, and dynamics data.""" + articulation = articulation_scene.fixed + assert articulation.is_initialized + assert articulation.is_fixed_base + assert articulation.num_instances == 2 + assert articulation.num_bodies == 7 + assert articulation.num_joints == 6 + initial_root_pose = articulation.data.root_link_pose_w.torch.clone() + target = articulation.data.joint_pos.torch.clone() + target[:, 0] = 0.05 + articulation.actuators.target_command.set_position_index(value=target) + + 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) + assert torch.linalg.vector_norm(articulation.data.body_link_vel_w.torch[0, -1]) > 1e-3 + jacobians = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + 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() 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 index aea6771fa21..29f615e1ff9 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -18,7 +18,10 @@ _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_real_newton_seams"), + ("test_articulation.py", "test_articulation_initialization_and_partial_state"), + ("test_articulation.py", "test_articulation_model_properties_notify_newton"), + ("test_articulation.py", "test_articulation_dynamics_and_wrench_response"), + ("test_articulation.py", "test_fixed_articulation_actuation_and_dynamics"), ("test_newton_actuators_newton.py", "test_newton_actuator_real_equivalence"), ("../controllers/test_newton_task_space_controllers.py", "test_differential_ik_tracks_local_newton_chain"), ( diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index f46d166fe17..b1d7fdbde13 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -3,17 +3,18 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Minimal real-OVPhysX integration coverage for articulations.""" +"""Real OVPhysX articulation coverage on one module-scoped scene.""" from __future__ import annotations +from dataclasses import dataclass from pathlib import Path import pytest import torch import warp as wp -from pxr import UsdPhysics +from pxr import Gf, Sdf, UsdPhysics pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") @@ -24,7 +25,7 @@ import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg # noqa: E402 from isaaclab.assets import ArticulationCfg # noqa: E402 -from isaaclab.sim import SimulationCfg, build_simulation_context # 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, @@ -35,8 +36,20 @@ _FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" -def _sim_context(device: str = "cpu", *, use_newton_actuators: bool = False): - """Build a local CPU OVPhysX context from an in-memory USD stage.""" +@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(), @@ -48,8 +61,19 @@ def _sim_context(device: str = "cpu", *, use_newton_actuators: bool = False): ) -def _spawn_ordered_articulation(*, native_actuator: bool = False) -> Articulation: - """Spawn the cached local branching fixture in nonidentity public order.""" +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( + prim_path: str, *, fixed_base: bool, native_actuator: bool = False, spatial_tendon: bool = False +) -> Articulation: + """Spawn one cached local branching articulation island.""" actuator_cfg = ( IdealPDActuatorCfg( joint_names_expr=[".*"], @@ -62,7 +86,7 @@ def _spawn_ordered_articulation(*, native_actuator: bool = False) -> Articulatio ) articulation = Articulation( ArticulationCfg( - prim_path="/World/Robot", + prim_path=prim_path, spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), actuators={"joints": actuator_cfg}, joint_ordering="mjwarp", @@ -71,147 +95,236 @@ def _spawn_ordered_articulation(*, native_actuator: bool = False) -> Articulatio ) stage = sim_utils.get_current_stage() for joint_name in ("left_shoulder", "left_elbow", "right_shoulder", "right_elbow"): - drive = UsdPhysics.DriveAPI.Apply(stage.GetPrimAtPath(f"/World/Robot/{joint_name}"), "angular") + drive = UsdPhysics.DriveAPI.Apply(stage.GetPrimAtPath(f"{prim_path}/{joint_name}"), "angular") drive.CreateStiffnessAttr(5.0) drive.CreateDampingAttr(0.5) drive.CreateMaxForceAttr(100.0) - fixed_joint = UsdPhysics.FixedJoint.Define(stage, "/World/Robot/fixed_root") - fixed_joint.GetBody1Rel().SetTargets(["/World/Robot/base"]) + 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 -def test_articulation_real_ovphysx_seams() -> None: - """Prove ordering, partial state/properties, drive delivery, and dynamics access.""" - with _sim_context() as sim: - articulation = _spawn_ordered_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("/World/Ordered", fixed_base=True) + floating = _spawn_articulation("/World/Floating", fixed_base=False) + tendon = _spawn_articulation("/World/Tendon", fixed_base=True, spatial_tendon=True) + native = _spawn_articulation("/World/Native", fixed_base=True, native_actuator=True) if native_enabled else None sim.reset() - - 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 - - joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32) - target_position = torch.tensor([[0.21, -0.13]]) - target_velocity = torch.tensor([[0.41, -0.23]]) - expected_position = articulation.data.joint_pos.torch.clone() - expected_velocity = articulation.data.joint_vel.torch.clone() - expected_position[:, joint_ids] = target_position - expected_velocity[:, joint_ids] = target_velocity - articulation.write_joint_state_to_sim_index( - position=target_position, - velocity=target_velocity, - joint_ids=joint_ids, - ) - torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) - torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) - - backend_friction_before = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)).clone() - static_friction = torch.tensor([[0.9, 0.7]]) - dynamic_friction = torch.tensor([[0.4, 0.3]]) - viscous_friction = torch.tensor([[0.11, 0.22]]) - 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, - joint_ids=joint_ids, - ) - backend_joint_ids = torch.as_tensor(articulation.joint_ordering.user_to_backend_indices)[joint_ids] - expected_backend_friction = backend_friction_before.clone() - expected_backend_friction[:, backend_joint_ids, 0] = static_friction - expected_backend_friction[:, backend_joint_ids, 1] = dynamic_friction - expected_backend_friction[:, backend_joint_ids, 2] = viscous_friction - raw_backend_friction = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)) - torch.testing.assert_close(raw_backend_friction, expected_backend_friction) - - body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32) - backend_body_ids = torch.as_tensor(articulation.body_ordering.user_to_backend_indices)[body_ids] - raw_mass_before = wp.to_torch(articulation.root_view.get_attribute(TT.BODY_MASS)).clone() - masses = torch.tensor([[2.5, 3.5]]) - articulation.set_masses_index(masses=masses, body_ids=body_ids) - expected_raw_mass = raw_mass_before.clone() - expected_raw_mass[:, backend_body_ids] = masses - 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[:, body_ids].clone() - coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03]) - coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02]) - articulation.set_coms_index(coms=wp.from_torch(coms, dtype=wp.transformf), body_ids=body_ids) - expected_raw_com = raw_com_before.clone() - expected_raw_com[:, backend_body_ids] = coms - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_attribute(TT.BODY_COM_POSE)), expected_raw_com + yield _ArticulationScene( + sim=sim, + ordered=ordered, + floating=floating, + tendon=tendon, + native=native, + device=device, ) - raw_inertia_before = wp.to_torch(articulation.root_view.get_attribute(TT.BODY_INERTIA)).clone() - inertias = articulation.data.body_inertia.torch[:, body_ids].clone() - inertias[0, 0, 0] *= 1.2 - inertias[0, 1, 4] *= 1.3 - articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) - expected_raw_inertia = raw_inertia_before.clone() - expected_raw_inertia[:, backend_body_ids] = inertias - 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[:, body_ids], masses) - torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) - torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) - - 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() - drive_target = articulation.data.joint_pos.torch.clone() - drive_target[:, 0] += 0.4 - articulation.actuators.target_command.set_position_index(value=drive_target, full_data=True) - articulation.write_data_to_sim() - backend_target = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)) - backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) - torch.testing.assert_close(backend_target, drive_target[:, backend_to_user]) - - for _ in range(8): - sim.step() - articulation.update(sim.cfg.dt) - articulation.write_data_to_sim() - assert torch.any(torch.abs(articulation.data.joint_pos.torch[:, 0] - initial_drive_position) > 1e-6) - jacobian = articulation.data.body_link_jacobian_w.torch - mass_matrix = articulation.data.mass_matrix.torch - assert jacobian.shape == (1, articulation.num_bodies - 1, 6, articulation.num_joints) - assert mass_matrix.shape == (1, articulation.num_joints, articulation.num_joints) - 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) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="Native actuator wheel probe requires CUDA") -def test_articulation_native_actuator_submits_real_ovphysx_effort() -> None: - """Prove the local native controller reaches the real OVPhysX effort binding.""" - with _sim_context(device="cuda:0", use_newton_actuators=True) as sim: - articulation = _spawn_ordered_articulation(native_actuator=True) - sim.reset() - 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) +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 + + 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[:, joint_ids] = target_position + expected_velocity[:, joint_ids] = target_velocity + articulation.write_joint_state_to_sim_index( + position=target_position, + velocity=target_velocity, + joint_ids=joint_ids, + ) + torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) + torch.testing.assert_close(articulation.data.joint_vel.torch, expected_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 + 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, + joint_ids=joint_ids, + ) + 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[:, backend_joint_ids, 0] = static_friction.cpu() + expected_backend_friction[:, backend_joint_ids, 1] = dynamic_friction.cpu() + expected_backend_friction[:, backend_joint_ids, 2] = viscous_friction.cpu() + torch.testing.assert_close( + 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, body_ids=body_ids) + expected_raw_mass = raw_mass_before.clone() + expected_raw_mass[:, 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[:, 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), body_ids=body_ids) + expected_raw_com = raw_com_before.clone() + expected_raw_com[:, 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[:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[0, 1, 4] *= 1.3 + articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) + expected_raw_inertia = raw_inertia_before.clone() + expected_raw_inertia[:, 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[:, body_ids], masses) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) + torch.testing.assert_close(articulation.data.body_inertia.torch[:, 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 + 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() + drive_target = articulation.data.joint_pos.torch.clone() + drive_target[:, 0] += 0.4 + articulation.actuators.target_command.set_position_index(value=drive_target, full_data=True) + articulation.write_data_to_sim() + backend_target = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)) + backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) + torch.testing.assert_close(backend_target, drive_target[:, backend_to_user]) + + for _ in range(8): + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) articulation.write_data_to_sim() + assert torch.any(torch.abs(articulation.data.joint_pos.torch[:, 0] - initial_drive_position) > 1e-6) + jacobian = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobian.shape == (1, articulation.num_bodies - 1, 6, articulation.num_joints) + assert mass_matrix.shape == (1, 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) - 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]) - - for _ in range(8): - sim.step() - articulation.update(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( - wp.to_torch(articulation.root_view.get_attribute(TT.DOF_ACTUATION_FORCE)), - recomputed_effort[:, backend_to_user], - ) + +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 == (1, 7) + assert articulation.data.root_com_pose_w.torch.shape == (1, 7) + assert articulation.data.body_link_pose_w.torch.shape == (1, articulation.num_bodies, 7) + assert articulation.data.body_com_pose_w.torch.shape == (1, articulation.num_bodies, 7) + 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=torch.tensor([0], dtype=torch.int32, device=device), + body_ids=torch.tensor([0], dtype=torch.int32, device=device), + ) + 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[0, 0] > initial_velocity[0, 0] + + +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_spatial_tendons == 1 + 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) + articulation.set_spatial_tendon_damping_index(damping=damping) + articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=limit_stiffness) + articulation.set_spatial_tendon_offset_index(offset=offset) + torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch, stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_damping.torch, damping) + torch.testing.assert_close(articulation.data.spatial_tendon_limit_stiffness.torch, limit_stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_offset.torch, 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._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() + + 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]) + + 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( + wp.to_torch(articulation.root_view.get_attribute(TT.DOF_ACTUATION_FORCE)), + recomputed_effort[:, backend_to_user], + ) diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index cc647436366..184f8640e3c 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -3,12 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Minimal real-PhysX integration coverage for articulations.""" +"""Real PhysX articulation coverage on one module-scoped scene.""" from isaaclab.app import AppLauncher simulation_app = AppLauncher(headless=True, device="cpu").app +from dataclasses import dataclass from pathlib import Path import pytest @@ -16,22 +17,33 @@ import warp as wp from isaaclab_physx.assets import Articulation -from pxr import UsdGeom, UsdPhysics +from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg -from isaaclab.sim import build_simulation_context +from isaaclab.sim import SimulationContext, build_simulation_context pytestmark = pytest.mark.integration _FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" -def _spawn_ordered_articulation() -> Articulation: - """Spawn the cached local branching fixture with nonidentity public axes.""" +@dataclass +class _ArticulationScene: + """Articulations that share one real PhysX lifecycle.""" + + sim: SimulationContext + ordered: Articulation + floating: Articulation + tendon: Articulation + device: str + + +def _spawn_articulation(prim_path: str, *, fixed_base: bool, spatial_tendon: bool = False) -> Articulation: + """Spawn one local branching articulation island.""" articulation = Articulation( ArticulationCfg( - prim_path="/World/Robot", + prim_path=prim_path, spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), actuators={}, joint_ordering="mjwarp", @@ -39,110 +51,177 @@ def _spawn_ordered_articulation() -> Articulation: ) ) stage = sim_utils.get_current_stage() - collision = UsdGeom.Cube.Define(stage, "/World/Robot/base/collision") + collision = UsdGeom.Cube.Define(stage, f"{prim_path}/base/collision") collision.CreateSizeAttr(0.1) UsdPhysics.CollisionAPI.Apply(collision.GetPrim()) - UsdPhysics.FixedJoint.Define(stage, "/World/Robot/fixed_root").GetBody1Rel().SetTargets(["/World/Robot/base"]) + 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 -def test_articulation_real_physx_seams() -> None: - """Prove ordered joint state, model-property writes, Jacobian, and mass access.""" - with build_simulation_context(device="cpu", gravity_enabled=False) as sim: - articulation = _spawn_ordered_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("/World/Ordered", fixed_base=True) + floating = _spawn_articulation("/World/Floating", fixed_base=False) + tendon = _spawn_articulation("/World/Tendon", fixed_base=True, spatial_tendon=True) sim.reset() - - 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 == 1 - assert articulation.num_joints >= 2 - assert articulation.num_bodies >= 3 - - joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int32) - target_position = torch.tensor([[0.21, -0.13]]) - target_velocity = torch.tensor([[0.41, -0.23]]) - expected_position = articulation.data.joint_pos.torch.clone() - expected_velocity = articulation.data.joint_vel.torch.clone() - expected_position[:, joint_ids] = target_position - expected_velocity[:, joint_ids] = target_velocity - articulation.write_joint_state_to_sim_index( - position=target_position, velocity=target_velocity, joint_ids=joint_ids - ) - torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) - torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) - joint_backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_dof_positions()), expected_position[:, joint_backend_to_user] - ) - - body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32) - masses = torch.tensor([[2.5, 3.5]]) - articulation.set_masses_index(masses=masses, body_ids=body_ids) - torch.testing.assert_close(articulation.data.body_mass.torch[:, body_ids], masses) - - coms = articulation.data.body_com_pose_b.torch[:, body_ids].clone() - coms[0, 0, :3] = torch.tensor([0.02, -0.01, 0.03]) - coms[0, 1, :3] = torch.tensor([-0.03, 0.01, 0.02]) - articulation.set_coms_index(coms=coms, body_ids=body_ids) - torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) - - inertias = articulation.data.body_inertia.torch[:, body_ids].clone() - inertias[0, 0, 0] *= 1.2 - inertias[0, 1, 4] *= 1.3 - articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) - torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) - - body_backend_to_user = list(articulation.body_ordering.backend_to_user_indices) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_masses()), - articulation.data.body_mass.torch[:, body_backend_to_user], - ) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_coms()), - articulation.data.body_com_pose_b.torch[:, body_backend_to_user], - ) - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_inertias()), - articulation.data.body_inertia.torch[:, body_backend_to_user], - ) - - materials = torch.empty((1, articulation.root_view.max_shapes, 3)) - materials[..., 0] = 0.91 - materials[..., 1] = 0.17 - materials[..., 2] = 0.63 - articulation.root_view.set_material_properties( - wp.from_torch(materials, dtype=wp.float32), wp.array([0], dtype=wp.int32, device="cpu") - ) - torch.testing.assert_close(wp.to_torch(articulation.root_view.get_material_properties()), materials) - - sim.step() - articulation.update(sim.cfg.dt) - jacobian = articulation.data.body_link_jacobian_w.torch - mass_matrix = articulation.data.mass_matrix.torch - assert jacobian.shape == ( - 1, - articulation.num_bodies - 1, - 6, - articulation.num_joints, - ) - assert mass_matrix.shape == (1, articulation.num_joints, articulation.num_joints) - 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) + yield _ArticulationScene(sim=sim, ordered=ordered, floating=floating, tendon=tendon, device=device) + + +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 == 1 + assert articulation.num_joints >= 2 + assert articulation.num_bodies >= 3 + + 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[:, joint_ids] = target_position + expected_velocity[:, joint_ids] = target_velocity + articulation.write_joint_state_to_sim_index( + position=target_position, + velocity=target_velocity, + joint_ids=joint_ids, + ) + torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) + torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + joint_backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_dof_positions()), expected_position[:, joint_backend_to_user] + ) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available") -def test_articulation_cuda_dynamics_access() -> None: - """Smoke-test the distinct CUDA-backed Jacobian and mass-matrix path.""" - with build_simulation_context(device="cuda:0", gravity_enabled=False) as sim: - articulation = _spawn_ordered_articulation() - sim.reset() - sim.step() - articulation.update(sim.cfg.dt) +def test_articulation_model_properties_round_trip(articulation_scene: _ArticulationScene) -> None: + """Prove selected body and material properties round-trip through PhysX.""" + articulation = articulation_scene.ordered + device = articulation_scene.device + body_ids = torch.tensor([articulation.num_bodies - 1, 1], dtype=torch.int32, device=device) + masses = torch.tensor([[2.5, 3.5]], device=device) + articulation.set_masses_index(masses=masses, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_mass.torch[:, body_ids], masses) + + coms = articulation.data.body_com_pose_b.torch[:, 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, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) + + inertias = articulation.data.body_inertia.torch[:, body_ids].clone() + inertias[0, 0, 0] *= 1.2 + inertias[0, 1, 4] *= 1.3 + articulation.set_inertias_index(inertias=inertias, body_ids=body_ids) + torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) + + body_backend_to_user = list(articulation.body_ordering.backend_to_user_indices) + torch.testing.assert_close( + wp.to_torch(articulation.root_view.get_masses()).to(device), + articulation.data.body_mass.torch[:, body_backend_to_user], + ) + torch.testing.assert_close( + 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( + wp.to_torch(articulation.root_view.get_inertias()).to(device), + articulation.data.body_inertia.torch[:, body_backend_to_user], + ) - assert articulation.data.body_link_jacobian_w.torch.device.type == "cuda" - assert articulation.data.mass_matrix.torch.device.type == "cuda" - assert torch.isfinite(articulation.data.body_link_jacobian_w.torch).all() - assert torch.isfinite(articulation.data.mass_matrix.torch).all() + materials = torch.empty((1, articulation.root_view.max_shapes, 3)) + materials[..., 0] = 0.91 + materials[..., 1] = 0.17 + materials[..., 2] = 0.63 + articulation.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.array([0], dtype=wp.int32, device="cpu") + ) + torch.testing.assert_close(wp.to_torch(articulation.root_view.get_material_properties()), materials) + + +def test_articulation_dynamics_are_finite_and_fresh(articulation_scene: _ArticulationScene) -> None: + """Prove live Jacobian and mass data remain valid after a simulation step.""" + articulation = articulation_scene.ordered + articulation_scene.sim.step() + articulation.update(articulation_scene.sim.cfg.dt) + jacobian = articulation.data.body_link_jacobian_w.torch + mass_matrix = articulation.data.mass_matrix.torch + assert jacobian.shape == (1, articulation.num_bodies - 1, 6, articulation.num_joints) + assert mass_matrix.shape == (1, 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 + assert articulation.is_initialized + assert not articulation.is_fixed_base + assert articulation.data.root_link_pose_w.torch.shape == (1, 7) + assert articulation.data.root_com_pose_w.torch.shape == (1, 7) + assert articulation.data.body_link_pose_w.torch.shape == (1, articulation.num_bodies, 7) + assert articulation.data.body_com_pose_w.torch.shape == (1, articulation.num_bodies, 7) + assert torch.isfinite(articulation.data.root_com_pose_w.torch).all() + + 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=articulation_scene.device), + torques=torch.zeros((1, 1, 3), device=articulation_scene.device), + env_ids=torch.tensor([0], dtype=torch.int32, device=articulation_scene.device), + body_ids=torch.tensor([0], dtype=torch.int32, device=articulation_scene.device), + ) + 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[0, 0] > initial_velocity[0, 0] + + +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 + assert articulation.is_fixed_base + assert articulation.num_spatial_tendons == 1 + 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) + articulation.set_spatial_tendon_damping_index(damping=damping) + articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=limit_stiffness) + articulation.set_spatial_tendon_offset_index(offset=offset) + torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch, stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_damping.torch, damping) + torch.testing.assert_close(articulation.data.spatial_tendon_limit_stiffness.torch, limit_stiffness) + torch.testing.assert_close(articulation.data.spatial_tendon_offset.torch, offset) diff --git a/source/isaaclab_physx/test/assets/unit/test_actuator_control.py b/source/isaaclab_physx/test/assets/unit/test_physx_actuator_control.py similarity index 99% rename from source/isaaclab_physx/test/assets/unit/test_actuator_control.py rename to source/isaaclab_physx/test/assets/unit/test_physx_actuator_control.py index 92d376d3c5d..6082c734cd9 100644 --- a/source/isaaclab_physx/test/assets/unit/test_actuator_control.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_actuator_control.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused PhysX dual actuator-dispatch tests.""" +"""Focused PhysX dual actuator-dispatch unit tests.""" from types import SimpleNamespace from unittest.mock import Mock, call diff --git a/source/isaaclab_physx/test/assets/unit/test_articulation.py b/source/isaaclab_physx/test/assets/unit/test_physx_articulation.py similarity index 99% rename from source/isaaclab_physx/test/assets/unit/test_articulation.py rename to source/isaaclab_physx/test/assets/unit/test_physx_articulation.py index f4a616c477b..b0e8c361876 100644 --- a/source/isaaclab_physx/test/assets/unit/test_articulation.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_articulation.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused PhysX articulation staging, cache, friction, and kernel tests.""" +"""Focused PhysX articulation staging, cache, friction, and kernel unit tests.""" from types import SimpleNamespace from unittest.mock import patch diff --git a/source/isaaclab_physx/test/assets/unit/test_deformable_object.py b/source/isaaclab_physx/test/assets/unit/test_physx_deformable_object.py similarity index 99% rename from source/isaaclab_physx/test/assets/unit/test_deformable_object.py rename to source/isaaclab_physx/test/assets/unit/test_physx_deformable_object.py index 9f4ceffd1dd..053a9947a7f 100644 --- a/source/isaaclab_physx/test/assets/unit/test_deformable_object.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_deformable_object.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused PhysX deformable type, material, and kinematic-target tests.""" +"""Focused PhysX deformable type, material, and kinematic-target unit tests.""" from types import SimpleNamespace diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object.py similarity index 97% rename from source/isaaclab_physx/test/assets/unit/test_rigid_object.py rename to source/isaaclab_physx/test/assets/unit/test_physx_rigid_object.py index 45ef867729f..a0fe27280a8 100644 --- a/source/isaaclab_physx/test/assets/unit/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused PhysX rigid-object CPU staging and cached-view tests.""" +"""Focused PhysX rigid-object CPU staging and cached-view unit tests.""" import sys from types import SimpleNamespace diff --git a/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object_collection.py similarity index 98% rename from source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py rename to source/isaaclab_physx/test/assets/unit/test_physx_rigid_object_collection.py index 3185fac92fd..a2e4d88b50e 100644 --- a/source/isaaclab_physx/test/assets/unit/test_rigid_object_collection.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_rigid_object_collection.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused PhysX rigid-object-collection ordering and selector tests.""" +"""Focused PhysX rigid-object-collection ordering and selector unit tests.""" import numpy as np import pytest diff --git a/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py b/source/isaaclab_physx/test/assets/unit/test_physx_surface_gripper.py similarity index 97% rename from source/isaaclab_physx/test/assets/unit/test_surface_gripper.py rename to source/isaaclab_physx/test/assets/unit/test_physx_surface_gripper.py index 9bedd2b1e2b..092a7381b9a 100644 --- a/source/isaaclab_physx/test/assets/unit/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/unit/test_physx_surface_gripper.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused surface-gripper command, property, and device-guard tests.""" +"""Focused PhysX surface-gripper command, property, and device-guard unit tests.""" from types import SimpleNamespace from unittest.mock import Mock From a3d771174dcac581a3bd8326161a71d1e71fb7b0 Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Mon, 24 Aug 2026 10:56:03 +0200 Subject: [PATCH 25/26] Preserve focused rigid state assertions Retain the upstream link and center-of-mass alias checks in the reduced Newton integration seams. Refresh the benchmark report against the rebased develop head so the PR speed-up remains reproducible. --- ...08-21-asset-test-suite-redesign-results.md | 38 +++++++++---------- .../test/assets/test_rigid_object.py | 16 ++++++++ .../assets/test_rigid_object_collection.py | 5 +++ 3 files changed, 40 insertions(+), 19 deletions(-) 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 index 4361e57422e..5b109ab43d7 100644 --- 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 @@ -9,8 +9,8 @@ SPDX-License-Identifier: BSD-3-Clause ## Outcome -The comparable asset and WrenchComposer scopes now finish in **140.41 s** of -subprocess wall time, down from **1,197.60 s**: an **8.53x wall-time speed-up**. +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. @@ -23,7 +23,7 @@ deliberately excluded. ## Measurement environment -- Final branch base: `0451444c24` from `origin/develop`, rebased before the +- Final branch base: `6aad90be0a` from `origin/develop`, rebased before the final measurements. - Original baseline base: `d7033a5a1a207f1d4284edb60d72d7838984413b`. - Worktree-local environment: `env_isaaclab`, installed with @@ -39,19 +39,19 @@ deliberately excluded. ## Copy-ready PR performance section The asset-test redesign reduced the five comparable CI-style scopes from -19m57.60s to 2m20.41s wall time (**8.53x faster**). This comparison uses the +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 | 7.89 / 25.47 s | **3.30x** | -| Newton assets, no cable/MPM | 6 / 644 | 15 / 59 pass | 590.94 / 608.44 s | 16.29 / 50.49 s | **12.05x** | -| PhysX assets | 7 / 486 | 12 / 44 pass | 236.80 / 255.30 s | 6.21 / 32.54 s | **7.85x** | -| OV assets | 9 / 492 | 12 / 60 pass | 196.94 / 220.05 s | 5.41 / 27.27 s | **8.07x** | -| WrenchComposer | 3 / 412 | 2 / 15 pass | 23.34 / 29.79 s | 0.81 / 4.64 s | **6.42x** | -| **Aggregate** | **33 / 6,365** | **47 / 1,416 pass, 103 skip** | **1,112.72 / 1,197.60 s** | **36.61 / 140.41 s** | **8.53x** | +| 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 @@ -60,24 +60,24 @@ islands alive until every test node in the module has run. | Articulation backend | Before cases | After cases | Before pytest / wall | After pytest / wall | |---|---:|---:|---:|---:| -| Newton | 3 | 4 | 3.04 / 4.65 s | 2.54 / 4.11 s | -| PhysX | 2 | 5 | 4.94 / 5.75 s | 3.44 / 4.17 s | -| OVPhysX | 2 | 6 | 2.38 / 3.46 s | 2.53 / 3.66 s | -| **Aggregate** | **7** | **15** | **10.36 / 13.86 s** | **8.51 / 11.94 s** | +| Newton | 3 | 4 | 3.04 / 4.65 s | 2.41 / 4.10 s | +| PhysX | 2 | 5 | 4.94 / 5.75 s | 3.17 / 3.98 s | +| OVPhysX | 2 | 6 | 2.38 / 3.46 s | 2.22 / 3.46 s | +| **Aggregate** | **7** | **15** | **10.36 / 13.86 s** | **7.80 / 11.54 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 | 7.89 / 25.47 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.54 / 16.21 s | -| PhysX minimal real integration | 6 files, 11 pass | 4.52 / 21.18 s | -| OV minimal real integration | 4 files, 10 pass | 4.61 / 12.13 s | -| WrenchComposer real delivery | 1 file, 1 pass | 0.79 / 3.12 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 | diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index c92ad310f99..673ba3d2075 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -108,6 +108,22 @@ def test_rigid_object_real_newton_seams(device: str, monkeypatch) -> None: 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), + ) + torch.testing.assert_close( + rigid_object.data.root_com_pose_w.torch, + rigid_object.data.body_com_pose_w.torch.squeeze(1), + ) + torch.testing.assert_close( + rigid_object.data.body_link_pose_w.torch, + rigid_object.data.body_com_pose_w.torch, + ) + torch.testing.assert_close( + rigid_object.data.root_com_vel_w.torch[..., 3:], + rigid_object.data.body_com_vel_w.torch.squeeze(1)[..., 3:], + ) model_changes = [] add_model_change = SimulationManager.add_model_change 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 454a5e1da02..4822af620ed 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -81,6 +81,11 @@ def test_rigid_object_collection_real_newton_seams(monkeypatch) -> None: 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:], + ) model_changes = [] add_model_change = SimulationManager.add_model_change From e828d86cd4aaeb037d1adf18a5ba2b9702b6cdcd Mon Sep 17 00:00:00 2001 From: AntoineRichard Date: Mon, 24 Aug 2026 11:30:03 +0200 Subject: [PATCH 26/26] Test articulation backends with two envs Align the real articulation probes across Newton, PhysX, and OVPhysX while preserving backend-specific coverage. Exercise partial writes against two live environments in one cached scene so multi-environment behavior is proven without restoring repeated startup cost. --- ...08-21-asset-test-suite-redesign-results.md | 18 +- .../test/assets/test_articulation.py | 100 +++++--- .../assets/unit/test_rigid_assets_import.py | 6 +- .../test/assets/test_articulation.py | 187 ++++++++------ .../test/assets/test_articulation.py | 231 ++++++++++++------ 5 files changed, 347 insertions(+), 195 deletions(-) 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 index 5b109ab43d7..70a72b04a21 100644 --- 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 @@ -56,14 +56,16 @@ and are not included in the denominator. 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. +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.41 / 4.10 s | -| PhysX | 2 | 5 | 4.94 / 5.75 s | 3.17 / 3.98 s | -| OVPhysX | 2 | 6 | 2.38 / 3.46 s | 2.22 / 3.46 s | -| **Aggregate** | **7** | **15** | **10.36 / 13.86 s** | **7.80 / 11.54 s** | +| 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: @@ -196,7 +198,7 @@ TEST_RESULT_FILE=task8-final-integration-ov.xml \ | 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 nodes cover partial state/property/wrench, drive, Jacobian, and mass matrix without rebuilding the model. FK, staging, and ordering remain in units; IK/OSC/gravity remain in the controller 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. | @@ -210,7 +212,7 @@ absent from every benchmark command. | Old module | Disposition | Coverage owner | |---|---|---| -| `test_articulation.py` | Retained and consolidated | One local composite scene contains ordered/fixed, floating, and spatial-tendon islands. Five nodes cover state, raw properties, dynamics, root/COM state, wrench delivery, and tendon writes after one reset. | +| `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. | @@ -222,7 +224,7 @@ absent from every benchmark command. | Old module | Disposition | Coverage owner | |---|---|---| -| `test_articulation.py` | Retained and consolidated | One CUDA composite scene contains ordered/fixed, floating, spatial-tendon, and native-actuator islands. Six nodes cover state/properties, drive and dynamics, root/wrench behavior, tendon writes, and native effort after one reset. | +| `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. | diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 8298234155b..91010bd3b0f 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -121,14 +121,10 @@ def test_articulation_initialization_and_partial_state(articulation_scene: _Arti 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() initial_joint_vel = articulation.data.joint_vel.torch.clone() - target_root_pose = initial_root_pose[env_ids].clone() - target_root_pose[:, :3] += torch.tensor([0.2, -0.1, 0.3], device=device) target_joint_pos = torch.tensor([[0.25]], device=device) target_joint_vel = torch.tensor([[-0.5]], device=device) - articulation.write_root_link_pose_to_sim_index(root_pose=target_root_pose, env_ids=env_ids) articulation.write_joint_state_to_sim_index( position=target_joint_pos, velocity=target_joint_vel, @@ -136,18 +132,16 @@ def test_articulation_initialization_and_partial_state(articulation_scene: _Arti joint_ids=joint_ids, ) - torch.testing.assert_close(articulation.data.root_link_pose_w.torch[env_ids], target_root_pose) - torch.testing.assert_close(articulation.data.root_link_pose_w.torch[:1], initial_root_pose[:1]) 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_model_properties_notify_newton( +def test_articulation_joint_and_body_properties_round_trip( articulation_scene: _ArticulationScene, monkeypatch: pytest.MonkeyPatch ) -> None: - """Prove partial inertial-property writes notify the live Newton model.""" + """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) @@ -160,6 +154,19 @@ def record_model_change(change: ModelFlags) -> None: 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, + ) + 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) @@ -184,45 +191,25 @@ def record_model_change(change: ModelFlags) -> None: assert notifications == [ModelFlags.BODY_INERTIAL_PROPERTIES] -def test_articulation_dynamics_and_wrench_response(articulation_scene: _ArticulationScene) -> None: - """Prove live floating-base dynamics data and isolated wrench delivery.""" - articulation = articulation_scene.floating - device = articulation_scene.device - jacobians = articulation.data.body_link_jacobian_w.torch - mass_matrix = articulation.data.mass_matrix.torch - assert jacobians.device.type == torch.device(device).type - assert mass_matrix.device.type == torch.device(device).type - assert jacobians.shape == (2, 2, 6, 7) - assert mass_matrix.shape == (2, 7, 7) - assert torch.isfinite(jacobians).all() - assert torch.isfinite(mass_matrix).all() - - 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=torch.tensor([1], dtype=torch.int32, device=device), - body_ids=torch.tensor([0], dtype=torch.int32, device=device), - ) - 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) - - -def test_fixed_articulation_actuation_and_dynamics(articulation_scene: _ArticulationScene) -> None: +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.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() - target = articulation.data.joint_pos.torch.clone() - target[:, 0] = 0.05 - articulation.actuators.target_command.set_position_index(value=target) + 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() @@ -230,10 +217,43 @@ def test_fixed_articulation_actuation_and_dynamics(articulation_scene: _Articula 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) - assert torch.linalg.vector_norm(articulation.data.body_link_vel_w.torch[0, -1]) > 1e-3 + 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), + ) + 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) 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 index 29f615e1ff9..c6df010c39f 100644 --- a/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py +++ b/source/isaaclab_newton/test/assets/unit/test_rigid_assets_import.py @@ -19,9 +19,9 @@ ("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_model_properties_notify_newton"), - ("test_articulation.py", "test_articulation_dynamics_and_wrench_response"), - ("test_articulation.py", "test_fixed_articulation_actuation_and_dynamics"), + ("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"), ( diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index b1d7fdbde13..d2ad71245ba 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -34,6 +34,7 @@ pytestmark = pytest.mark.integration _FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" +_NUM_ENVS = 2 @dataclass @@ -71,9 +72,18 @@ def _apply_api_schema(prim, schema_name: str) -> None: def _spawn_articulation( - prim_path: str, *, fixed_base: bool, native_actuator: bool = False, spatial_tendon: bool = False + name: str, + *, + y_offset: float, + fixed_base: bool, + native_actuator: bool = False, + spatial_tendon: bool = False, ) -> Articulation: - """Spawn one cached local branching articulation island.""" + """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=[".*"], @@ -86,7 +96,7 @@ def _spawn_articulation( ) articulation = Articulation( ArticulationCfg( - prim_path=prim_path, + prim_path=f"{island_path}/Env_[^/]*/Robot", spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), actuators={"joints": actuator_cfg}, joint_ordering="mjwarp", @@ -94,31 +104,33 @@ def _spawn_articulation( ) ) stage = sim_utils.get_current_stage() - 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) + 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 @@ -128,10 +140,14 @@ def articulation_scene() -> _ArticulationScene: 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("/World/Ordered", fixed_base=True) - floating = _spawn_articulation("/World/Floating", fixed_base=False) - tendon = _spawn_articulation("/World/Tendon", fixed_base=True, spatial_tendon=True) - native = _spawn_articulation("/World/Native", fixed_base=True, native_actuator=True) if native_enabled else None + 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.reset() yield _ArticulationScene( sim=sim, @@ -153,27 +169,36 @@ def test_articulation_initialization_and_partial_state(articulation_scene: _Arti 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], 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[:, joint_ids] = target_position - expected_velocity[:, joint_ids] = target_velocity + 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=target_position, velocity=target_velocity, + env_ids=env_ids, joint_ids=joint_ids, ) torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + 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], + ) 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) @@ -183,13 +208,14 @@ def test_articulation_joint_and_body_properties_round_trip(articulation_scene: _ 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, ) 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[:, backend_joint_ids, 0] = static_friction.cpu() - expected_backend_friction[:, backend_joint_ids, 1] = dynamic_friction.cpu() - expected_backend_friction[:, backend_joint_ids, 2] = viscous_friction.cpu() + 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( wp.to_torch(articulation.root_view.get_attribute(TT.DOF_FRICTION_PROPERTIES)), expected_backend_friction, @@ -199,55 +225,63 @@ def test_articulation_joint_and_body_properties_round_trip(articulation_scene: _ 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, body_ids=body_ids) + articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) expected_raw_mass = raw_mass_before.clone() - expected_raw_mass[:, backend_body_ids] = masses.cpu() + 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[:, body_ids].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), body_ids=body_ids) + 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[:, backend_body_ids] = coms.cpu() + 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[:, body_ids].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, body_ids=body_ids) + articulation.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) expected_raw_inertia = raw_inertia_before.clone() - expected_raw_inertia[:, backend_body_ids] = inertias.cpu() + 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[:, body_ids], masses) - torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) - torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) + 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() - drive_target = articulation.data.joint_pos.torch.clone() - drive_target[:, 0] += 0.4 - articulation.actuators.target_command.set_position_index(value=drive_target, full_data=True) + 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, + ) articulation.write_data_to_sim() backend_target = wp.to_torch(articulation.root_view.get_attribute(TT.DOF_POSITION_TARGET)) - backend_to_user = list(articulation.joint_ordering.backend_to_user_indices) - torch.testing.assert_close(backend_target, drive_target[:, backend_to_user]) + 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.any(torch.abs(articulation.data.joint_pos.torch[:, 0] - initial_drive_position) > 1e-6) + 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 == (1, articulation.num_bodies - 1, 6, articulation.num_joints) - assert mass_matrix.shape == (1, articulation.num_joints, articulation.num_joints) + 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() @@ -261,21 +295,29 @@ def test_floating_articulation_root_and_wrench_response(articulation_scene: _Art device = articulation_scene.device assert articulation.is_initialized assert not articulation.is_fixed_base - assert articulation.data.root_link_pose_w.torch.shape == (1, 7) - assert articulation.data.root_com_pose_w.torch.shape == (1, 7) - assert articulation.data.body_link_pose_w.torch.shape == (1, articulation.num_bodies, 7) - assert articulation.data.body_com_pose_w.torch.shape == (1, articulation.num_bodies, 7) + 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=torch.tensor([[[8.0, 0.0, 0.0]]], device=device), torques=torch.zeros((1, 1, 3), device=device), - env_ids=torch.tensor([0], dtype=torch.int32, device=device), + env_ids=env_ids, body_ids=torch.tensor([0], dtype=torch.int32, device=device), ) 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[0, 0] > initial_velocity[0, 0] + 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) def test_spatial_tendon_properties_round_trip(articulation_scene: _ArticulationScene) -> None: @@ -284,19 +326,23 @@ def test_spatial_tendon_properties_round_trip(articulation_scene: _ArticulationS 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) - articulation.set_spatial_tendon_damping_index(damping=damping) - articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=limit_stiffness) - articulation.set_spatial_tendon_offset_index(offset=offset) - torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch, stiffness) - torch.testing.assert_close(articulation.data.spatial_tendon_damping.torch, damping) - torch.testing.assert_close(articulation.data.spatial_tendon_limit_stiffness.torch, limit_stiffness) - torch.testing.assert_close(articulation.data.spatial_tendon_offset.torch, offset) + 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: @@ -304,6 +350,7 @@ def test_native_actuator_submits_real_effort(articulation_scene: _ArticulationSc 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 diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 184f8640e3c..de0860671e5 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -20,12 +20,14 @@ from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.sim import SimulationContext, build_simulation_context pytestmark = pytest.mark.integration _FIXTURE = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" +_NUM_ENVS = 2 @dataclass @@ -39,43 +41,56 @@ class _ArticulationScene: device: str -def _spawn_articulation(prim_path: str, *, fixed_base: bool, spatial_tendon: bool = False) -> Articulation: - """Spawn one local branching articulation island.""" +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=prim_path, + prim_path=f"{island_path}/Env_[^/]*/Robot", spawn=sim_utils.UsdFileCfg(usd_path=str(_FIXTURE)), - actuators={}, + actuators={ + "joints": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=5.0, damping=0.5), + }, joint_ordering="mjwarp", body_ordering="mjwarp", ) ) stage = sim_utils.get_current_stage() - 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) + 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 @@ -84,9 +99,9 @@ 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("/World/Ordered", fixed_base=True) - floating = _spawn_articulation("/World/Floating", fixed_base=False) - tendon = _spawn_articulation("/World/Tendon", fixed_base=True, spatial_tendon=True) + 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) @@ -98,20 +113,22 @@ def test_articulation_initialization_and_partial_state(articulation_scene: _Arti assert articulation.is_fixed_base assert articulation.joint_ordering is not None assert articulation.body_ordering is not None - assert articulation.num_instances == 1 + assert articulation.num_instances == 2 assert articulation.num_joints >= 2 assert articulation.num_bodies >= 3 + 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[:, joint_ids] = target_position - expected_velocity[:, joint_ids] = target_velocity + 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=target_position, velocity=target_velocity, + env_ids=env_ids, joint_ids=joint_ids, ) torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) @@ -122,26 +139,55 @@ def test_articulation_initialization_and_partial_state(articulation_scene: _Arti ) -def test_articulation_model_properties_round_trip(articulation_scene: _ArticulationScene) -> None: - """Prove selected body and material properties round-trip through PhysX.""" +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, + ) + 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, body_ids=body_ids) - torch.testing.assert_close(articulation.data.body_mass.torch[:, body_ids], masses) + 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]) - coms = articulation.data.body_com_pose_b.torch[:, body_ids].clone() + 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, body_ids=body_ids) - torch.testing.assert_close(articulation.data.body_com_pose_b.torch[:, body_ids], coms) + 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]) - inertias = articulation.data.body_inertia.torch[:, body_ids].clone() + 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, body_ids=body_ids) - torch.testing.assert_close(articulation.data.body_inertia.torch[:, body_ids], inertias) + 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( @@ -157,25 +203,48 @@ def test_articulation_model_properties_round_trip(articulation_scene: _Articulat articulation.data.body_inertia.torch[:, body_backend_to_user], ) - materials = torch.empty((1, articulation.root_view.max_shapes, 3)) - materials[..., 0] = 0.91 - materials[..., 1] = 0.17 - materials[..., 2] = 0.63 + 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([0], dtype=wp.int32, device="cpu") + wp.from_torch(materials, dtype=wp.float32), wp.array([1], dtype=wp.int32, device="cpu") ) - torch.testing.assert_close(wp.to_torch(articulation.root_view.get_material_properties()), materials) + 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_dynamics_are_finite_and_fresh(articulation_scene: _ArticulationScene) -> None: - """Prove live Jacobian and mass data remain valid after a simulation step.""" +def test_articulation_drive_and_dynamics(articulation_scene: _ArticulationScene) -> None: + """Prove implicit drive delivery and live PhysX dynamics access.""" articulation = articulation_scene.ordered - articulation_scene.sim.step() - articulation.update(articulation_scene.sim.cfg.dt) + 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, + ) + 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 == (1, articulation.num_bodies - 1, 6, articulation.num_joints) - assert mass_matrix.shape == (1, articulation.num_joints, articulation.num_joints) + 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() @@ -186,25 +255,35 @@ def test_articulation_dynamics_are_finite_and_fresh(articulation_scene: _Articul 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 assert not articulation.is_fixed_base - assert articulation.data.root_link_pose_w.torch.shape == (1, 7) - assert articulation.data.root_com_pose_w.torch.shape == (1, 7) - assert articulation.data.body_link_pose_w.torch.shape == (1, articulation.num_bodies, 7) - assert articulation.data.body_com_pose_w.torch.shape == (1, articulation.num_bodies, 7) + 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=articulation_scene.device), - torques=torch.zeros((1, 1, 3), device=articulation_scene.device), - env_ids=torch.tensor([0], dtype=torch.int32, device=articulation_scene.device), - body_ids=torch.tensor([0], dtype=torch.int32, device=articulation_scene.device), + 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), ) 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[0, 0] > initial_velocity[0, 0] + 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) def test_spatial_tendon_properties_round_trip(articulation_scene: _ArticulationScene) -> None: @@ -212,16 +291,20 @@ def test_spatial_tendon_properties_round_trip(articulation_scene: _ArticulationS articulation = articulation_scene.tendon 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=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) - articulation.set_spatial_tendon_damping_index(damping=damping) - articulation.set_spatial_tendon_limit_stiffness_index(limit_stiffness=limit_stiffness) - articulation.set_spatial_tendon_offset_index(offset=offset) - torch.testing.assert_close(articulation.data.spatial_tendon_stiffness.torch, stiffness) - torch.testing.assert_close(articulation.data.spatial_tendon_damping.torch, damping) - torch.testing.assert_close(articulation.data.spatial_tendon_limit_stiffness.torch, limit_stiffness) - torch.testing.assert_close(articulation.data.spatial_tendon_offset.torch, offset) + 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)