diff --git a/.github/actions/run-package-tests/action.yml b/.github/actions/run-package-tests/action.yml index 2f32aa5f031c..e04859354305 100644 --- a/.github/actions/run-package-tests/action.yml +++ b/.github/actions/run-package-tests/action.yml @@ -47,6 +47,13 @@ inputs: spawned by tools/conftest.py (combined with device-split selectors). default: '' required: false + test-jobs: + description: >- + Number of test-file slots tools/conftest.py runs at once. Each file still runs in + its own pytest process; files listed in tools/test_settings.py PYTEST_WORKERS split + across several slots with pytest-xdist. Empty or 1 runs files one by one. + default: '' + required: false shard-index: description: 'Zero-based shard index' default: '' @@ -333,6 +340,7 @@ runs: filter-pattern: ${{ inputs.filter-pattern }} exclude-pattern: ${{ inputs.exclude-pattern }} test-k-expr: ${{ inputs.test-k-expr }} + test-jobs: ${{ inputs.test-jobs }} shard-index: ${{ inputs.shard-index }} shard-count: ${{ inputs.shard-count }} curobo-only: ${{ inputs.curobo-only }} diff --git a/.github/actions/run-tests/action.yml b/.github/actions/run-tests/action.yml index 510c9e4cc033..96953cf647a9 100644 --- a/.github/actions/run-tests/action.yml +++ b/.github/actions/run-tests/action.yml @@ -49,6 +49,13 @@ inputs: can deselect parametrized cases (e.g. "not ovphysx"). default: '' required: false + test-jobs: + description: >- + Number of test-file slots tools/conftest.py runs at once. Each file still runs in + its own pytest process; files listed in tools/test_settings.py PYTEST_WORKERS split + across several slots with pytest-xdist. Empty or 1 runs files one by one. + default: '' + required: false curobo-only: description: 'Run only cuRobo and SkillGen tests (requires the cuRobo Docker image)' default: 'false' @@ -155,13 +162,14 @@ runs: TEST_NODE_IDS_KEY: ${{ inputs.test-node-ids-key }} TEST_PATH: ${{ inputs.test-path }} TEST_K_EXPR_INPUT: ${{ inputs.test-k-expr }} + TEST_JOBS_INPUT: ${{ inputs.test-jobs }} CI_MARKER_INPUT: ${{ inputs.ci-marker }} VOLUME_MOUNT_SOURCE: ${{ inputs.volume-mount-source }} WARP_CACHE_HOST_DIR: ${{ inputs.warp-cache-host-dir }} WHEELHOUSE_HOST_DIR: ${{ inputs.wheelhouse-host-dir }} WHEELHOUSE_PACKAGES: ${{ inputs.wheelhouse-packages }} run: | - bash .github/actions/run-tests/run_tests.sh "$TEST_PATH" "$RESULT_FILE" "$CONTAINER_NAME" "$IMAGE_TAG" "$REPORTS_DIR" "$PYTEST_OPTIONS" "$FILTER_PATTERN" "$EXCLUDE_PATTERN" "$CUROBO_ONLY" "$INCLUDE_FILES" "$QUARANTINED_ONLY" "$SHARD_INDEX" "$SHARD_COUNT" "$VOLUME_MOUNT_SOURCE" "$EXTRA_PIP_PACKAGES" "$TEST_NODE_IDS_FILE" "$TEST_NODE_IDS_KEY" "$WHEELHOUSE_HOST_DIR" "$WHEELHOUSE_PACKAGES" "$TEST_K_EXPR_INPUT" "$CI_MARKER_INPUT" "$STANDALONE_SCRIPT_SCOPE" "$STANDALONE_SCRIPT_VISUALIZER" "$STANDALONE_SCRIPT_RUNTIME_GROUP" "$WARP_CACHE_HOST_DIR" "$EXTRA_UV_PACKAGES" "$OVRTX_SHADER_CACHE_HOST_DIR" + bash .github/actions/run-tests/run_tests.sh "$TEST_PATH" "$RESULT_FILE" "$CONTAINER_NAME" "$IMAGE_TAG" "$REPORTS_DIR" "$PYTEST_OPTIONS" "$FILTER_PATTERN" "$EXCLUDE_PATTERN" "$CUROBO_ONLY" "$INCLUDE_FILES" "$QUARANTINED_ONLY" "$SHARD_INDEX" "$SHARD_COUNT" "$VOLUME_MOUNT_SOURCE" "$EXTRA_PIP_PACKAGES" "$TEST_NODE_IDS_FILE" "$TEST_NODE_IDS_KEY" "$WHEELHOUSE_HOST_DIR" "$WHEELHOUSE_PACKAGES" "$TEST_K_EXPR_INPUT" "$CI_MARKER_INPUT" "$STANDALONE_SCRIPT_SCOPE" "$STANDALONE_SCRIPT_VISUALIZER" "$STANDALONE_SCRIPT_RUNTIME_GROUP" "$WARP_CACHE_HOST_DIR" "$EXTRA_UV_PACKAGES" "$OVRTX_SHADER_CACHE_HOST_DIR" "$TEST_JOBS_INPUT" - name: Kill container on cancellation if: cancelled() shell: bash diff --git a/.github/actions/run-tests/run_tests.sh b/.github/actions/run-tests/run_tests.sh index 655f3a3919f3..aa05831cbb00 100755 --- a/.github/actions/run-tests/run_tests.sh +++ b/.github/actions/run-tests/run_tests.sh @@ -38,6 +38,7 @@ run_tests() { local warp_cache_host_dir="${25}" local extra_uv_packages="${26}" local ovrtx_shader_cache_host_dir="${27}" + local test_jobs="${28}" local logs_pid="" local wait_pid="" local docker_wait_file="/tmp/.docker_exit_${container_name}" @@ -194,6 +195,11 @@ run_tests() { echo "Setting per-file pytest -k expression: $test_k_expr" fi + if [ -n "$test_jobs" ]; then + docker_env_args+=(-e "TEST_JOBS=$test_jobs") + echo "Setting TEST_JOBS=$test_jobs" + fi + if [ -n "$ci_marker" ]; then docker_env_args+=(-e "CI_MARKER=$ci_marker") echo "Setting CI_MARKER=$ci_marker" diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 0eb5d8c4602b..bafbebb87671 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -275,6 +275,8 @@ jobs: shard-index: "0" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-tasks-1-test test-isaaclab-tasks-2: @@ -301,6 +303,8 @@ jobs: shard-index: "1" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-tasks-2-test test-isaaclab-tasks-3: @@ -327,6 +331,8 @@ jobs: shard-index: "2" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-tasks-3-test test-isaaclab-core: @@ -352,6 +358,8 @@ jobs: shard-index: "0" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-core-1-test test-isaaclab-core-2: @@ -377,6 +385,8 @@ jobs: shard-index: "1" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-core-2-test test-isaaclab-core-3: @@ -402,6 +412,8 @@ jobs: shard-index: "2" shard-count: "3" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-core-3-test # Kit and non-Kit are written out rather than expressed as a matrix: these job @@ -486,6 +498,8 @@ jobs: isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_rl" extra-pip-packages: "leapp==0.6.1 torchrl>=0.13" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-rl-test test-isaaclab-mimic: @@ -507,6 +521,8 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_mimic" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-mimic-test test-isaaclab-contrib: @@ -528,6 +544,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_contrib" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-contrib-test test-isaaclab-teleop: @@ -549,6 +568,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_teleop" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-teleop-test test-isaaclab-visualizers: @@ -570,6 +592,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_visualizers" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-visualizers-test test-isaaclab-assets: @@ -591,6 +616,8 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_assets" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-assets-test test-isaaclab-experimental: @@ -612,6 +639,8 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_experimental" + warp-cache: restore + test-jobs: "4" container-name: isaac-lab-experimental-test test-isaaclab-newton: @@ -634,6 +663,8 @@ jobs: isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_newton" warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-newton-test test-isaaclab-physx: @@ -655,6 +686,9 @@ jobs: isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} filter-pattern: "isaaclab_physx" + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-physx-test test-isaaclab-ov: @@ -684,6 +718,9 @@ jobs: extra-pip-packages: ${{ env.USE_OVPHYSX_WHEELHOUSE == 'true' && steps.ov_pins.outputs.ovrtx || format('{0} {1}', steps.ov_pins.outputs.ovrtx, steps.ov_pins.outputs.ovphysx) }} wheelhouse-image: ${{ env.USE_OVPHYSX_WHEELHOUSE == 'true' && needs.config.outputs.ovphysx_wheelhouse_image || '' }} wheelhouse-packages: ${{ env.USE_OVPHYSX_WHEELHOUSE == 'true' && 'ovphysx' || '' }} + warp-cache: restore + ovrtx-shader-cache: restore + test-jobs: "4" container-name: isaac-lab-ov-test # Folded from the former standalone verify-base-non-root job: reuses the diff --git a/.github/workflows/tools-tests.yml b/.github/workflows/tools-tests.yml index 0903ebea5727..c76c0c3c1297 100644 --- a/.github/workflows/tools-tests.yml +++ b/.github/workflows/tools-tests.yml @@ -52,8 +52,9 @@ jobs: # flaky, so they run without an Isaac Sim install or a full project sync. flaky drives the # rerun in test_crash_during_a_flaky_retry_is_blamed_on_the_retried_test; without it # installed that test skips itself rather than failing, so keep it in this list. + # pytest-xdist likewise drives test_an_xdist_run_journals_each_event_once. - name: Install test dependencies - run: bash "$GITHUB_WORKSPACE/.github/actions/_lib/with-python-package-retries.sh" python3 -m pip install pytest junitparser flaky pyyaml + run: bash "$GITHUB_WORKSPACE/.github/actions/_lib/with-python-package-retries.sh" python3 -m pip install pytest pytest-xdist junitparser flaky pyyaml - name: Run tools tests env: @@ -65,5 +66,5 @@ jobs: # --noconftest keeps tools/conftest.py out of the session. That file is the CI test # orchestrator - its pytest_sessionstart scans source/ and scripts/ and runs the whole # suite, so loading it here would ignore the files named below. - run: python3 -m pytest tools/test_crash_journal.py tools/test_device_split.py + run: python3 -m pytest tools/test_crash_journal.py tools/test_device_split.py tools/test_file_scheduler.py .github/actions/_lib/test_registry_credential_fallback.py -v --noconftest diff --git a/AGENTS.md b/AGENTS.md index 78a307848df3..4474a56e436e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -10,6 +10,8 @@ - Use modern Python type hints, including `X | None` instead of `Optional[X]`. - Use `snake_case` for methods, functions, and CLI arguments. - Keep related public symbols discoverable through consistent prefixes. +- Keep Newton solver schema registration in the active manager's builder factory; the cloner must not depend on solver modules. +- Resolve Newton raycast BVH requirements before builder finalization; sensor task registration must not add a late BVH fallback. - Keep joint-wrench sensor coverage separate from articulation control-joint selection. Reuse cached body bindings without changing the shared view's joint filters or creating a second view for sensing. - Keep articulation ordering maps on articulation data; do not mirror maps or add cached ordering flags. diff --git a/conftest.py b/conftest.py index d5bf93c8ca11..05b3721a43be 100644 --- a/conftest.py +++ b/conftest.py @@ -51,10 +51,19 @@ def _journal_write(record: dict) -> None: The per-record flush is the whole point: it puts the data in the OS page cache before the next test starts, so a process killed by a signal cannot take down verdicts it had already reported. Journaling failures are swallowed — losing debug context must never fail a run. + + Under ``pytest-xdist`` the controller receives every worker's start, report and finish, so only + it journals those; each worker journaling too would record every event twice and leave a + worker crash that xdist recovered from looking like an in-flight test. The controller never + collects, so the ``collected`` record comes from the first worker instead (every worker + collects the same items). """ path = os.environ.get(JOURNAL_ENV_VAR) if not path: return + worker = os.environ.get("PYTEST_XDIST_WORKER") + if worker and (record["event"] != "collected" or worker != "gw0"): + return try: with open(path, "a", encoding="utf-8") as handle: handle.write(json.dumps(record, separators=(",", ":")) + "\n") diff --git a/pyproject.toml b/pyproject.toml index 493b18cd78eb..8ecc273c0e98 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -126,6 +126,8 @@ importers = [ test = [ "pytest", "pytest-mock", + # Splits a test file across worker processes; see TEST_PYTEST_WORKERS in tools/conftest.py. + "pytest-xdist", "junitparser", "flaky", # numba subclasses coverage.types.Tracer at import; >=7.6.1 restores that shim diff --git a/scripts/tutorials/04_sensors/run_frame_transformer.py b/scripts/tutorials/04_sensors/run_frame_transformer.py index 7346cd11e500..4b44fcf38d57 100644 --- a/scripts/tutorials/04_sensors/run_frame_transformer.py +++ b/scripts/tutorials/04_sensors/run_frame_transformer.py @@ -64,7 +64,7 @@ def define_sensor() -> FrameTransformer: """Defines the FrameTransformer sensor to add to the scene.""" # define offset rot_offset = math_utils.quat_from_euler_xyz(torch.zeros(1), torch.zeros(1), torch.tensor(-math.pi / 2)) - pos_offset = math_utils.quat_apply(rot_offset, torch.tensor([0.08795, 0.01305, -0.33797])) + pos_offset = math_utils.quat_apply(rot_offset[0], torch.tensor([0.08795, 0.01305, -0.33797])) # Example using .* to get full body + LF_FOOT frame_transformer_cfg = FrameTransformerCfg( diff --git a/source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst b/source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst new file mode 100644 index 000000000000..12d2287a6006 --- /dev/null +++ b/source/isaaclab/changelog.d/core-cleanup-benchmark-fixes.rst @@ -0,0 +1,9 @@ +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab.benchmark.measurements.TestPhase.from_json` raising ``TypeError`` for phases + serialized with :class:`~isaaclab.benchmark.measurements.TestPhaseEncoder` that have measurements, and + dropping the metadata of phases without measurements. This also affected + :meth:`~isaaclab.benchmark.measurements.TestPhase.aggregate_json_files`. +* Fixed :meth:`~isaaclab.benchmark.recorders.record_cpu_info.CPUInfoRecorder.get_data` raising ``KeyError`` + when called before the first :meth:`~isaaclab.benchmark.recorders.record_cpu_info.CPUInfoRecorder.update`. diff --git a/source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst b/source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst new file mode 100644 index 000000000000..b5dd14c2ad84 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-actuator-net-lstm-velocity-clipping.rst @@ -0,0 +1,8 @@ +Fixed +^^^^^ + +* Fixed :class:`~isaaclab.actuators.ActuatorNetLSTM` clipping its output with a zero joint velocity. The DC-motor + torque-speed limits now use the current joint velocity, as :class:`~isaaclab.actuators.ActuatorNetMLP` does. +* Passed measured joint velocity directly into DC-motor clipping, removing the cached velocity and its redundant + copies. The base ``_clip_effort`` accepts ``(effort, *args, **kwargs)``; custom overrides used by explicit PD and + neural-network actuators must accept ``(effort, joint_vel)``. Implicit actuator clipping still takes only effort. diff --git a/source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst b/source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst new file mode 100644 index 000000000000..1d1edd535f42 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-body-projected-gravity-multi-body.rst @@ -0,0 +1,10 @@ +Fixed +^^^^^ + +* Fixed :func:`~isaaclab.utils.math.quat_apply` and :func:`~isaaclab.utils.math.quat_apply_inverse` to broadcast + leading dimensions following NumPy rules. This fixed :func:`~isaaclab.envs.mdp.observations.body_projected_gravity_b` + for multiple selected bodies. **Breaking change:** results retain the broadcast batch shape, including singleton + dimensions. Use a quaternion of shape ``(4,)`` for an unbatched vector result of shape ``(3,)``. Incompatible batch + shapes now raise an error even when their element counts match; callers relying on flattened pairing must explicitly + reshape their inputs to matching batch shapes. Outputs may be noncontiguous for transposed inputs; use ``reshape`` + instead of ``view`` when flattening these results. diff --git a/source/isaaclab/changelog.d/ground-plane-walking-margin.rst b/source/isaaclab/changelog.d/ground-plane-walking-margin.rst new file mode 100644 index 000000000000..c878f766937e --- /dev/null +++ b/source/isaaclab/changelog.d/ground-plane-walking-margin.rst @@ -0,0 +1,6 @@ +Fixed +^^^^^ + +* Extended the default visual ground plane 50 m beyond the outermost environment + origins so locomotion robots did not immediately leave the visible floor in + large batches. Preserved metric texture tiling and the infinite collision plane. diff --git a/source/isaaclab/changelog.d/performance-test-scheduling.skip b/source/isaaclab/changelog.d/performance-test-scheduling.skip new file mode 100644 index 000000000000..df9521b1d5f9 --- /dev/null +++ b/source/isaaclab/changelog.d/performance-test-scheduling.skip @@ -0,0 +1 @@ +Regression coverage for performance test isolation and updated test fixtures for the parallel runner. diff --git a/source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst b/source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst new file mode 100644 index 000000000000..34dcf3eaf3dc --- /dev/null +++ b/source/isaaclab/changelog.d/pink-ik-concurrent-conversion.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed concurrent Pink IK controller initialization corrupting shared USD-to-URDF exports by locking + the output directory through conversion and model loading. diff --git a/source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst b/source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst new file mode 100644 index 000000000000..d853cf5d5b33 --- /dev/null +++ b/source/isaaclab/changelog.d/resolve-local-usd-remote-dependencies.rst @@ -0,0 +1,4 @@ +Fixed +^^^^^ + +* Followed USD dependencies through both local layers and remote assets before adding references, resolving nested remote references in working copies without editing authored layers or raw downloads. Preserved renderer-provided MDL identifiers and self-contained USDZ packages. Completed local trees skipped repeated traversal until their files changed or disappeared. diff --git a/source/isaaclab/changelog.d/test-audit-newton-tests.rst b/source/isaaclab/changelog.d/test-audit-newton-tests.rst new file mode 100644 index 000000000000..6686d1ea4a6d --- /dev/null +++ b/source/isaaclab/changelog.d/test-audit-newton-tests.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed :func:`~isaaclab.utils.warp.math_ops.transform_to_vec_quat` raising a Warp ``RuntimeError`` instead of its + documented ``ValueError`` for 4D transform arrays. diff --git a/source/isaaclab/isaaclab/actuators/actuator_base.py b/source/isaaclab/isaaclab/actuators/actuator_base.py index 3b8f181908c6..b904b6f512f9 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_base.py +++ b/source/isaaclab/isaaclab/actuators/actuator_base.py @@ -310,11 +310,15 @@ def compute( Helper functions. """ - def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: - """Clip the desired torques based on the motor limits. + def _clip_effort(self, effort: torch.Tensor, *args, **kwargs) -> torch.Tensor: + """Clip the desired effort using actuator effort limits. + + Model-specific inputs are handled by subclasses. Args: effort: The effort to clip [N or N·m, depending on joint type]. + *args: Model-specific positional inputs. Unused by the base implementation. + **kwargs: Model-specific keyword inputs. Unused by the base implementation. Returns: The clipped effort [N or N·m, depending on joint type]. diff --git a/source/isaaclab/isaaclab/actuators/actuator_net.py b/source/isaaclab/isaaclab/actuators/actuator_net.py index 148dce18481e..f96717d2bd0e 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_net.py +++ b/source/isaaclab/isaaclab/actuators/actuator_net.py @@ -88,7 +88,7 @@ def compute( self.computed_effort = torques.reshape(self._num_envs, self.num_joints) # clip the computed effort based on the motor limits - self.applied_effort = self._clip_effort(self.computed_effort) + self.applied_effort = self._clip_effort(self.computed_effort, joint_vel) # return torques control_action.joint_efforts = self.applied_effort @@ -152,9 +152,6 @@ def compute( # -- velocity self._joint_vel_history = self._joint_vel_history.roll(1, 1) self._joint_vel_history[:, 0] = joint_vel - # save current joint vel for dc-motor clipping - self._joint_vel[:] = joint_vel - # compute network inputs # -- positions pos_input = torch.cat([self._joint_pos_error_history[:, i].unsqueeze(2) for i in self.cfg.input_idx], dim=2) @@ -178,7 +175,7 @@ def compute( self.computed_effort = torques.view(self._num_envs, self.num_joints) * self.cfg.torque_scale # clip the computed effort based on the motor limits - self.applied_effort = self._clip_effort(self.computed_effort) + self.applied_effort = self._clip_effort(self.computed_effort, joint_vel) # return torques control_action.joint_efforts = self.applied_effort diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd.py b/source/isaaclab/isaaclab/actuators/actuator_pd.py index 70d2d21ecd31..b11efb4ad109 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_pd.py +++ b/source/isaaclab/isaaclab/actuators/actuator_pd.py @@ -325,7 +325,7 @@ def compute( # calculate the desired joint torques self.computed_effort = self.stiffness * error_pos + self.damping * error_vel + control_action.joint_efforts # clip the torques based on the motor limits - self.applied_effort = self._clip_effort(self.computed_effort) + self.applied_effort = self._clip_effort(self.computed_effort, joint_vel) # set the computed actions back into the control action control_action.joint_efforts = self.applied_effort control_action.joint_positions = None @@ -413,33 +413,19 @@ def __init__(self, cfg: DCMotorCfg, *args, **kwargs): self._vel_at_effort_lim = self.actuator_velocity_limit * ( 1 + self.actuator_effort_limit / self._saturation_effort ) - # prepare joint vel buffer for max effort computation - self._joint_vel = torch.zeros_like(self.computed_effort) # create buffer for zeros effort self._zeros_effort = torch.zeros_like(self.computed_effort) - """ - Operations. - """ - - def compute( - self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor - ) -> ArticulationActions: - # save current joint vel - self._joint_vel[:] = joint_vel - # calculate the desired joint torques - return super().compute(control_action, joint_pos, joint_vel) - """ Helper functions. """ - def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: - # save current joint vel - self._joint_vel[:] = torch.clip(self._joint_vel, min=-self._vel_at_effort_lim, max=self._vel_at_effort_lim) + def _clip_effort(self, effort: torch.Tensor, joint_vel: torch.Tensor) -> torch.Tensor: + # Clamp the local value without modifying the measured joint velocity. + joint_vel = torch.clip(joint_vel, min=-self._vel_at_effort_lim, max=self._vel_at_effort_lim) # compute torque limits - torque_speed_top = self._saturation_effort * (1.0 - self._joint_vel / self.actuator_velocity_limit) - torque_speed_bottom = self._saturation_effort * (-1.0 - self._joint_vel / self.actuator_velocity_limit) + torque_speed_top = self._saturation_effort * (1.0 - joint_vel / self.actuator_velocity_limit) + torque_speed_bottom = self._saturation_effort * (-1.0 - joint_vel / self.actuator_velocity_limit) # -- max limit max_effort = torch.clip(torque_speed_top, max=self.actuator_effort_limit) # -- min limit diff --git a/source/isaaclab/isaaclab/benchmark/measurements.py b/source/isaaclab/isaaclab/benchmark/measurements.py index 5004a694277c..d43ace55d3a3 100644 --- a/source/isaaclab/isaaclab/benchmark/measurements.py +++ b/source/isaaclab/isaaclab/benchmark/measurements.py @@ -291,7 +291,7 @@ def from_json(cls, m: dict) -> "TestPhase": curr_meas = BooleanMeasurement(name=meas["name"], bvalue=meas["bvalue"]) curr_run.measurements.append(curr_meas) - curr_run.metadata = TestPhase.metadata_from_dict(m["metadata"]) + curr_run.metadata = TestPhase.metadata_from_dict(m) return curr_run @classmethod diff --git a/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py b/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py index 13cf3225d376..e0f2aa263b01 100644 --- a/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py +++ b/source/isaaclab/isaaclab/benchmark/recorders/record_cpu_info.py @@ -70,9 +70,9 @@ def get_runtime_data(self) -> dict: def get_data(self) -> MeasurementData: return MeasurementData( measurements=[ - SingleMeasurement(name="CPU Utilization", value=self._cpu_runtime_info["mean"], unit="%"), - SingleMeasurement(name="CPU Utilization std", value=self._cpu_runtime_info["std"], unit="%"), - SingleMeasurement(name="CPU Utilization n", value=self._cpu_runtime_info["n"], unit=""), + SingleMeasurement(name="CPU Utilization", value=self._mean, unit="%"), + SingleMeasurement(name="CPU Utilization std", value=self._std, unit="%"), + SingleMeasurement(name="CPU Utilization n", value=self._n, unit=""), ], metadata=[ StringMetadata(name="cpu_name", data=self._cpu_hardware_info["name"]), diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index af9a36b03cb6..bed0f5e04fb1 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -14,10 +14,14 @@ from __future__ import annotations +import os +import tempfile +from contextlib import ExitStack from typing import TYPE_CHECKING, cast import numpy as np import torch +from filelock import FileLock from pink import solve_ik from pink.tasks import Task from qpsolvers.exceptions import SolverNotFound @@ -81,27 +85,28 @@ def __init__( # Validate consistency between controlled_joint_indices and configuration self._validate_consistency(cfg, controlled_joint_indices) - # Resolve URDF/mesh paths at runtime. If only usd_path is provided, convert USD→URDF first. - if cfg.urdf_path is None and cfg.usd_path is not None: - import tempfile - - urdf_output_dir = cfg.urdf_output_dir or tempfile.gettempdir() - urdf_path, mesh_path = controller_utils.convert_usd_to_urdf( - cfg.usd_path, urdf_output_dir, force_conversion=True + with ExitStack() as stack: + # Resolve URDF/mesh paths at runtime. If only usd_path is provided, convert USD→URDF first. + if cfg.urdf_path is None and cfg.usd_path is not None: + urdf_output_dir = cfg.urdf_output_dir or tempfile.gettempdir() + # Conversions share mesh filenames. Protect the output directory until Pinocchio finishes reading it. + stack.enter_context(FileLock(os.path.join(urdf_output_dir, ".isaaclab_urdf.lock"))) + urdf_path, mesh_path = controller_utils.convert_usd_to_urdf( + cfg.usd_path, urdf_output_dir, force_conversion=True + ) + else: + urdf_path = retrieve_file_path(cfg.urdf_path) if cfg.urdf_path else cfg.urdf_path + mesh_path = retrieve_file_path(cfg.mesh_path) if cfg.mesh_path else cfg.mesh_path + + if urdf_path is None: + raise ValueError("Either urdf_path or usd_path must be provided in the controller configuration") + + # Initialize the Kinematics model used by pink IK to control robot + self.pink_configuration = PinkKinematicsConfiguration( + urdf_path=urdf_path, + mesh_path=mesh_path, + controlled_joint_names=cfg.joint_names, ) - else: - urdf_path = retrieve_file_path(cfg.urdf_path) if cfg.urdf_path else cfg.urdf_path - mesh_path = retrieve_file_path(cfg.mesh_path) if cfg.mesh_path else cfg.mesh_path - - if urdf_path is None: - raise ValueError("Either urdf_path or usd_path must be provided in the controller configuration") - - # Initialize the Kinematics model used by pink IK to control robot - self.pink_configuration = PinkKinematicsConfiguration( - urdf_path=urdf_path, - mesh_path=mesh_path, - controlled_joint_names=cfg.joint_names, - ) # Find the initial joint positions by matching Pink's joint names to robot_cfg.init_state.joint_pos, # where the joint_pos keys may be regex patterns and the values are the initial positions. diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 383b391555f3..3ca6b9b1d404 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -169,7 +169,7 @@ def body_projected_gravity_b( [x,y,z]. Output is stacked horizontally per body. """ asset: Articulation = env.scene[asset_cfg.name] - body_quat = asset.data.body_quat_w.torch[:, asset_cfg.body_ids] + body_quat = asset.data.body_quat_w.torch[:, asset_cfg.body_ids].reshape(env.num_envs, -1, 4) # ``GRAVITY_VEC_W`` carries the per-env world-frame gravity in m/s^2 (Newton # backend) or scene-wide gravity (PhysX backend). gravity_w = asset.data.GRAVITY_VEC_W.torch diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index 04a74addedaf..29c13932c215 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -14,7 +14,6 @@ import numpy as np from filelock import FileLock -from isaaclab.utils.assets import check_file_path, retrieve_file_path from isaaclab.utils.version import has_kit from ... import converters, schemas @@ -711,17 +710,11 @@ def _spawn_from_usd_file( # the same cached USD files cause segfaults in Sdf_CrateFile::_MmapStream::Read. _world_size = int(os.environ.get("LOCAL_WORLD_SIZE", "1")) - file_status = check_file_path(usd_path) - if file_status == 0: - raise FileNotFoundError(f"USD file not found at path: '{usd_path}'.") - if _world_size > 1: lock = FileLock(os.path.join(tempfile.gettempdir(), "isaaclab_usd_spawn.lock")) else: lock = nullcontext() with lock: - if file_status == 2: - usd_path = retrieve_file_path(usd_path, force_download=False) stage = get_current_stage() if not stage.GetPrimAtPath(prim_path).IsValid(): create_prim( diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index 185ef9f6529d..27b2b0088e08 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -16,7 +16,7 @@ import torch -from ...utils.assets import check_file_path, retrieve_file_path +from ...utils.assets import retrieve_file_path from ...utils.string import to_camel_case from .queries import ( find_matching_prim_paths, @@ -912,16 +912,9 @@ def add_usd_reference( Raises: FileNotFoundError: When the input USD file is not found at the specified path. + RuntimeError: When retrieving the file or adding the USD reference fails. """ - # resolve remote USD paths to local (same as Newton / add_reference_to_stage) - file_status = check_file_path(usd_path) - if file_status == 0: - raise FileNotFoundError(f"Unable to open the usd file at path: {usd_path}") - if file_status == 2: - try: - usd_path = retrieve_file_path(usd_path, force_download=False) - except Exception as e: - raise FileNotFoundError(f"Failed to retrieve USD file from {usd_path}") from e + usd_path = retrieve_file_path(usd_path) stage = get_current_stage() if stage is None else stage prim = stage.GetPrimAtPath(prim_path) diff --git a/source/isaaclab/isaaclab/terrains/terrain_importer.py b/source/isaaclab/isaaclab/terrains/terrain_importer.py index 9629c9c48825..cb13afdefa3c 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_importer.py +++ b/source/isaaclab/isaaclab/terrains/terrain_importer.py @@ -193,8 +193,8 @@ def import_ground_plane(self, name: str, size: tuple[float, float] | None = None Args: name: The name of the imported terrain. This name is used to create the USD prim corresponding to the terrain. - size: The visual size of the plane [m]. If None, the visual mesh covers the configured - environment grid with a 100 m minimum. The collision plane remains infinite. + size: The visual size of the plane [m]. If None, the visual mesh extends 50 m beyond + each side of the configured environment grid. The collision plane remains infinite. Raises: ValueError: If a terrain with the same name already exists. @@ -256,11 +256,11 @@ def import_mesh(self, name: str, mesh: trimesh.Trimesh): ) def _compute_ground_plane_size(self) -> tuple[float, float]: - """Compute a bounded visual plane size that covers the environment grid [m].""" + """Cover the environment grid with 50 m of visual walking room on each side [m].""" num_rows = int(np.ceil(self.cfg.num_envs / np.sqrt(self.cfg.num_envs))) num_cols = int(np.ceil(self.cfg.num_envs / num_rows)) spacing = self.cfg.env_spacing or 0.0 - return (max(100.0, (num_rows + 1) * spacing), max(100.0, (num_cols + 1) * spacing)) + return ((num_rows - 1) * spacing + 100.0, (num_cols - 1) * spacing + 100.0) def _is_heightfield_collider_requested(self, cfg: TerrainGeneratorCfg) -> bool: """Check whether the generated terrain should be collided against as a heightfield. diff --git a/source/isaaclab/isaaclab/utils/assets.py b/source/isaaclab/isaaclab/utils/assets.py index 5f83354e7991..8683da835ed3 100644 --- a/source/isaaclab/isaaclab/utils/assets.py +++ b/source/isaaclab/isaaclab/utils/assets.py @@ -23,6 +23,7 @@ import subprocess import tempfile import uuid +from collections.abc import Iterator from types import ModuleType from typing import Literal, NotRequired, TypedDict from urllib.parse import urlparse @@ -34,7 +35,8 @@ logger = logging.getLogger(__name__) _UDIM_RE = re.compile(r"", re.IGNORECASE) -_USD_EXTENSIONS = {".usd", ".usda", ".usdc", ".usdz"} +# USDZ packages own their internal dependency layout and must not be rewritten. +_USD_EXTENSIONS = {".usd", ".usda", ".usdc"} _MDL_RESOURCE_RE = re.compile(r'"([^"\\]*(?:\\.[^"\\]*)*)"|/\*.*?\*/|//[^\r\n]*', re.DOTALL) _MDL_TEXTURE_RE = re.compile(r"\.(?:bmp|dds|exr|hdr|ies|jpe?g|ktx2?|png|tga|tiff?|tx)(?:[?#].*)?$", re.IGNORECASE) _MDL_MODULE_PATTERN = r"(?:(?:\.\.::)++|\.::)[A-Za-z_]\w*+(?:::[A-Za-z_]\w*+)*+(?:::\*)?" @@ -246,6 +248,9 @@ def _resolve_asset_root() -> str: """Source URL per locally cached copy, recorded as the copy is located rather than recovered from its path, so a cache path is never inferred from a directory that merely looks like one.""" +_LOCALIZED_ASSETS: dict[str, tuple[str, dict[str, tuple[int, int]]]] = {} +"""Original roots and file stamps of completed managed trees, recorded per process.""" + _GIT_SSH_RE = re.compile(r"^[^@/:]+@[^:]+:.+") @@ -620,9 +625,11 @@ def check_file_path(path: str) -> Literal[0, 1, 2]: def retrieve_file_path(path: str, download_dir: str | None = None, force_download: bool = False) -> str: """Retrieves the path to a file on the Nucleus Server or locally. - If the file exists locally, then the absolute path to the file is returned. - If the file exists on the Nucleus Server, then the file is downloaded to the local machine - and the absolute path to the file is returned. + Dependencies are traversed through local files and remote URLs. Changed USD layers + are written to working copies, preserving authored layers and raw downloads. Completed + managed copies skip discovery while their file stamps match. Remote URLs retain the + normal server freshness checks. Renderer module identifiers and USDZ packages + retain their existing resolution behavior. Args: path: The path to the file. @@ -636,95 +643,146 @@ def retrieve_file_path(path: str, download_dir: str | None = None, force_downloa Raises: FileNotFoundError: When the file not found locally or on Nucleus Server. - RuntimeError: When the file cannot be copied from the Nucleus Server to the local machine. This - can happen when the file already exists locally and :attr:`force_download` is set to False. + RuntimeError: When the root file or a forced dependency download fails, or a resolved USD copy + cannot be saved. """ - # check file status - file_status = check_file_path(path) - if file_status == 1: - return os.path.abspath(path) - elif file_status == 2: - omni_client = _get_omni_client() - - from ..app.loading_screen import report_activity + # Only trees completed by this process may bypass discovery. A download directory + # can also contain raw mirrors, caller-owned files, or interrupted downloads. + local_path = os.path.abspath(path) + completed = _LOCALIZED_ASSETS.get(local_path) + if completed is not None: + source, files = completed + if not force_download: + try: + if all(((stat := os.stat(file)).st_mtime_ns, stat.st_size) == stamp for file, stamp in files.items()): + return local_path + except OSError: + pass + path = source + else: + path = unmirror_file_path(local_path) or path - # resolve download directory - if download_dir is None: - download_dir = tempfile.gettempdir() + if check_file_path(path) == 0: + raise FileNotFoundError(f"Unable to find the file: {path}") + root = os.path.abspath(path) if os.path.isfile(path) else path.replace(os.sep, "/") + download_dir = os.path.abspath(download_dir or tempfile.gettempdir()) + prepared = {} + files = {} + complete = True + copy_dir = os.path.join(download_dir, f"isaaclab_usd_{uuid.uuid4().hex}") + + def prepare(source: str) -> str | None: + nonlocal complete + if source in prepared: + return prepared[source] + remote = bool(urlparse(source).scheme) and not os.path.isabs(source) + target = _mirror_path(source, download_dir) if remote else source + prepared[source] = None + if _UDIM_RE.search(source): + for tile in range(1001, 1101): + tile_path = _UDIM_RE.sub(str(tile), source) + if check_file_path(tile_path) == 0: + complete = complete and tile != 1001 + break + prepare(tile_path) + prepared[source] = target + return target + + # Read detached contents while the download lock protects the raw mirror. + suffix = os.path.splitext(target)[1].lower() + with _download_file(source, download_dir, force_download) as local_path: + if local_path is None: + if source == root or (force_download and remote): + raise RuntimeError(f"Unable to copy file: '{source}'") + complete = False + return None + stat = os.stat(local_path) + files[local_path] = (stat.st_mtime_ns, stat.st_size) + if suffix in _USD_EXTENSIONS: + from pxr import Ar, Sdf, UsdUtils # noqa: PLC0415 + + layer = Sdf.Layer.OpenAsAnonymous(local_path) + else: + refs = _find_mdl_dependencies(local_path) if suffix == ".mdl" else () + + prepared[source] = local_path + if suffix not in _USD_EXTENSIONS: + for ref in refs: + dependency = prepare(_resolve_reference_url(source, ref)) + complete = complete and dependency == _resolve_reference_url(local_path, ref) + return local_path + + # Reserve before descending so shared children and cycles have one destination. + output = os.path.join(copy_dir, str(len(prepared)), os.path.basename(local_path)) + prepared[source] = output + changed = False + + def rewrite(ref: str) -> str: + nonlocal changed + if not ref: + return ref + dependency = _resolve_reference_url(source, ref) + # Unresolved MDL search identifiers belong to the renderer's module path. + if ( + ref.endswith(".mdl") + and Ar.GetResolver().CreateIdentifier(ref, Ar.ResolvedPath(local_path)) == ref + and check_file_path(dependency) == 0 + ): + return ref + resolved = prepare(dependency) or dependency + changed |= resolved != _resolve_reference_url(local_path, ref) + return resolved + + UsdUtils.ModifyAssetPaths(layer, rewrite) + if changed: + os.makedirs(os.path.dirname(output)) + if not layer.Export(output): + raise RuntimeError(f"Unable to save resolved USD layer: {output}") + stat = os.stat(output) + files[output] = (stat.st_mtime_ns, stat.st_size) + if remote: + _MIRRORED_URLS[output] = source else: - download_dir = os.path.abspath(download_dir) - # create download directory if it does not exist - if not os.path.exists(download_dir): - os.makedirs(download_dir) - # recursive download: mirror remote tree under download_dir - remote_url = path.replace(os.sep, "/") - to_visit = [remote_url] - visited = set() - local_root = None - - report_activity("Loading assets") - while to_visit: - cur_url = to_visit.pop() - if cur_url in visited: - continue - visited.add(cur_url) - - # UDIM textures use a placeholder (e.g. texture..png) that does not - # correspond to a real file. Expand to individual tile URLs by probing tile numbers - # starting at 1001; UDIM tiles are contiguous so stop at the first missing tile. - if _UDIM_RE.search(cur_url): - for tile in range(1001, 1101): - tile_url = _UDIM_RE.sub(str(tile), cur_url) - if omni_client.stat(tile_url.replace(os.sep, "/"))[0] == omni_client.Result.OK: - if tile_url not in visited: - to_visit.append(tile_url) - else: - break - continue - - target_path = _mirror_path(cur_url, download_dir) - os.makedirs(os.path.dirname(target_path), exist_ok=True) - - is_root_asset = local_root is None - # Ranks can initialize against the same cold cache concurrently. Serialize a single - # mirrored file so no USD parser observes another rank overwriting it mid-read. - with FileLock(target_path + ".lock"): - # Re-check after acquiring the lock: another rank may have downloaded this asset - # while this rank was waiting. - if force_download or not _usable_mirror(cur_url, download_dir): - temporary_path = f"{target_path}.{uuid.uuid4().hex}.partial" - try: - result = omni_client.copy(cur_url, temporary_path, omni_client.CopyBehavior.OVERWRITE) - if result != omni_client.Result.OK: - if force_download or is_root_asset: - raise RuntimeError(f"Unable to copy file: '{cur_url}'. Is the Nucleus Server running?") - logger.debug("Skipping unavailable dependency: %s", cur_url) - continue - # A reader that does not take this process-local lock must never observe - # a partially copied USD file. - os.replace(temporary_path, target_path) - _write_mirror_fingerprint(cur_url, target_path) - finally: - with contextlib.suppress(OSError): - os.remove(temporary_path) - - # Resolve references while the mirror is stable. Each dependency gets its own - # lock below, preserving parallel initialization of unrelated asset trees. - references = _find_asset_dependencies(target_path) - - if local_root is None: - local_root = target_path - - # recurse into dependencies (USD references, payloads, MDL textures, etc.) - for ref in references: - ref_url = _resolve_reference_url(cur_url, ref) - if ref_url and ref_url not in visited: - to_visit.append(ref_url) + prepared[source] = local_path + return prepared[source] + from ..app.loading_screen import report_activity + + report_activity("Loading assets") + try: + result = prepare(root) + if complete: + for source, local_path in prepared.items(): + if source != local_path: + _LOCALIZED_ASSETS[local_path] = (source, files) + return result + finally: report_activity(None) - return os.path.abspath(local_root) - else: - raise FileNotFoundError(f"Unable to find the file: {path}") + + +@contextlib.contextmanager +def _download_file(source: str, download_dir: str, force_download: bool) -> Iterator[str | None]: + """Yield a local file while holding its remote mirror's download lock.""" + if os.path.isabs(source) or not urlparse(source).scheme: + yield source if os.path.isfile(source) else None + return + omni_client = _get_omni_client() + target = _mirror_path(source, download_dir) + os.makedirs(os.path.dirname(target), exist_ok=True) + with FileLock(target + ".lock"): + if force_download or not _usable_mirror(source, download_dir): + temporary_path = f"{target}.{uuid.uuid4().hex}.partial" + try: + result = omni_client.copy(source, temporary_path, omni_client.CopyBehavior.OVERWRITE) + if result != omni_client.Result.OK: + yield None + return + os.replace(temporary_path, target) + _write_mirror_fingerprint(source, target) + finally: + with contextlib.suppress(OSError): + os.remove(temporary_path) + yield target def read_file(path: str) -> io.BytesIO: @@ -764,61 +822,15 @@ def read_file(path: str) -> io.BytesIO: raise FileNotFoundError(f"Unable to find the file: {path}") -def _find_asset_dependencies(local_asset_path: str) -> set[str]: - """Collect external asset dependencies from a local asset file. - - USD layers are parsed with OpenUSD. MDL files are scanned for quoted texture - resources and relative module imports because those references are resolved - later by the MDL compiler and are not reported by USD dependency discovery. - """ - suffix = os.path.splitext(local_asset_path)[1].lower() - - if suffix == ".mdl": - try: - with open(local_asset_path, encoding="utf-8") as f: - source = f.read() - except OSError as e: - logger.warning("Failed to open MDL file: %s (%s)", local_asset_path, e) - return set() - - return _find_mdl_dependencies(source) - - if suffix not in _USD_EXTENSIONS: - return set() - - from pxr import Sdf, UsdUtils # noqa: PLC0415 - +def _find_mdl_dependencies(local_path: str) -> set[str]: + """Collect MDL resources and relative imports that USD does not discover.""" try: - layer = Sdf.Layer.FindOrOpen(local_asset_path) - except Exception: - logger.warning("Failed to open USD layer: %s", local_asset_path, exc_info=True) + with open(local_path, encoding="utf-8") as f: + source = f.read() + except OSError as e: + logger.warning("Failed to open MDL file: %s (%s)", local_path, e) return set() - if layer is None: - return set() - - refs: set[str] = set() - - def _collect(path: str) -> str: - """Record an asset path. - - Args: - path: Asset path from the USD layer. - - Returns: - The input path unchanged. - """ - if path: - refs.add(path) - return path - - UsdUtils.ModifyAssetPaths(layer, _collect) - - return refs - - -def _find_mdl_dependencies(source: str) -> set[str]: - """Collect local asset dependencies from MDL source text.""" refs = set() for match in _MDL_RESOURCE_RE.finditer(source): diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index 1c298a9d856c..1a454d80acc2 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -634,44 +634,36 @@ def quat_box_plus(q: torch.Tensor, delta: torch.Tensor, eps: float = 1.0e-6) -> def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Apply a quaternion rotation to a vector. + Leading dimensions follow NumPy broadcasting rules. Incompatible batch shapes raise an error. + Args: quat: The quaternion in (x, y, z, w). Shape is (..., 4). vec: The vector in (x, y, z). Shape is (..., 3). Returns: - The rotated vector in (x, y, z). Shape is (..., 3). + The rotated vector in (x, y, z). Shape is the broadcast batch shape followed by (3,). """ - # store shape - shape = vec.shape - # reshape to (N, 3) for multiplication - quat = quat.reshape(-1, 4) - vec = vec.reshape(-1, 3) - # extract components from quaternions (xyzw format) - xyz = quat[:, :3] + xyz, vec = torch.broadcast_tensors(quat[..., :3], vec) t = xyz.cross(vec, dim=-1) * 2 - return (vec + quat[:, 3:4] * t + xyz.cross(t, dim=-1)).view(shape) + return vec + quat[..., 3:4] * t + xyz.cross(t, dim=-1) @torch.jit.script def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: """Apply an inverse quaternion rotation to a vector. + Leading dimensions follow NumPy broadcasting rules. Incompatible batch shapes raise an error. + Args: quat: The quaternion in (x, y, z, w). Shape is (..., 4). vec: The vector in (x, y, z). Shape is (..., 3). Returns: - The rotated vector in (x, y, z). Shape is (..., 3). + The rotated vector in (x, y, z). Shape is the broadcast batch shape followed by (3,). """ - # store shape - shape = vec.shape - # reshape to (N, 3) for multiplication - quat = quat.reshape(-1, 4) - vec = vec.reshape(-1, 3) - # extract components from quaternions (xyzw format) - xyz = quat[:, :3] + xyz, vec = torch.broadcast_tensors(quat[..., :3], vec) t = xyz.cross(vec, dim=-1) * 2 - return (vec - quat[:, 3:4] * t + xyz.cross(t, dim=-1)).view(shape) + return vec - quat[..., 3:4] * t + xyz.cross(t, dim=-1) @torch.jit.script diff --git a/source/isaaclab/isaaclab/utils/warp/math_ops.py b/source/isaaclab/isaaclab/utils/warp/math_ops.py index 36a0f960a8b7..e23e0676f0e8 100644 --- a/source/isaaclab/isaaclab/utils/warp/math_ops.py +++ b/source/isaaclab/isaaclab/utils/warp/math_ops.py @@ -21,14 +21,16 @@ def transform_to_vec_quat( Raises: TypeError: If *t* does not have dtype ``wp.transformf``. + ValueError: If *t* has more than 3 dimensions. """ if t.dtype != wp.transformf: raise TypeError(f"Expected wp.transformf array, got dtype={t.dtype}") + # Check before viewing: the float view adds a dimension, which Warp rejects for a 4D input. + if t.ndim > 3: + raise ValueError(f"Expected 1D, 2D, or 3D transform array, got ndim={t.ndim}") floats = t.view(wp.float32) if t.ndim == 1: return floats[:, :3].view(wp.vec3f), floats[:, 3:].view(wp.quatf) if t.ndim == 2: return floats[:, :, :3].view(wp.vec3f), floats[:, :, 3:].view(wp.quatf) - if t.ndim == 3: - return floats[:, :, :, :3].view(wp.vec3f), floats[:, :, :, 3:].view(wp.quatf) - raise ValueError(f"Expected 1D, 2D, or 3D transform array, got ndim={t.ndim}") + return floats[:, :, :, :3].view(wp.vec3f), floats[:, :, :, 3:].view(wp.quatf) diff --git a/source/isaaclab/test/actuators/test_dc_motor.py b/source/isaaclab/test/actuators/test_dc_motor.py index 67ea950f99a9..22022f950970 100644 --- a/source/isaaclab/test/actuators/test_dc_motor.py +++ b/source/isaaclab/test/actuators/test_dc_motor.py @@ -6,7 +6,7 @@ import pytest import torch -from isaaclab.actuators import DCMotorCfg +from isaaclab.actuators import ActuatorNetLSTMCfg, DCMotorCfg from isaaclab.utils.types import ArticulationActions pytestmark = pytest.mark.integration @@ -130,13 +130,15 @@ def test_dc_motor_clip(test_point): torque, speed = torque_speed_pairs[test_point] zeros = torch.zeros(num_envs, num_joints, device=device) + joint_vel = torch.full_like(zeros, speed) control_action = ArticulationActions( joint_positions=zeros.clone(), joint_velocities=zeros.clone(), joint_efforts=torch.full_like(zeros, torque) ) - applied = actuator.compute(control_action, joint_pos=zeros, joint_vel=torch.full_like(zeros, speed)) + applied = actuator.compute(control_action, joint_pos=zeros, joint_vel=joint_vel) expected = torch.full_like(zeros, expected_clipped_effort[test_point]) torch.testing.assert_close(actuator.applied_effort, expected) torch.testing.assert_close(applied.joint_efforts, expected) + torch.testing.assert_close(joint_vel, torch.full_like(joint_vel, speed)) def test_dc_motor_clip_with_per_joint_saturation_effort(): @@ -167,6 +169,41 @@ def test_dc_motor_clip_with_per_joint_saturation_effort(): # at half the no-load speed each joint delivers half of its own stall torque, and the shared # effort limit is high enough to clip neither - actuator._joint_vel[:] = 25.0 - clipped_effort = actuator._clip_effort(torch.full((1, 2), 500.0, device=device)) + joint_vel = torch.full((1, 2), 25.0, device=device) + clipped_effort = actuator._clip_effort(torch.full((1, 2), 500.0, device=device), joint_vel) torch.testing.assert_close(clipped_effort, torch.tensor([[50.0, 95.0]], device=device)) + + +class _ConstantTorqueLSTM(torch.nn.Module): + """LSTM-shaped network that always requests 100 N·m.""" + + def __init__(self): + super().__init__() + self.lstm = torch.nn.LSTM(input_size=2, hidden_size=4, num_layers=1, batch_first=True) + + def forward( + self, x: torch.Tensor, hc: tuple[torch.Tensor, torch.Tensor] + ) -> tuple[torch.Tensor, tuple[torch.Tensor, torch.Tensor]]: + return torch.full((x.shape[0], 1), 100.0), hc + + +def test_lstm_actuator_clips_with_torque_speed_curve(tmp_path): + """LSTM compute must supply the current velocity to DC-motor clipping.""" + network_file = tmp_path / "constant_lstm.pt" + torch.jit.script(_ConstantTorqueLSTM()).save(str(network_file)) + cfg = ActuatorNetLSTMCfg( + joint_names_expr=["joint_.*"], + network_file=str(network_file), + saturation_effort=120.0, + actuator_effort_limit=80.0, + actuator_velocity_limit=7.5, + ) + actuator = cfg.class_type(cfg, joint_names=["joint_0", "joint_1"], joint_ids=[0, 1], num_envs=2, device="cpu") + + zeros = torch.zeros(2, 2) + joint_vel = torch.tensor([[0.0, 3.75], [7.5, -7.5]]) + action = actuator.compute(ArticulationActions(joint_positions=zeros), zeros, joint_vel) + + # Positive torque falls to zero at the velocity limit; braking torque remains available. + torch.testing.assert_close(action.joint_efforts, torch.tensor([[80.0, 60.0], [0.0, 80.0]])) + assert "_joint_vel" not in vars(actuator) diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py index 6d0a54a8167d..e809aab7f509 100644 --- a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -175,6 +175,7 @@ def test_ideal_pd_compute(effort_lim): actuator.applied_effort, computed_control_action.joint_efforts, ) + torch.testing.assert_close(actuator._clip_effort(actuator.computed_effort), actuator.applied_effort) if __name__ == "__main__": diff --git a/source/isaaclab/test/benchmark/test_formatters.py b/source/isaaclab/test/benchmark/test_formatters.py index 8f8198f6e1d4..c4ecc953d836 100644 --- a/source/isaaclab/test/benchmark/test_formatters.py +++ b/source/isaaclab/test/benchmark/test_formatters.py @@ -14,7 +14,7 @@ import pytest from isaaclab.benchmark import formatters -from isaaclab.benchmark.measurements import SingleMeasurement, StringMetadata, TestPhase +from isaaclab.benchmark.measurements import SingleMeasurement, StringMetadata, TestPhase, TestPhaseEncoder from isaaclab.benchmark.schema import ( GpuDeviceInfo, Hardware, @@ -29,6 +29,19 @@ ) +@pytest.mark.parametrize("measurements", [[SingleMeasurement(name="fps", value=60.0, unit="Hz")], []]) +def test_phase_json_round_trip_keeps_metadata(measurements) -> None: + """A phase serialized with TestPhaseEncoder reads back with its metadata, with or without measurements.""" + phase = TestPhase( + phase_name="runtime", measurements=measurements, metadata=[StringMetadata(name="gpu", data="A10")] + ) + + restored = TestPhase.from_json(json.loads(json.dumps(phase, cls=TestPhaseEncoder))) + + assert [(m.name, m.value) for m in restored.measurements] == [(m.name, m.value) for m in measurements] + assert [(m.name, m.data) for m in restored.metadata] == [("gpu", "A10")] + + def test_default_output_filenames_are_unique_with_identical_timestamps(monkeypatch) -> None: class FixedDatetime: @classmethod diff --git a/source/isaaclab/test/benchmark/test_recorders.py b/source/isaaclab/test/benchmark/test_recorders.py index 0706a59dc256..cd5c4d39a90c 100644 --- a/source/isaaclab/test/benchmark/test_recorders.py +++ b/source/isaaclab/test/benchmark/test_recorders.py @@ -48,7 +48,8 @@ def test_get_runtime_data_after_updates(self, recorder): assert isinstance(data["cpu_utilization"]["n"], int) def test_get_data_measurement_names(self, recorder): - """Test that get_data returns measurements with correct names.""" + """Test get_data before the first update and the measurement names after updates.""" + assert len(recorder.get_data().measurements) == 3 for _ in range(3): recorder.update() diff --git a/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py b/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py index 98a501f70580..07d712f4ba0b 100644 --- a/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py +++ b/source/isaaclab/test/cli/test_test_orchestrator_result_handling.py @@ -144,6 +144,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[missing_node_id], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -180,6 +181,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -218,6 +220,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr="ovphysx", suffix="") @@ -251,6 +254,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -287,7 +291,7 @@ def test_abnormal_termination_report_quotes_bounded_renderer_log( # quietly turn this into a test of a log that fits inside it. filler_lines = orchestrator.ovrtx_log.LOG_LIMIT_BYTES // len("filler-line\n") + 1 - def _capture(cmd, timeout, env, *, startup_deadline, report_file): + def _capture(cmd, timeout, env, *, startup_deadline, report_file, options): # Render verbosely, then die without writing a report. Path(log_path).write_text("head-line\n" + "filler-line\n" * filler_lines + "tail-line\n", encoding="utf-8") report_paths.append(Path(report_file)) @@ -308,6 +312,7 @@ def _capture(cmd, timeout, env, *, startup_deadline, report_file): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) with caplog.at_level("INFO"): @@ -369,6 +374,7 @@ def _capture(_cmd, _timeout, env, *, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) _report, status, _was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -390,7 +396,7 @@ def test_shutdown_hang_after_report_is_not_a_failure(monkeypatch, tmp_path: Path test_file = tmp_path / "test_sample.py" test_file.write_text("def test_present():\n pass\n", encoding="utf-8") - def _capture(cmd, timeout, env, *, startup_deadline, report_file): + def _capture(cmd, timeout, env, *, startup_deadline, report_file, options): _write_partial_junit_report(report_file) return -1, b"", b"", "shutdown_hang", 30.0, "" @@ -407,6 +413,7 @@ def _capture(cmd, timeout, env, *, startup_deadline, report_file): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) _report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -444,6 +451,7 @@ def _capture(*_args, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) _report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -494,6 +502,7 @@ def _capture(_cmd, _timeout, env, *, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -571,6 +580,7 @@ def _capture(_cmd, _timeout, env, *, report_file: str, **_kwargs): env={}, inject_shard_select=False, pytest_targets=[str(test_file)], + capture=orchestrator._CaptureOptions(), ) report, status, was_failure = orchestrator._run_one_pass(context, k_expr=None, suffix="") @@ -678,6 +688,26 @@ def test_hang_dump_plugin_is_inert_without_signal_support(monkeypatch) -> None: hang_dump.pytest_configure(config=None) # must not raise +def test_performance_files_reserve_the_runner_without_xdist(monkeypatch, tmp_path): + """Reserving every slot for timing tests must not split their measurements across pytest workers.""" + orchestrator = _load_orchestrator_module() + performance_files = ["test_kit_startup_performance.py", "test_robot_load_performance.py"] + files = [str(tmp_path / name) for name in (*performance_files, "test_regular.py")] + observed = {} + + def run(job, context, *, workers, **kwargs): + observed[Path(job.path).name] = (job.slots, workers) + return orchestrator._FileResult(reports=[], status={}, failed=False) + + monkeypatch.setenv("TEST_JOBS", "4") + monkeypatch.delenv("ISAACLAB_TEST_QUEUE", raising=False) + monkeypatch.setattr(orchestrator, "_run_test_file", run) + orchestrator.run_individual_tests(files, str(tmp_path), ci_marker=None) + + assert all(observed[name] == (4, 1) for name in performance_files) + assert observed["test_regular.py"] == (1, 1) + + def test_external_git_asset_tests_receive_extended_startup_deadline(): """Environment discovery must allow a cold external Git asset clone to complete.""" orchestrator = _load_orchestrator_module() diff --git a/source/isaaclab/test/controllers/test_pink_ik_components.py b/source/isaaclab/test/controllers/test_pink_ik_components.py index 8dca1995e1bb..42ec7d19d853 100644 --- a/source/isaaclab/test/controllers/test_pink_ik_components.py +++ b/source/isaaclab/test/controllers/test_pink_ik_components.py @@ -5,7 +5,9 @@ """Test cases for PinkKinematicsConfiguration class.""" +from concurrent.futures import ThreadPoolExecutor from pathlib import Path +from threading import Event import numpy as np import pinocchio as pin @@ -17,6 +19,58 @@ pytestmark = pytest.mark.integration +@pytest.mark.parametrize("load_fails", [False, True]) +def test_concurrent_controller_conversion_preserves_model(monkeypatch, tmp_path, load_fails): + """Another controller must not overwrite an export before its consumer finishes loading it.""" + from isaaclab.assets import ArticulationCfg + from isaaclab.controllers.pink_ik import PinkIKControllerCfg, pink_ik + + urdf = (Path(__file__).parent / "urdfs/test_urdf_two_link_robot.urdf").read_text() + first_loading, second_started, second_converted = Event(), Event(), Event() + + def convert(usd_path, output_path, force_conversion): + output = Path(output_path) / "robot.urdf" + name = Path(usd_path).parent.name + output.write_text(urdf.replace("test_two_link_robot", name)) + if name == "second": + second_converted.set() + return str(output), "" + + def load(**kwargs): + if not first_loading.is_set(): + first_loading.set() + assert second_started.wait(5) + # Give the contender a chance to overwrite the URDF while this reader is paused. + second_converted.wait(1) + if load_fails: + raise RuntimeError("model loading failed") + return PinkKinematicsConfiguration(**kwargs) + + def initialize(name): + cfg = PinkIKControllerCfg( + usd_path=f"/{name}/robot.usd", + urdf_output_dir=str(tmp_path), + joint_names=["joint_1", "joint_2"], + all_joint_names=["joint_1", "joint_2"], + ) + if name == "second": + second_started.set() + return pink_ik.PinkIKController(cfg, ArticulationCfg(), "cpu", [0, 1]) + + monkeypatch.setattr(pink_ik.controller_utils, "convert_usd_to_urdf", convert) + monkeypatch.setattr(pink_ik, "PinkKinematicsConfiguration", load) + with ThreadPoolExecutor(max_workers=2) as pool: + first = pool.submit(initialize, "first") + assert first_loading.wait(5) + second = pool.submit(initialize, "second") + if load_fails: + with pytest.raises(RuntimeError, match="model loading failed"): + first.result(timeout=10) + else: + assert first.result(timeout=10).pink_configuration.full_model.name == "first" + assert second.result(timeout=10).pink_configuration.full_model.name == "second" + + class TestPinkKinematicsConfiguration: """Test suite for PinkKinematicsConfiguration class.""" diff --git a/source/isaaclab/test/envs/test_gravity_randomization.py b/source/isaaclab/test/envs/test_gravity_randomization.py index 0668cfc0b7b7..710dfbb0508c 100644 --- a/source/isaaclab/test/envs/test_gravity_randomization.py +++ b/source/isaaclab/test/envs/test_gravity_randomization.py @@ -3,15 +3,17 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for scene-wide gravity randomization.""" +"""Tests for gravity randomization and observations.""" +import math from types import SimpleNamespace import pytest import torch from isaaclab.envs.mdp.events import randomize_physics_scene_gravity -from isaaclab.managers import EventTermCfg +from isaaclab.envs.mdp.observations import body_projected_gravity_b +from isaaclab.managers import EventTermCfg, SceneEntityCfg @pytest.mark.parametrize("backend", ["physx", "ovphysx"]) @@ -45,3 +47,26 @@ def test_scene_wide_backends_use_configured_distribution(monkeypatch: pytest.Mon torch.manual_seed(0) gravity_event(env, env_ids=None, **cfg.params) assert gravity_sink.value == pytest.approx((1.0, 2.0, 3.0)) + + +@pytest.mark.unit +def test_body_projected_gravity_b_stacks_every_selected_body(): + """Each selected body receives its own environment's gravity, including integer selections.""" + num_envs = 2 + angles = (0.0, 0.5 * math.pi, math.pi) + body_quat = torch.tensor([[math.sin(0.5 * a), 0.0, 0.0, math.cos(0.5 * a)] for a in angles]).repeat(num_envs, 1, 1) + asset = SimpleNamespace( + data=SimpleNamespace( + body_quat_w=SimpleNamespace(torch=body_quat), + GRAVITY_VEC_W=SimpleNamespace(torch=torch.tensor([[0.0, 0.0, -9.81], [0.0, 9.81, 0.0]])), + ) + ) + env = SimpleNamespace(scene={"robot": asset}, num_envs=num_envs) + # R_x(a)^T applied to -Z in the first environment and +Y in the second. + expected_z = [[0.0, -math.sin(a), -math.cos(a)] for a in angles] + expected_y = [[0.0, math.cos(a), -math.sin(a)] for a in angles] + expected = torch.tensor([expected_z, expected_y]).reshape(num_envs, -1) + torch.testing.assert_close(body_projected_gravity_b(env, SceneEntityCfg("robot")), expected) + for body_ids in ([1], 1): + asset_cfg = SceneEntityCfg("robot", body_ids=body_ids) + torch.testing.assert_close(body_projected_gravity_b(env, asset_cfg), expected[:, 3:6]) diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index 1ef078c5fd21..e8cd3f847fb2 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -81,7 +81,8 @@ def test_visual_material_defaults(): assert unmaterialized_generator_cfg.visual_material is None -def test_plane(): +@pytest.mark.parametrize("num_envs,env_spacing", [(1, 1.0), (4096, 4.0), (16384, 2.5)]) +def test_plane(num_envs, env_spacing): """Generates a plane and tests that the resulting mesh has the correct size.""" with build_simulation_context(device="cuda:0", auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -90,47 +91,43 @@ def test_plane(): terrain_importer_cfg = terrain_gen.TerrainImporterCfg( prim_path="/World/ground", terrain_type="plane", - num_envs=4096, - env_spacing=4.0, + num_envs=num_envs, + env_spacing=env_spacing, visual_material=PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), ) terrain_importer = TerrainImporter(terrain_importer_cfg) - # The importer lays the environments out on a 64 x 64 grid with the configured spacing. + # These square grids are centered on the world origin. origins = terrain_importer.env_origins - assert origins.shape == (4096, 3) + half_span = (int(np.sqrt(num_envs)) - 1) * env_spacing / 2.0 + assert origins.shape == (num_envs, 3) assert origins.device == torch.device("cuda:0") - torch.testing.assert_close(origins[0].cpu(), torch.tensor([126.0, -126.0, 0.0])) - torch.testing.assert_close(origins[-1].cpu(), torch.tensor([-126.0, 126.0, 0.0])) + torch.testing.assert_close(origins[0].cpu(), torch.tensor([half_span, -half_span, 0.0])) + torch.testing.assert_close(origins[-1].cpu(), torch.tensor([-half_span, half_span, 0.0])) # check if mesh prim path exists mesh_prim_path = terrain_importer.cfg.prim_path + "/terrain" assert mesh_prim_path in terrain_importer.terrain_prim_paths - # The visual mesh is bounded to the environment grid while the collision Plane stays infinite. + # Leave walking room beyond the outermost origins; collision remains infinite. + origins = terrain_importer.env_origins.cpu().numpy() + half_size = np.max(np.abs(origins[:, :2]), axis=0) + 50.0 + expected_scale = (*tuple(half_size / 50.0), 1.0) environment = sim.stage.GetPrimAtPath(f"{mesh_prim_path}/Environment") - assert tuple(environment.GetAttribute("xformOp:scale").Get()) == pytest.approx((2.6, 2.6, 1.0)) + assert tuple(environment.GetAttribute("xformOp:scale").Get()) == pytest.approx(expected_scale) visual_mesh = UsdGeom.Mesh(sim.stage.GetPrimAtPath(f"{mesh_prim_path}/Environment/Geometry")) - assert [tuple(uv) for uv in UsdGeom.PrimvarsAPI(visual_mesh).GetPrimvar("st").Get()] == [ - (-65.0, -65.0), - (65.0, -65.0), - (65.0, 65.0), - (-65.0, 65.0), - ] + uvs = np.asarray(UsdGeom.PrimvarsAPI(visual_mesh).GetPrimvar("st").Get()) + # A texture repeat remains 2 m even when the visual plane grows. + np.testing.assert_allclose(np.ptp(uvs, axis=0) * 2.0, half_size * 2.0) # Direct imports use the same bounded default instead of the legacy 2,000 km visual mesh. terrain_importer.import_ground_plane("direct") direct_environment = sim.stage.GetPrimAtPath(f"{terrain_importer.cfg.prim_path}/direct/Environment") - assert tuple(direct_environment.GetAttribute("xformOp:scale").Get()) == pytest.approx((2.6, 2.6, 1.0)) + assert tuple(direct_environment.GetAttribute("xformOp:scale").Get()) == pytest.approx(expected_scale) direct_mesh = UsdGeom.Mesh( sim.stage.GetPrimAtPath(f"{terrain_importer.cfg.prim_path}/direct/Environment/Geometry") ) - assert [tuple(uv) for uv in UsdGeom.PrimvarsAPI(direct_mesh).GetPrimvar("st").Get()] == [ - (-65.0, -65.0), - (65.0, -65.0), - (65.0, 65.0), - (-65.0, 65.0), - ] + np.testing.assert_allclose(UsdGeom.PrimvarsAPI(direct_mesh).GetPrimvar("st").Get(), uvs) # obtain underling mesh mesh = _obtain_collision_mesh(mesh_prim_path, mesh_type="Plane") diff --git a/source/isaaclab/test/utils/test_assets.py b/source/isaaclab/test/utils/test_assets.py index cd107cf876d7..d9bdd088d1c5 100644 --- a/source/isaaclab/test/utils/test_assets.py +++ b/source/isaaclab/test/utils/test_assets.py @@ -245,7 +245,7 @@ def test_check_file_path_invalid(): assert assets_utils.check_file_path(usd_path) == 0 -def test_find_asset_dependencies_collects_mdl_texture_resources(tmp_path): +def test_find_mdl_dependencies_collects_mdl_texture_resources(tmp_path): """Test collecting texture resources from quoted MDL strings.""" mdl_path = tmp_path / "material.mdl" mdl_path.write_text( @@ -264,7 +264,7 @@ def test_find_asset_dependencies_collects_mdl_texture_resources(tmp_path): encoding="utf-8", ) - assert assets_utils._find_asset_dependencies(str(mdl_path)) == { + assert assets_utils._find_mdl_dependencies(str(mdl_path)) == { "./textures/Albedo.png", "../shared/Normal.EXR", "https://example.com/materials/orm..png", @@ -272,7 +272,7 @@ def test_find_asset_dependencies_collects_mdl_texture_resources(tmp_path): } -def test_find_asset_dependencies_collects_mdl_relative_import_modules(tmp_path): +def test_find_mdl_dependencies_collects_mdl_relative_import_modules(tmp_path): """Test collecting sibling MDL modules imported by material files.""" mdl_path = tmp_path / "material.mdl" mdl_path.write_text( @@ -293,7 +293,7 @@ def test_find_asset_dependencies_collects_mdl_relative_import_modules(tmp_path): encoding="utf-8", ) - assert assets_utils._find_asset_dependencies(str(mdl_path)) == { + assert assets_utils._find_mdl_dependencies(str(mdl_path)) == { "OmniUe4Function.mdl", "OmniUe4Translucent.mdl", "Shared/OmniUe4Base.mdl", @@ -307,11 +307,11 @@ def test_find_asset_dependencies_collects_mdl_relative_import_modules(tmp_path): } -def test_find_asset_dependencies_missing_mdl_does_not_log_traceback(tmp_path, caplog): +def test_find_mdl_dependencies_missing_mdl_does_not_log_traceback(tmp_path, caplog): """Test unavailable MDL dependencies do not emit tracebacks in training logs.""" missing_mdl = tmp_path / "missing.mdl" - assert assets_utils._find_asset_dependencies(str(missing_mdl)) == set() + assert assets_utils._find_mdl_dependencies(str(missing_mdl)) == set() assert "Traceback (most recent call last):" not in caplog.text @@ -495,6 +495,7 @@ def asset_cache(tmp_path, monkeypatch): monkeypatch.setattr(assets_utils, "_ANNOUNCED_MIRROR_DIRS", set()) monkeypatch.setattr(assets_utils, "_ANNOUNCED_MIRRORS", set()) monkeypatch.setattr(assets_utils, "_MIRRORED_URLS", {}) + monkeypatch.setattr(assets_utils, "_LOCALIZED_ASSETS", {}) return tmp_path @@ -529,6 +530,85 @@ def _cache_asset(cache_dir, url: str, payload: bytes, fingerprint: dict | None) return mirrored +@pytest.mark.parametrize("layout", ["direct", "nested", "remote", "package"]) +def test_local_usd_mirrors_remote_sublayer_without_editing_source(asset_cache, monkeypatch, layout): + """Compose local and remote dependency chains without editing the authored layers.""" + import omni.client + from pxr import Sdf, Usd + + layers = { + "local.usda": '#usda 1.0\ndef Shader "local" {\n asset info:mdl:sourceAsset = @OmniPBR.mdl@\n}\n', + "robot.usda": f"#usda 1.0\n(subLayers = [@{_REMOTE_URL}@, @local.usda@])\n", + "scene.usda": "#usda 1.0\n(subLayers = [@robot.usda@])\n", + } + for name, content in layers.items(): + (asset_cache / name).write_text(content, encoding="utf-8") + root_url = "https://example.com/scene.usda" + payloads = { + _REMOTE_URL: '#usda 1.0\ndef Xform "cartpole" {}\n', + root_url: layers["robot.usda"], + "https://example.com/local.usda": layers["local.usda"], + } + revision = {"hash": "abc123", "version": "", "size": 32, "modified_time": "2026-07-01 10:00:00"} + + def fake_copy(url, target_path, behavior): + if url not in payloads: + return omni.client.Result.ERROR_NOT_FOUND + data = payloads[url] + Path(target_path).write_bytes(data.encode() if isinstance(data, str) else data) + return omni.client.Result.OK + + monkeypatch.setattr(omni.client, "copy", fake_copy) + source = {"direct": str(asset_cache / "robot.usda"), "nested": str(asset_cache / "scene.usda"), "remote": root_url} + if layout == "package": + source[layout] = _REMOTE_URL + "z" + package_path = asset_cache / "scene.usdz" + package_root = asset_cache / "package.usda" + package_root.write_text('#usda 1.0\n(subLayers = [@local.usda@])\ndef Xform "cartpole" {}\n') + with Usd.ZipFileWriter.CreateNew(str(package_path)) as package: + package.AddFile(str(package_root), "package.usda") + package.AddFile(str(asset_cache / "local.usda"), "local.usda") + payloads[source[layout]] = package_path.read_bytes() + _serve(monkeypatch, dict.fromkeys(payloads, revision)) + resolved_path = assets_utils.retrieve_file_path(source[layout]) + stage = Usd.Stage.Open(resolved_path) + assert stage.GetPrimAtPath("/cartpole").IsValid() + assert stage.GetPrimAtPath("/local").IsValid() + assert stage.GetPrimAtPath("/local").GetAttribute("info:mdl:sourceAsset").Get().path == "OmniPBR.mdl" + assert {name: (asset_cache / name).read_text(encoding="utf-8") for name in layers} == layers + if layout == "package": + assert Path(resolved_path).read_bytes() == payloads[source[layout]] + + monkeypatch.setattr(Sdf.Layer, "OpenAsAnonymous", lambda _: pytest.fail("walked a completed tree")) + assert assets_utils.retrieve_file_path(resolved_path) == resolved_path + + +def test_retrieve_file_path_retries_incomplete_tree(asset_cache, monkeypatch): + """A downloaded root is not a completed tree when a dependency fails to download.""" + import omni.client + from pxr import Usd + + child_url = _REMOTE_URL.replace("example.usd", "child.usda") + revision = {"hash": "abc123", "version": "", "size": 32, "modified_time": "2026-07-01 10:00:00"} + mirrored = _cache_asset(asset_cache, _REMOTE_URL, b"#usda 1.0\n(subLayers = [@child.usda@])\n", revision) + _serve(monkeypatch, {_REMOTE_URL: revision, child_url: revision}) + fail_child = True + + def fake_copy(url, target_path, behavior): + assert url == child_url + if fail_child: + return omni.client.Result.ERROR_NOT_FOUND + Path(target_path).write_text('#usda 1.0\ndef Xform "child" {}\n', encoding="utf-8") + return omni.client.Result.OK + + monkeypatch.setattr(omni.client, "copy", fake_copy) + assets_utils.retrieve_file_path(_REMOTE_URL) + fail_child = False + resolved = assets_utils.retrieve_file_path(str(mirrored)) + stage = Usd.Stage.Open(resolved) + assert stage.GetPrimAtPath("/child").IsValid() + + def test_read_file_uses_the_local_copy_when_it_matches_the_server(asset_cache, monkeypatch): """Test an unchanged remote asset is read from disk instead of downloaded again.""" revision = {"hash": "abc123", "version": "", "size": 12, "modified_time": "2026-07-01 10:00:00"} @@ -590,7 +670,6 @@ def fake_copy(url, target_path, behavior): return omni.client.Result.OK monkeypatch.setattr(omni.client, "copy", fake_copy) - monkeypatch.setattr(assets_utils, "_find_asset_dependencies", lambda path: set()) start = threading.Barrier(2) def retrieve() -> str: diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index 41e411793f92..b041cca86648 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -904,46 +904,41 @@ def test_matrix_from_euler(device, euler_angles, convention): @pytest.mark.parametrize("device", test_devices()) -def test_quat_apply(device): - """Test for quat_apply against scipy.""" - # prepare random quaternions and vectors - n = 1024 - q_rand = math_utils.random_orientation(num=n, device=device) - # Our quaternions are already in xyzw format, which scipy expects - Rotation = scipy_tf.Rotation.from_quat(q_rand.to(device="cpu").numpy()) - - v_rand = math_utils.sample_uniform(-1000, 1000, (n, 3), device=device) - - # compute the result using the new implementation - scipy_result = torch.tensor(Rotation.apply(v_rand.to(device="cpu").numpy()), device=device, dtype=torch.float) - apply_result = math_utils.quat_apply(q_rand, v_rand) - torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4) - # batched (..., 4) inputs keep their leading dimensions - batched_result = math_utils.quat_apply(q_rand.reshape(n // 8, 2, 4, 4), v_rand.reshape(n // 8, 2, 4, 3)) - torch.testing.assert_close(batched_result, scipy_result.reshape(n // 8, 2, 4, 3), atol=2e-4, rtol=2e-4) - - -@pytest.mark.parametrize("device", test_devices()) -def test_quat_apply_inverse(device): - """Test for quat_apply against scipy.""" - - # prepare random quaternions and vectors - n = 1024 - q_rand = math_utils.random_orientation(num=n, device=device) - # Our quaternions are already in xyzw format, which scipy expects - Rotation = scipy_tf.Rotation.from_quat(q_rand.to(device="cpu").numpy()) - - v_rand = math_utils.sample_uniform(-1000, 1000, (n, 3), device=device) - - # compute the result using the new implementation - scipy_result = torch.tensor( - Rotation.apply(v_rand.to(device="cpu").numpy(), inverse=True), device=device, dtype=torch.float - ) - apply_result = math_utils.quat_apply_inverse(q_rand, v_rand) - torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4) - # batched (..., 4) inputs keep their leading dimensions - batched_result = math_utils.quat_apply_inverse(q_rand.reshape(n // 8, 2, 4, 4), v_rand.reshape(n // 8, 2, 4, 3)) - torch.testing.assert_close(batched_result, scipy_result.reshape(n // 8, 2, 4, 3), atol=2e-4, rtol=2e-4) +@pytest.mark.parametrize("inverse", [False, True]) +@pytest.mark.parametrize( + "quat_shape,vec_shape", + [ + ((1024,), (1024,)), + ((128, 2, 4), (128, 2, 4)), + ((), ()), + ((1,), ()), + ((), (2, 3)), + ((2, 3), ()), + ((2, 3), (2, 1)), + ((2, 1), (3,)), + ((2, 3), (3, 2)), + ], +) +def test_quat_apply(device, inverse, quat_shape, vec_shape): + """Rotations follow NumPy broadcasting and agree with SciPy, including strided inputs.""" + quat = math_utils.random_orientation(num=2 * math.prod(quat_shape), device=device)[::2].reshape(*quat_shape, 4) + vec = math_utils.sample_uniform(-1000, 1000, (2 * math.prod(vec_shape), 3), device=device)[::2] + vec = vec.reshape(*vec_shape, 3) + apply = math_utils.quat_apply_inverse if inverse else math_utils.quat_apply + try: + shape = np.broadcast_shapes(quat_shape, vec_shape) + (3,) + except ValueError: + # Equal element counts do not make incompatible batch dimensions broadcastable. + with pytest.raises((RuntimeError, torch.jit.Error)): + apply(quat, vec) + return + + # Broadcast with NumPy before flattening for SciPy versions that only accept a single batch axis. + quat_np = np.broadcast_to(quat.cpu().numpy(), shape[:-1] + (4,)).reshape(-1, 4) + vec_np = np.broadcast_to(vec.cpu().numpy(), shape).reshape(-1, 3) + expected = scipy_tf.Rotation.from_quat(quat_np).apply(vec_np, inverse=inverse).reshape(shape) + result = apply(quat, vec) + torch.testing.assert_close(result, torch.tensor(expected, device=device, dtype=vec.dtype), atol=2e-4, rtol=2e-4) @pytest.mark.parametrize("device", test_devices()) diff --git a/source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip b/source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip new file mode 100644 index 000000000000..df6d36b6bed5 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/test-audit-newton-tests.skip @@ -0,0 +1 @@ +Restored Newton tests removed without a remaining proof and repaired Newton regression tests that could not fail. diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index d0731f63a222..834db2c6ef6a 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -39,16 +39,14 @@ from isaaclab_newton.assets import Articulation from isaaclab_newton.assets.articulation.actuator_control import NewtonActuatorControl from isaaclab_newton.assets.articulation.articulation import _configure_builder_joint_target_modes -from isaaclab_newton.assets.articulation.articulation_data import ArticulationData from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics import NewtonManager as SimulationManager from isaaclab_physx.sim.schemas import PhysxJointCfg -from newton import JointTargetMode, JointType, ModelBuilder, ModelFlags +from newton import JointTargetMode, JointType, ModelBuilder, ModelFlags, ShapeFlags from newton.solvers import SolverMuJoCo from pxr import UsdPhysics -import isaaclab.assets.articulation.ordering_kernels as ordering_kernels import isaaclab.assets.articulation.ordering_resolvers as ordering_resolvers import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils @@ -61,18 +59,12 @@ from isaaclab.assets import ArticulationCfg, AssetBaseCfg from isaaclab.assets.articulation.ordering_resolvers import get_articulation_name_ordering from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate -from isaaclab.controllers import ( - DifferentialIKController, - DifferentialIKControllerCfg, - OperationalSpaceController, - OperationalSpaceControllerCfg, -) -from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit -from isaaclab.managers import SceneEntityCfg +from isaaclab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg +from isaaclab.envs.mdp.events import randomize_rigid_body_collider_offsets, randomize_rigid_body_material +from isaaclab.managers import EventTermCfg, SceneEntityCfg from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.math import compute_pose_error, matrix_from_quat, quat_inv, subtract_frame_transforms -from isaaclab.utils.warp.proxy_array import ProxyArray ## # Pre-defined configs @@ -442,7 +434,7 @@ def generate_articulation( # --------------------------------------------------------------------------- -# Franka task-space tracking helpers (shared between IK and OSC tests). +# Franka task-space tracking helpers (shared between the OSC tests). # --------------------------------------------------------------------------- @@ -521,7 +513,7 @@ def _compute_ee_vel_root(jacobian_b, joint_vel): velocity buffers can return stale/zero values until forced materialization, while ``joint_vel`` and ``J`` are already pulled by the loop. ``J`` correctness is pinned independently by - ``test_get_jacobians_link_origin_contract``. + ``test_get_gravity_compensation_forces_matches_jacobian_gravity``. """ return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) @@ -533,10 +525,10 @@ def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device): return torch.cat([target_pos_b, initial_ee_quat_b], dim=-1) -def _summarize_history(history, tail: int = 200): - """Return ``(min, mean)`` over the last ``tail`` samples.""" +def _tail_mean(history, tail: int = 200): + """Return the mean over the last ``tail`` samples.""" tail_slice = history[-tail:] - return min(tail_slice), sum(tail_slice) / len(tail_slice) + return sum(tail_slice) / len(tail_slice) @pytest.fixture @@ -600,55 +592,21 @@ def _make_target_mode_builder( return builder -def test_viscous_writer_updates_finalized_newton_model(monkeypatch): - """Test the production viscous writer updates a finalized Newton model binding.""" - builder = ModelBuilder() - link = builder.add_link(mass=1.0, inertia=wp.mat33(1.0)) - joint = builder.add_joint_revolute(-1, link, label="joint") - builder.add_articulation([joint], label="articulation") - model = builder.finalize(device="cpu") - model_damping = wp.array( - ptr=model.joint_damping.ptr, - dtype=wp.float32, - shape=(1, 1), - strides=(model.joint_damping.strides[0], model.joint_damping.strides[0]), - device="cpu", - copy=False, - ) - - data_type = type( - "_Data", - (), - {"joint_viscous_friction_coeff": ArticulationData.joint_viscous_friction_coeff}, - ) - data = data_type() - data.has_joint_ordering = False - data.joint_ordering = None - data._joint_viscous_friction_user = None - data._sim_bind_joint_viscous_friction_coeff = model_damping - data._joint_viscous_friction_coeff_ta = ProxyArray(model_damping) - - articulation = object.__new__(Articulation) - articulation._device = "cpu" - articulation._data = data - articulation._root_view = SimpleNamespace(count=1) - articulation._ALL_INDICES = wp.array([0], dtype=wp.int32, device="cpu") - articulation._ALL_JOINT_INDICES = wp.array([0], dtype=wp.int32, device="cpu") - articulation._initialize_handle = None - articulation._invalidate_initialize_handle = None - articulation._prim_deletion_handle = None - monkeypatch.setattr(SimulationManager, "add_model_change", lambda flags: None) - - articulation.write_joint_viscous_friction_coefficient_to_sim_index( - joint_viscous_friction_coeff=torch.tensor([[0.25]], dtype=torch.float32), - ) - - torch.testing.assert_close(data.joint_viscous_friction_coeff.torch, torch.tensor([[0.25]])) - torch.testing.assert_close(torch.from_numpy(model.joint_damping.numpy()), torch.tensor([0.25])) - +@pytest.mark.parametrize( + ("actuator_cfg", "expected_native_groups"), + [ + (ImplicitActuatorCfg(joint_names_expr=["joint"], stiffness=10.0, damping=1.0), set()), + (IdealPDActuatorCfg(joint_names_expr=["joint"], stiffness=None, damping=None), {"explicit"}), + ], + ids=["implicit", "explicit"], +) +def test_prepare_native_actuators_activates_only_explicit_groups(monkeypatch, actuator_cfg, expected_native_groups): + """Keep implicit-only articulations on the solver-drive path and leave solver gains untouched. -def test_prepare_native_actuators_does_not_zero_solver_gains(monkeypatch): - """Leave solver gains untouched until collection construction resolves actuator defaults.""" + Explicit groups activate the Newton-actuator path without writing gains; collection construction resolves + the actuator defaults later. + """ + activation_calls = [] gain_writes = [] articulation = SimpleNamespace( _sim_cfg=SimpleNamespace(use_newton_actuators=True), @@ -657,33 +615,20 @@ def test_prepare_native_actuators_does_not_zero_solver_gains(monkeypatch): write_joint_stiffness_to_sim_index=lambda **_: gain_writes.append("stiffness"), write_joint_damping_to_sim_index=lambda **_: gain_writes.append("damping"), ) - monkeypatch.setattr(SimulationManager, "activate_newton_actuator_path", lambda: None) - - native_groups = NewtonActuatorControl(articulation).prepare_native_actuators( - collection=None, - actuator_cfgs={"explicit": IdealPDActuatorCfg(joint_names_expr=["joint"], stiffness=None, damping=None)}, - ) - - assert native_groups == {"explicit"} - assert gain_writes == [] - - -def test_prepare_native_actuators_leaves_implicit_only_articulation_on_standard_path(monkeypatch): - """Keep implicit-only articulations on the unchanged solver-drive path.""" - activation_calls = [] - articulation = SimpleNamespace(_sim_cfg=SimpleNamespace(use_newton_actuators=True)) monkeypatch.setattr(SimulationManager, "activate_newton_actuator_path", lambda: activation_calls.append(True)) control = NewtonActuatorControl(articulation) - native_groups = control.prepare_native_actuators( - collection=None, - actuator_cfgs={"implicit": ImplicitActuatorCfg(joint_names_expr=["joint"], stiffness=10.0, damping=1.0)}, - ) + group_name = "explicit" if expected_native_groups else "implicit" + native_groups = control.prepare_native_actuators(collection=None, actuator_cfgs={group_name: actuator_cfg}) - assert native_groups == set() - assert not control.native_actuator_path_active - assert not articulation._has_newton_actuators - assert activation_calls == [] + assert native_groups == expected_native_groups + assert gain_writes == [] + if expected_native_groups: + assert activation_calls == [True] + else: + assert not control.native_actuator_path_active + assert not articulation._has_newton_actuators + assert activation_calls == [] @pytest.mark.parametrize( @@ -787,7 +732,7 @@ def test_actuator_cfg_leaves_excluded_joint_types_imported(joint_type): assert builder.joint_target_mode == [int(JointTargetMode.NONE)] -def test_actuator_cfg_keeps_imported_newton_target_mode_for_none_gain(): +def test_actuator_cfg_uses_imported_gain_for_none_stiffness(): """Retain the imported stiffness when an implicit actuator config leaves it unset.""" articulation_cfg = ArticulationCfg( prim_path="/World/Env_[^/]*/Robot", @@ -839,90 +784,7 @@ def test_actuator_cfg_aligns_partial_dictionary_gains_by_joint_name(stiffness, d assert builder.joint_target_mode == [int(mode) for mode in expected_modes] -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_write_joint_state_accepts_int64_selector(sim, device, gravity_enabled, articulation_type) -> None: - """Write joint state with int64 selectors.""" - articulation_cfg = generate_articulation_cfg(articulation_type="spatial_tendon_test_asset") - articulation, _ = generate_articulation(articulation_cfg, 2, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.num_joints >= 2 - - env_ids = torch.tensor([1, 0], dtype=torch.int64, device=device) - joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int64, device=device) - position = torch.tensor([[0.21, 0.11], [0.22, 0.12]], device=device) - velocity = torch.tensor([[1.21, 1.11], [1.22, 1.12]], device=device) - - expected_position = articulation.data.joint_pos.torch.clone() - expected_velocity = articulation.data.joint_vel.torch.clone() - - articulation.write_joint_state_to_sim_index( - position=position, velocity=velocity, env_ids=env_ids, joint_ids=joint_ids - ) - expected_position[env_ids[:, None], joint_ids[None, :]] = position - expected_velocity[env_ids[:, None], joint_ids[None, :]] = velocity - torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) - torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) - - -@pytest.mark.parametrize("device", ["cpu"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) -def test_mjwarp_ordering_resolver_matches_newton_backend_names(sim, device, gravity_enabled, articulation_type): - """Compare the resolver's emulated MJWarp ordering against the live Newton backend view. - - The articulation below is already native to the Newton backend, so - :func:`~isaaclab.assets.get_articulation_name_ordering` with the ``"mjwarp"`` - convention takes the same-backend identity fast path and never exercises the - temporary Newton USD builder used for cross-backend discovery (the path a - PhysX-backed articulation would take). That fast path is checked below, - but it is not sufficient by itself: it would pass even if the emulation's - BFS/DFS traversal had silently diverged from the live backend. To close - that gap, this test also calls the private builder helper directly — - forcing the temporary-builder emulation to run — and compares its output - against the live backend view. A branching (non-single-joint) fixture is - required for this comparison to be meaningful, since BFS and DFS produce - the same order on a single-joint chain. - """ - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - sim_utils.create_prim("/World/Env_0", "Xform") - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_0/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - ) - clone_plan_from_env_0(CloneCfg(clone_template="/World/Env_{}"), (articulation_cfg,), 1, 0.0) - articulation = Articulation(articulation_cfg) - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - # Newton's native traversal is depth-first (see NewtonManager.instantiate_builder_from_stage), - # so the live backend view already reflects MJWarp order on this branching fixture. These - # values are the same ground truth isaaclab_physx's own - # test_branching_fixture_resolves_distinct_conventions asserts for expected_mjwarp_*_names. - assert tuple(articulation.backend_joint_names) == BRANCHING_MJWARP_JOINT_NAMES - assert tuple(articulation.backend_body_names) == BRANCHING_MJWARP_BODY_NAMES - - # Force the cross-backend emulation path (bypassing the same-backend identity fast path) and - # compare its independently rebuilt Newton view against the live backend view above. A - # BFS/DFS regression in the emulation would fail this even though the fixture is small. - emulated_names = ordering_resolvers._get_mjwarp_names_from_newton_usd_builder(articulation) - assert emulated_names is not None - assert emulated_names["joint"] == tuple(articulation.backend_joint_names) - assert emulated_names["body"] == tuple(articulation.backend_body_names) - - # The public resolver still returns live names without discovery for a same-backend request. - assert get_articulation_name_ordering(articulation, "mjwarp", kind="joint") == tuple( - articulation.backend_joint_names - ) - assert get_articulation_name_ordering(articulation, "mjwarp", kind="body") == tuple(articulation.backend_body_names) - - -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +@pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("gravity_enabled", [False]) @pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gravity_enabled, articulation_type): @@ -937,6 +799,9 @@ def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gr The branching fixture is shared between both backends; a copy lives in this package's test data directory so the two backends assert against the same ground-truth asset. + + The same articulation also checks the MJWarp-order emulation used for cross-backend discovery and that + selected inertial-property writes keep Newton's inverse arrays current under the body ordering. """ fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" sim_utils.create_prim("/World/Env_0", "Xform") @@ -958,6 +823,18 @@ def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gr assert tuple(articulation.backend_joint_names) == BRANCHING_MJWARP_JOINT_NAMES assert tuple(articulation.backend_body_names) == BRANCHING_MJWARP_BODY_NAMES + # The same-backend "mjwarp" request takes an identity fast path, so also force the cross-backend + # emulation (the temporary Newton USD builder a PhysX-backed articulation would use) and compare its + # independently rebuilt view against the live backend view. BFS and DFS differ on this branching fixture. + emulated_names = ordering_resolvers._get_mjwarp_names_from_newton_usd_builder(articulation) + assert emulated_names is not None + assert emulated_names["joint"] == tuple(articulation.backend_joint_names) + assert emulated_names["body"] == tuple(articulation.backend_body_names) + assert get_articulation_name_ordering(articulation, "mjwarp", kind="joint") == tuple( + articulation.backend_joint_names + ) + assert get_articulation_name_ordering(articulation, "mjwarp", kind="body") == tuple(articulation.backend_body_names) + # Cross-backend discovery (bypassing the same-backend fast path) resolves the breadth-first PhysX order. assert get_articulation_name_ordering(articulation, "physx", kind="joint") == BRANCHING_PHYSX_JOINT_NAMES assert get_articulation_name_ordering(articulation, "physx", kind="body") == BRANCHING_PHYSX_BODY_NAMES @@ -968,6 +845,27 @@ def test_branching_fixture_physx_ordering_reorders_newton_to_bfs(sim, device, gr assert articulation.joint_ordering is not None assert articulation.body_ordering is not None + # Selected mass and inertia writes with int64 selectors update Newton's inverse arrays in backend order. + env_ids = torch.tensor([0], dtype=torch.int64, device=device) + body_ids = torch.tensor([2, articulation.num_bodies - 1], dtype=torch.int64, device=device) + backend_body_ids = torch.tensor( + [articulation.data.body_ordering.user_to_backend_indices[index] for index in body_ids.tolist()], + dtype=torch.int64, + device=device, + ) + assert backend_body_ids[0] != body_ids[0] + model = SimulationManager.get_model() + + masses = articulation.data.body_mass.torch[env_ids][:, body_ids].clone() + torch.tensor([[1.0, 2.0]], device=device) + articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) + model_inv_mass = wp.to_torch(articulation.root_view.get_attribute("body_inv_mass", model)[:, 0]) + torch.testing.assert_close(model_inv_mass[env_ids][:, backend_body_ids], masses.reciprocal()) + + inertia_matrices = torch.diag_embed(torch.tensor([[[2.0, 3.0, 4.0], [5.0, 6.0, 7.0]]], device=device)) + articulation.set_inertias_index(inertias=inertia_matrices.reshape(1, 2, 9), env_ids=env_ids, body_ids=body_ids) + model_inv_inertia = wp.to_torch(articulation.root_view.get_attribute("body_inv_inertia", model)[:, 0]) + torch.testing.assert_close(model_inv_inertia[env_ids][:, backend_body_ids], torch.linalg.inv(inertia_matrices)) + def test_num_shapes_per_body_follows_public_body_order() -> None: """Align Newton shape counts with the public body-name axis.""" @@ -1449,12 +1347,21 @@ def _other_callback() -> None: @pytest.mark.parametrize("articulation_type", ["anymal"]) @pytest.mark.parametrize("use_newton_actuators", [False, True]) @pytest.mark.parametrize("ordering_mode", ["none", "reversed"]) -def test_write_data_to_sim_gathers_joint_targets_only_when_ordering_active( - sim, num_articulations, device, gravity_enabled, articulation_type, use_newton_actuators, ordering_mode, monkeypatch +def test_write_data_to_sim_writes_joint_targets_in_backend_order( + sim, num_articulations, device, gravity_enabled, articulation_type, use_newton_actuators, ordering_mode ): - """Gather joint targets only when non-identity joint ordering is active.""" + """Write the published joint position targets into the backend buffer in backend joint order. + + Explicit actuators on the Newton-actuator path publish their raw targets; implicit actuators on the Lab + path publish the processed targets. Both must reach the solver-bound buffer permuted to backend order. + """ + actuator_cfg = ( + IdealPDActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0) + if use_newton_actuators + else ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0) + ) articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).replace( - actuators={"legs": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=40.0, damping=5.0)}, + actuators={"legs": actuator_cfg}, ) if ordering_mode == "reversed": articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(ANYMAL_C_PHYSX_JOINT_NAMES))) @@ -1462,141 +1369,57 @@ def test_write_data_to_sim_gathers_joint_targets_only_when_ordering_active( replicate(sim.get_clone_plan()) sim.reset() assert articulation.is_initialized + assert (articulation.data.joint_ordering is not None) is (ordering_mode == "reversed") + assert articulation._has_newton_actuators is use_newton_actuators - has_ordering = ordering_mode == "reversed" - assert (articulation.data.joint_ordering is not None) is has_ordering - on_newton_path = getattr(articulation, "_has_newton_actuators", False) - if use_newton_actuators and not on_newton_path: - pytest.skip("newton.actuators unavailable; the Newton-actuator branch is not exercised") - - # Drive an in-limits position target so the write path has data to forward. - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - - # Record every kernel launched during write_data_to_sim, then delegate to the - # real launch so the sim-bound buffers are still written. - launched_kernels: list = [] - real_launch = wp.launch - - def recording_launch(kernel, *args, **kwargs): - launched_kernels.append(kernel) - return real_launch(kernel, *args, **kwargs) - - monkeypatch.setattr(wp, "launch", recording_launch) + # Distinct per-joint targets away from the defaults, so a skipped or unpermuted write is visible. + target = articulation.data.default_joint_pos.torch.clone() + target += 0.01 * torch.arange(1, articulation.num_joints + 1, device=device) + articulation.set_joint_position_target_index(target=target) articulation.write_data_to_sim() - monkeypatch.undo() - - target_gather = ordering_kernels.reorder_joint_targets_user_to_backend - if has_ordering: - assert target_gather in launched_kernels - else: - # Identity ordering binds the user-order source directly. - assert target_gather not in launched_kernels - expected_source = ( - articulation.actuators.target_command.position.warp - if on_newton_path - else articulation.actuators.output_command.position.warp - ) - np.testing.assert_allclose(articulation.data._sim_bind_joint_position_target.numpy(), expected_source.numpy()) - - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) -@pytest.mark.parametrize("index_dtype", [torch.int32, torch.int64]) -def test_set_body_inertial_properties_updates_inverses( - sim, device, gravity_enabled, articulation_type, index_dtype, monkeypatch -): - """Selected inertial-property writes keep Newton inverse arrays current under body ordering.""" - fixture_path = Path(__file__).parent / "data" / "articulation_ordering_branching.usda" - sim_utils.create_prim("/World/Env_0", "Xform") - articulation_cfg = ArticulationCfg( - prim_path="/World/Env_0/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=str(fixture_path)), - actuators={}, - body_ordering="physx", - ) - clone_plan_from_env_0(CloneCfg(clone_template="/World/Env_{}"), (articulation_cfg,), 1, 0.0) - articulation = Articulation(articulation_cfg) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.data.body_ordering is not None - env_ids = torch.tensor([0], dtype=index_dtype, device=device) - body_ids = torch.tensor([2, articulation.num_bodies - 1], dtype=index_dtype, device=device) - backend_body_ids = torch.tensor( - [articulation.data.body_ordering.user_to_backend_indices[index] for index in body_ids.tolist()], - dtype=torch.int64, - device=device, + source = ( + articulation.actuators.target_command.position.torch + if use_newton_actuators + else articulation.actuators.output_command.position.torch ) - assert backend_body_ids[0] != body_ids[0] - - launches = [] - real_launch = wp.launch - - def recording_launch(kernel, *args, **kwargs): - launches.append(kernel) - return real_launch(kernel, *args, **kwargs) - - monkeypatch.setattr(wp, "launch", recording_launch) - masses = articulation.data.body_mass.torch[env_ids][:, body_ids].clone() + torch.tensor([[1.0, 2.0]], device=device) - articulation.set_masses_index(masses=masses, env_ids=env_ids, body_ids=body_ids) - assert len(launches) == 1 - - raw_model_inv_mass = articulation.root_view.get_attribute("body_inv_mass", SimulationManager.get_model())[:, 0] - assert articulation.data._sim_bind_body_inv_mass.ptr == raw_model_inv_mass.ptr - model_inv_mass = wp.to_torch(articulation.data._sim_bind_body_inv_mass) - torch.testing.assert_close(model_inv_mass[env_ids][:, backend_body_ids], masses.reciprocal()) - - inertia_matrices = torch.diag_embed(torch.tensor([[[2.0, 3.0, 4.0], [5.0, 6.0, 7.0]]], device=device)) - inertias = inertia_matrices.reshape(1, 2, 9) - launches.clear() - articulation.set_inertias_index(inertias=inertias, env_ids=env_ids, body_ids=body_ids) - assert len(launches) == 1 - - raw_model_inv_inertia = articulation.root_view.get_attribute("body_inv_inertia", SimulationManager.get_model())[ - :, 0 - ] - assert articulation.data._sim_bind_body_inv_inertia.ptr == raw_model_inv_inertia.ptr - model_inv_inertia = wp.to_torch(articulation.data._sim_bind_body_inv_inertia) - torch.testing.assert_close( - model_inv_inertia[env_ids][:, backend_body_ids], - torch.linalg.inv(inertia_matrices), + torch.testing.assert_close(source, target) + user_to_backend = ( + list(articulation.joint_ordering.user_to_backend_indices) + if articulation.joint_ordering is not None + else list(range(articulation.num_joints)) ) + expected_backend_target = torch.empty_like(source) + expected_backend_target[:, user_to_backend] = source + torch.testing.assert_close(wp.to_torch(articulation.data._sim_bind_joint_position_target), expected_backend_target) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("num_articulations", [3]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_initialization_floating_base_non_root(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test initialization for a floating-base with articulation root on a rigid body. +def test_gravity_vec_w_tracks_model_gravity(sim, num_articulations, device, add_ground_plane, articulation_type): + """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is not fixed base - 3. All buffers have correct shapes - 4. The articulation can be simulated + Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's + gravity broadcast to every env, hiding per-env gravity randomization (e.g. + :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on + The humanoid's articulation root sits on a rigid body, so this also checks floating-base initialization. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation( - articulation_cfg, num_articulations, device=sim.device, add_ground_plane=True - ) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) # Check that the framework doesn't hold excessive strong references. assert sys.getrefcount(articulation) < 10 - # Play sim replicate(sim.get_clone_plan()) sim.reset() # Check if articulation is initialized assert articulation.is_initialized - # Check that is fixed base + # Check that is floating base assert not articulation.is_fixed_base # Check buffers that exists and have correct shapes assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) @@ -1608,31 +1431,6 @@ def test_initialization_floating_base_non_root(sim, num_articulations, device, a is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_articulations", [2, 3]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_gravity_vec_w_tracks_model_gravity(sim, num_articulations, device, add_ground_plane, articulation_type): - """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - - Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's - gravity broadcast to every env, hiding per-env gravity randomization (e.g. - :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - replicate(sim.get_clone_plan()) - sim.reset() - # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. model = SimulationManager.get_model() model_gravity_arr = model.gravity[: model.world_count] @@ -1662,68 +1460,22 @@ def test_gravity_vec_w_tracks_model_gravity(sim, num_articulations, device, add_ torch.testing.assert_close(articulation.data.projected_gravity_b.torch, expected, atol=1e-5, rtol=1e-5) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_initialization_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test initialization for a floating-base with articulation root on provided prim path. - - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is not fixed base - 3. All buffers have correct shapes - 4. The articulation can be simulated - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type, stiffness=0.0, damping=0.0) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # Check that floating base - assert not articulation.is_fixed_base - # Check buffers that exists and have correct shapes - assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 12) - assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) - assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) - - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("articulation_type", ["panda"]) -def test_initialization_fixed_base(sim, num_articulations, device, articulation_type): - """Test initialization for fixed base. +def test_fixed_base_reports_body_velocities(sim, num_articulations, device, articulation_type): + """Test that fixed-base articulations report live body velocities while their joints move. + + Regression test: the fixed-base fallback in ``_create_buffers`` zeroed the body + center-of-mass velocity binding together with the (genuinely unavailable) root velocity, + so :attr:`body_lin_vel_w` and :attr:`body_ang_vel_w` read zeros for every fixed-base + robot regardless of motion. This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is fixed base - 3. All buffers have correct shapes - 4. The articulation maintains its default state + 1. The articulation is initialized as fixed base with correctly shaped buffers + 2. Commanding a joint-space motion moves the bodies (finite difference of positions) while the root holds + its default state + 3. The reported body velocities track the finite-difference ground truth Args: sim: The simulation fixture @@ -1755,50 +1507,10 @@ def test_initialization_fixed_base(sim, num_articulations, device, articulation_ is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_pose = articulation.data.default_root_pose.torch.clone() - default_root_vel = articulation.data.default_root_vel.torch.clone() - default_root_pose[:, :3] = default_root_pose[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_fixed_base_reports_body_velocities(sim, num_articulations, device, articulation_type): - """Test that fixed-base articulations report live body velocities while their joints move. - - Regression test: the fixed-base fallback in ``_create_buffers`` zeroed the body - center-of-mass velocity binding together with the (genuinely unavailable) root velocity, - so :attr:`body_lin_vel_w` and :attr:`body_ang_vel_w` read zeros for every fixed-base - robot regardless of motion. - - This test verifies that: - 1. The articulation is fixed base - 2. Commanding a joint-space motion moves the bodies (finite difference of positions) - 3. The reported body velocities track the finite-difference ground truth - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_fixed_base + # the root holds its default state as it is fixed base + default_root_pose = articulation.data.default_root_pose.torch.clone() + default_root_pose[:, :3] = default_root_pose[:, :3] + translations + default_root_vel = articulation.data.default_root_vel.torch.clone() # command a step away from the default pose so the distal bodies move joint_pos_target = articulation.data.default_joint_pos.torch.clone() @@ -1811,6 +1523,8 @@ def test_fixed_base_reports_body_velocities(sim, num_articulations, device, arti articulation.write_data_to_sim() sim.step() articulation.update(sim.cfg.dt) + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) + torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) body_pos = articulation.data.body_link_pos_w.torch reported_max.append(articulation.data.body_lin_vel_w.torch.norm(dim=-1).amax()) fin_diff_max.append(((body_pos - prev_body_pos) / sim.cfg.dt).norm(dim=-1).amax()) @@ -1824,67 +1538,6 @@ def test_fixed_base_reports_body_velocities(sim, num_articulations, device, arti assert reported_max > 0.5 * fin_diff_max -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) -def test_initialization_fixed_base_single_joint(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test initialization for fixed base articulation with a single joint. - - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is fixed base - 3. All buffers have correct shapes - 4. The articulation maintains its default state - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, translations = generate_articulation( - articulation_cfg, num_articulations, device=device, add_ground_plane=True - ) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # Check that fixed base - assert articulation.is_fixed_base - # Check buffers that exists and have correct shapes - assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 1) - assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) - assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) - - # -- actuator type - for actuator_name, actuator in articulation.actuators.items(): - is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) - assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg - - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - - # check that the root is at the correct state - its default state as it is fixed base - default_root_pose = articulation.data.default_root_pose.torch.clone() - default_root_vel = articulation.data.default_root_vel.torch.clone() - default_root_pose[:, :3] = default_root_pose[:, :3] + translations - - torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) - torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("articulation_type", ["shadow_hand"]) @@ -1979,11 +1632,16 @@ def test_fixed_tendon_properties_reach_solver(sim, num_articulations, device, ar @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["anymal"]) def test_fragment_fix_root_link_uses_base_manager(sim, device, add_ground_plane, articulation_type): - """Newton consumes the base manager's world joint without relocating the root API.""" + """Newton consumes the base manager's world joint without relocating the root API. + + The floating-base ANYmal made fixed-base must then hold its root at the default state. + """ articulation_cfg = deepcopy(generate_articulation_cfg(articulation_type=articulation_type)) articulation_cfg.spawn.articulation_props = [] articulation_cfg.spawn.fix_root_link = True - articulation, _ = generate_articulation(articulation_cfg, num_articulations=1, device=device, add_ground_plane=True) + articulation, translations = generate_articulation( + articulation_cfg, num_articulations=1, device=device, add_ground_plane=True + ) root = sim_utils.get_first_matching_child_prim( "/World/Env_0/Robot", @@ -1997,54 +1655,10 @@ def test_fragment_fix_root_link_uses_base_manager(sim, device, add_ground_plane, sim.reset() assert articulation.is_initialized assert articulation.is_fixed_base + assert articulation.data.joint_pos.torch.shape == (1, 12) - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_initialization_floating_base_made_fixed_base( - sim, num_articulations, device, add_ground_plane, articulation_type -): - """Test initialization for a floating-base articulation made fixed-base using schema properties. - - This test verifies that: - 1. The articulation is properly initialized - 2. The articulation is fixed base after modification - 3. All buffers have correct shapes - 4. The articulation maintains its default state - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).copy() - # Fix root link by making it kinematic - articulation_cfg.spawn.fix_root_link = True - articulation, translations = generate_articulation( - articulation_cfg, num_articulations, device=device, add_ground_plane=True - ) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(articulation) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - # Check that is fixed base - assert articulation.is_fixed_base - # Check buffers that exists and have correct shapes - assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) - assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) - assert articulation.data.joint_pos.torch.shape == (num_articulations, 12) - - # Simulate physics for _ in range(10): - # perform rendering sim.step() - # update articulation articulation.update(sim.cfg.dt) # check that the root is at the correct state - its default state as it is fixed base @@ -2057,7 +1671,7 @@ def test_initialization_floating_base_made_fixed_base( @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["panda"]) def test_initialization_fixed_base_made_floating_base( @@ -2069,7 +1683,6 @@ def test_initialization_fixed_base_made_floating_base( 1. The articulation is properly initialized 2. The articulation is floating base after modification 3. All buffers have correct shapes - 4. The articulation can be simulated Args: sim: The simulation fixture @@ -2097,13 +1710,6 @@ def test_initialization_fixed_base_made_floating_base( assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) assert articulation.data.joint_pos.torch.shape == (num_articulations, 9) - # Simulate physics - for _ in range(10): - # perform rendering - sim.step() - # update articulation - articulation.update(sim.cfg.dt) - @pytest.mark.parametrize("device", ["cpu"]) @pytest.mark.parametrize("articulation_type", ["panda"]) @@ -2120,133 +1726,23 @@ def test_out_of_range_default_joint_state(sim, device, articulation_type, state_ # Check that the framework doesn't hold excessive strong references. assert sys.getrefcount(articulation) < 10 + quantity = "positions" if state_field == "joint_pos" else "velocities" replicate(sim.get_clone_plan()) - with pytest.raises(ValueError): + with pytest.raises(ValueError, match=f"default {quantity} out of the limits"): + replicate(sim.get_clone_plan()) sim.reset() -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test write_joint_limits_to_sim API and when default pos falls outside of the new limits. - - This test verifies that: - 1. Joint limits can be set correctly - 2. Default positions are preserved when setting new limits - 3. Joint limits can be set with indexing - 4. Invalid joint positions are properly handled - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device, add_ground_plane=True) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - # Check if articulation is initialized - assert articulation.is_initialized - - # Get current default joint pos - default_joint_pos = articulation._data.default_joint_pos.torch.clone() - - # Set new joint limits - limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) - limits[..., 0] = (torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0) * -1.0 - limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 - articulation.write_joint_position_limit_to_sim_index(limits=limits) - - # Check new limits are in place - torch.testing.assert_close(articulation._data.joint_pos_limits.torch, limits) - torch.testing.assert_close(articulation._data.default_joint_pos.torch, default_joint_pos) - - # Set new joint limits with indexing - env_ids = torch.arange(1, device=device, dtype=torch.int32) - joint_ids = torch.arange(2, device=device, dtype=torch.int32) - limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) - limits[..., 0] = (torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0) * -1.0 - limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0 - articulation.write_joint_position_limit_to_sim_index(limits=limits, env_ids=env_ids, joint_ids=joint_ids) - - # Check new limits are in place - torch.testing.assert_close(articulation._data.joint_pos_limits.torch[env_ids][:, joint_ids], limits) - torch.testing.assert_close(articulation._data.default_joint_pos.torch, default_joint_pos) - - # Set new joint limits that invalidate default joint pos - limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) - limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1 - limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 - articulation.write_joint_position_limit_to_sim_index(limits=limits) - - # Check if all values are within the bounds - default_joint_pos_torch = articulation._data.default_joint_pos.torch - within_bounds = (default_joint_pos_torch >= limits[..., 0]) & (default_joint_pos_torch <= limits[..., 1]) - assert torch.all(within_bounds) - - # Set new joint limits that invalidate default joint pos with indexing - limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) - limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 - limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 - articulation.write_joint_position_limit_to_sim_index(limits=limits, env_ids=env_ids, joint_ids=joint_ids) - - # Check if all values are within the bounds - default_joint_pos_torch = articulation._data.default_joint_pos.torch - within_bounds = (default_joint_pos_torch[env_ids][:, joint_ids] >= limits[..., 0]) & ( - default_joint_pos_torch[env_ids][:, joint_ids] <= limits[..., 1] - ) - assert torch.all(within_bounds) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_joint_effort_limits(sim, num_articulations, device, add_ground_plane, articulation_type): - """Validate joint effort limits via joint_effort_out_of_limit().""" - # Create articulation - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device, add_ground_plane=True) - - # Minimal env wrapper exposing scene["robot"] - class _Env: - def __init__(self, art): - self.scene = {"robot": art} - - env = _Env(articulation) - robot_all = SceneEntityCfg(name="robot") - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - # Case A: no clipping → should NOT terminate - articulation._data.computed_torque.torch.zero_() - articulation._data.applied_torque.torch.zero_() - out = joint_effort_out_of_limit(env, robot_all) # [N] - assert torch.all(~out) - - # Case B: simulate clipping → should terminate - articulation._data.computed_torque.torch.fill_(100.0) # pretend controller commanded 100 - articulation._data.applied_torque.torch.fill_(50.0) # pretend actuator clipped to 50 - out = joint_effort_out_of_limit(env, robot_all) # [N] - assert torch.all(out) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_buffer(sim, num_articulations, device, articulation_type): - """Test if external force buffer correctly updates in the force value is zero case. +def test_external_force_on_multiple_bodies(sim, num_articulations, device, articulation_type): + """Test application of external force on the legs of the articulation. This test verifies that: - 1. External forces can be applied correctly - 2. Force buffers are updated properly - 3. Zero forces are handled correctly + 1. External forces can be applied to multiple bodies + 2. The forces affect the articulation's motion correctly + 3. The articulation responds to the forces as expected Args: sim: The simulation fixture @@ -2255,84 +1751,49 @@ def test_external_force_buffer(sim, num_articulations, device, articulation_type articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # play the simulator + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() - # find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") - - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) + # Find bodies to apply the force + body_ids, _ = articulation.find_bodies(".*_SHANK") + # Sample a large force + external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_b[..., 1] = 200.0 # reset articulation articulation.reset() - + # apply force + articulation.permanent_wrench_composer.set_forces_and_torques_index( + forces=external_wrench_b[..., :3], torques=external_wrench_b[..., 3:], body_ids=body_ids + ) # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - - # check if the articulation's force and torque buffers are correctly updated - for i in range(num_articulations): - assert articulation.permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force - assert articulation.permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force - - # Check if the instantaneous wrench is correctly added to the permanent wrench - articulation.instantaneous_wrench_composer.add_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - + for _ in range(100): # apply action to the articulation articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) articulation.write_data_to_sim() - # perform step sim.step() - # update buffers articulation.update(sim.cfg.dt) + # check condition + for i in range(num_articulations): + # since there is a moment applied on the articulation, the articulation should rotate + assert articulation.data.root_ang_vel_w.torch[i, 2].item() > 0.1 @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_single_body(sim, num_articulations, device, articulation_type): - """Test application of external force on the base of the articulation. +def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device, articulation_type): + """Test application of external force on the legs of the articulation at a given position. This test verifies that: - 1. External forces can be applied to specific bodies - 2. The forces affect the articulation's motion correctly - 3. The articulation responds to the forces as expected + 1. External forces can be applied to multiple bodies at a given position + 2. External forces can be applied to multiple bodies in the global frame + 3. External forces are calculated and composed correctly + 4. The forces affect the articulation's motion correctly + 5. The articulation responds to the forces as expected Args: sim: The simulation fixture @@ -2340,241 +1801,18 @@ def test_external_force_on_single_body(sim, num_articulations, device, articulat """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() # Find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") + body_ids, _ = articulation.find_bodies(".*_SHANK") # Sample a large force external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 1] = 100.0 - - # Now we are ready! - for _ in range(2): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset articulation - articulation.reset() - # apply force - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], torques=external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition that the articulations have fallen down - for i in range(num_articulations): - assert articulation.data.root_pos_w.torch[i, 2].item() < 0.2 - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_single_body_at_position(sim, num_articulations, device, articulation_type): - """Test application of external force on the base of the articulation at a given position. - - This test verifies that: - 1. External forces can be applied to specific bodies at a given position - 2. External forces can be applied to specific bodies in the global frame - 3. External forces are calculated and composed correctly - 4. The forces affect the articulation's motion correctly - 5. The articulation responds to the forces as expected - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies("base") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 2] = 100.0 - external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - external_wrench_positions_b[..., 1] = 1.0 - - desired_force = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_force[..., 2] = 200.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 200.0 - - # Now we are ready! - for i in range(2): - # reset root state - root_pose = articulation.data.default_root_pose.torch.clone() - root_pose[0, 0] = 2.5 # space them apart by 2.5m - - articulation.write_root_pose_to_sim_index(root_pose=root_pose) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset articulation - articulation.reset() - # apply force - is_global = False - - if i % 2 == 0: - body_com_pos_w = articulation.data.body_com_pos_w.torch[:, body_ids, :3] - # is_global = True - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - external_wrench_positions_b += body_com_pos_w - else: - external_wrench_positions_b[..., 0] = 0.0 - external_wrench_positions_b[..., 1] = 1.0 - external_wrench_positions_b[..., 2] = 0.0 - - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=external_wrench_positions_b, - body_ids=body_ids, - is_global=is_global, - ) - articulation.permanent_wrench_composer.add_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - positions=external_wrench_positions_b, - body_ids=body_ids, - is_global=is_global, - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition that the articulations have fallen down - for i in range(num_articulations): - assert articulation.data.root_pos_w.torch[i, 2].item() < 0.2 - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_multiple_bodies(sim, num_articulations, device, articulation_type): - """Test application of external force on the legs of the articulation. - - This test verifies that: - 1. External forces can be applied to multiple bodies - 2. The forces affect the articulation's motion correctly - 3. The articulation responds to the forces as expected - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies(".*_SHANK") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 1] = 200.0 - - # Now we are ready! - for _ in range(2): - # reset root state - articulation.write_root_pose_to_sim_index(root_pose=articulation.data.default_root_pose.torch.clone()) - articulation.write_root_velocity_to_sim_index(root_velocity=articulation.data.default_root_vel.torch.clone()) - # reset dof state - joint_pos, joint_vel = ( - articulation.data.default_joint_pos.torch, - articulation.data.default_joint_vel.torch, - ) - articulation.write_joint_position_to_sim_index(position=joint_pos) - articulation.write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset articulation - articulation.reset() - # apply force - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], torques=external_wrench_b[..., 3:], body_ids=body_ids - ) - # perform simulation - for _ in range(100): - # apply action to the articulation - articulation.set_joint_position_target_index(target=articulation.data.default_joint_pos.torch.clone()) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - # check condition - for i in range(num_articulations): - # since there is a moment applied on the articulation, the articulation should rotate - assert articulation.data.root_ang_vel_w.torch[i, 2].item() > 0.1 - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device, articulation_type): - """Test application of external force on the legs of the articulation at a given position. - - This test verifies that: - 1. External forces can be applied to multiple bodies at a given position - 2. External forces can be applied to multiple bodies in the global frame - 3. External forces are calculated and composed correctly - 4. The forces affect the articulation's motion correctly - 5. The articulation responds to the forces as expected - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, _ = articulation.find_bodies(".*_SHANK") - # Sample a large force - external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device) - external_wrench_b[..., 2] = 50.0 - external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - external_wrench_positions_b[..., 1] = 1.0 - - desired_force = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_force[..., 2] = 200.0 - desired_torque = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) - desired_torque[..., 0] = 200.0 + external_wrench_b[..., 2] = 50.0 + external_wrench_positions_b = torch.zeros(articulation.num_instances, len(body_ids), 3, device=sim.device) + external_wrench_positions_b[..., 1] = 1.0 # Now we are ready! for i in range(2): @@ -2635,7 +1873,7 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["humanoid"]) def test_loading_gains_from_usd(sim, num_articulations, device, articulation_type): """Test that gains are loaded from USD file if actuator model has them as None. @@ -2697,70 +1935,41 @@ def test_loading_gains_from_usd(sim, num_articulations, device, articulation_typ torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["humanoid"]) -def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that gains are loaded from the configuration correctly. - - This test verifies that: - 1. Gains are loaded correctly from configuration - 2. The gains match the expected values - 3. The gains are applied correctly to the actuators - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=sim.device, add_ground_plane=True - ) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Expected gains - expected_stiffness = torch.full( - (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device - ) - expected_damping = torch.full_like(expected_stiffness, 2.0) - - # Check that gains are loaded from USD file - torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) - torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_velocity_limit", [1e5, None]) -@pytest.mark.parametrize("vel_limit", [1e2, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) # consumed by the sim fixture -def test_setting_velocity_limit_implicit( - sim, articulation_type, num_articulations, device, joint_velocity_limit, vel_limit +@pytest.mark.parametrize( + ("joint_limit", "actuator_limit"), + [(1e5, None), (None, 1e2), (1e5, 1e2)], + ids=["joint_limit", "actuator_limit", "both_limits"], +) +@pytest.mark.parametrize("articulation_type", ["single_joint_implicit", "single_joint_explicit"]) +@pytest.mark.parametrize("use_newton_actuators", [False]) # consumed by the sim fixture +def test_setting_joint_limits_from_cfg( + sim, articulation_type, num_articulations, device, joint_limit, actuator_limit, use_newton_actuators ): - """Test setting of velocity limit for implicit actuators. + """Test the velocity and effort limit resolution for implicit and explicit actuators. This test verifies that: - 1. The solver clamp ``joint_velocity_limit`` is applied to the simulation; when unset, the - USD-authored value is kept - 2. The actuator velocity limit ``actuator_velocity_limit`` is never pushed to the solver and keeps its - configured value; when unset, it falls back to the solver clamp + 1. The solver clamps ``joint_velocity_limit`` and ``joint_effort_limit`` are applied to the simulation; + when unset, the USD-authored values are kept + 2. The actuator limits keep their configured values and are never pushed to the solver + 3. When unset, the actuator velocity limit falls back to the solver clamp, implicit actuators track the + solver effort clamp, and an explicit actuator's effort limit falls back to the USD-authored value Args: sim: The simulation fixture + articulation_type: The implicit or explicit single-joint articulation num_articulations: Number of articulations to test device: The device to run the simulation on - joint_velocity_limit: The velocity limit to set in simulation - vel_limit: The velocity limit to set in actuator + joint_limit: The velocity and effort limits to set in simulation + actuator_limit: The velocity and effort limits to set in the actuator """ - # create simulation articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=vel_limit, + articulation_type=articulation_type, + joint_velocity_limit=joint_limit, + joint_effort_limit=joint_limit, + actuator_velocity_limit=actuator_limit, + actuator_effort_limit=actuator_limit, ) articulation, _ = generate_articulation( articulation_cfg=articulation_cfg, @@ -2772,208 +1981,56 @@ def test_setting_velocity_limit_implicit( sim.reset() # read the values set into the simulation - newton_vel_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_velocity_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - # check data buffer - torch.testing.assert_close(articulation.data.joint_vel_limits.torch, newton_vel_limit) - # the solver clamp comes from joint_velocity_limit when set, otherwise the USD-authored value - if joint_velocity_limit is None: - sim_limit = next( - p.max_joint_velocity for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, PhysxJointCfg) - ) - else: - sim_limit = joint_velocity_limit - expected_velocity_limit = torch.full_like(newton_vel_limit, sim_limit) - torch.testing.assert_close(newton_vel_limit, expected_velocity_limit) - - # the joint velocity limit keeps its configured value and is not pushed to the solver; - # when unset it falls back to the solver clamp - joint_limit = vel_limit if vel_limit is not None else sim_limit - expected_joint_limit = torch.full_like(newton_vel_limit, joint_limit) - torch.testing.assert_close(articulation.actuators["joint"].actuator_velocity_limit, expected_joint_limit) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_velocity_limit", [1e5, None]) -@pytest.mark.parametrize("vel_limit", [1e2, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) # consumed by the sim fixture -@pytest.mark.parametrize("use_newton_actuators", [False]) -def test_setting_velocity_limit_explicit( - sim, articulation_type, num_articulations, device, joint_velocity_limit, vel_limit, use_newton_actuators -): - """Test setting of velocity limit for explicit actuators.""" - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_explicit", - joint_velocity_limit=joint_velocity_limit, - actuator_velocity_limit=vel_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # collect limit init values - newton_vel_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_velocity_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - actuator_vel_limit = articulation.actuators["joint"].actuator_velocity_limit - - # check data buffer for joint_vel_limits - torch.testing.assert_close(articulation.data.joint_vel_limits.torch, newton_vel_limit) - - if vel_limit is not None: - expected_actuator_vel_limit = torch.full( - (articulation.num_instances, articulation.num_joints), - vel_limit, - device=articulation.device, - ) - # check actuator is set - torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit) - # check physx is not actuator_velocity_limit - assert not torch.allclose(actuator_vel_limit, newton_vel_limit) - else: - # check actuator_velocity_limit is the same as the PhysX default - torch.testing.assert_close(actuator_vel_limit, newton_vel_limit) - - # simulation velocity limit is set to USD value unless user overrides - if joint_velocity_limit is not None: - limit = joint_velocity_limit - else: - limit = next( - p.max_joint_velocity for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, PhysxJointCfg) - ) - # check physx is set to expected value - expected_vel_limit = torch.full_like(newton_vel_limit, limit) - torch.testing.assert_close(newton_vel_limit, expected_vel_limit) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_effort_limit", [1e5, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) # consumed by the sim fixture -def test_setting_effort_limit_implicit(sim, articulation_type, num_articulations, device, joint_effort_limit): - """Test setting of effort limit for implicit actuators. - - This test verifies the effort limit resolution logic for actuator models implemented in :class:`ActuatorBase`: - - Case 1: If USD value == actuator config value: values match correctly - - Case 2: If USD value != actuator config value: actuator config value is used - - Case 3: If actuator config value is None: USD value is used as default - """ - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_implicit", - joint_effort_limit=joint_effort_limit, - ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, - ) - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # obtain the physx effort limits - newton_effort_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_effort_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - - torch.testing.assert_close(articulation.data.joint_effort_limits.torch, newton_effort_limit) - torch.testing.assert_close(articulation.actuators["joint"].joint_effort_limit, newton_effort_limit) - # without a separately configured rated limit, the actuator limit tracks the solver clamp - torch.testing.assert_close(articulation.actuators["joint"].actuator_effort_limit, newton_effort_limit) - - # decide the limit based on what is set - if joint_effort_limit is None: - limit = next( - p.max_force for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, sim_utils.UsdPhysicsDriveCfg) - ) - else: - limit = joint_effort_limit - - # check that the max force is what we set - expected_effort_limit = torch.full_like(newton_effort_limit, limit) - torch.testing.assert_close(newton_effort_limit, expected_effort_limit) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("joint_effort_limit", [1e5, None]) -@pytest.mark.parametrize("actuator_effort_limit", [1e2, None]) -@pytest.mark.parametrize("articulation_type", ["single_joint_explicit"]) # consumed by the sim fixture -@pytest.mark.parametrize("use_newton_actuators", [False]) -def test_setting_effort_limit_explicit( - sim, - articulation_type, - num_articulations, - device, - joint_effort_limit, - actuator_effort_limit, - use_newton_actuators, -): - """Test setting of effort limit for explicit actuators. - - This test verifies the effort limit resolution logic for actuator models implemented in :class:`ActuatorBase`: - - Case 1: If USD value == actuator config value: values match correctly - - Case 2: If USD value != actuator config value: actuator config value is used - - Case 3: If actuator config value is None: USD value is used as default - - """ - - articulation_cfg = generate_articulation_cfg( - articulation_type="single_joint_explicit", - joint_effort_limit=joint_effort_limit, - actuator_effort_limit=actuator_effort_limit, + model = SimulationManager.get_model() + newton_vel_limit = wp.to_torch(articulation.root_view.get_attribute("joint_velocity_limit", model)).to(device)[ + :, 0, : + ] + newton_effort_limit = wp.to_torch(articulation.root_view.get_attribute("joint_effort_limit", model)).to(device)[ + :, 0, : + ] + usd_vel_limit = next( + p.max_joint_velocity for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, PhysxJointCfg) ) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, - num_articulations=num_articulations, - device=device, + usd_effort_limit = next( + p.max_force for p in articulation_cfg.spawn.joint_drive_props if isinstance(p, sim_utils.UsdPhysicsDriveCfg) ) - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # usd default effort limit is set to 80 - usd_default_effort_limit = 80.0 - - # collect limit init values - newton_effort_limit = wp.to_torch( - articulation.root_view.get_attribute("joint_effort_limit", SimulationManager.get_model()) - ).to(device)[:, 0, :] - actuator_effort_limit_actual = articulation.actuators["joint"].actuator_effort_limit + actuator = articulation.actuators["joint"] - if actuator_effort_limit is not None: - expected_actuator_effort_limit = torch.full_like(actuator_effort_limit_actual, actuator_effort_limit) - # check actuator is set - torch.testing.assert_close(actuator_effort_limit_actual, expected_actuator_effort_limit) + # check data buffers + torch.testing.assert_close(articulation.data.joint_vel_limits.torch, newton_vel_limit) + torch.testing.assert_close(articulation.data.joint_effort_limits.torch, newton_effort_limit) + # the solver clamps come from the joint limits when set, otherwise the USD-authored values + expected_vel_limit = usd_vel_limit if joint_limit is None else joint_limit + expected_effort_limit = usd_effort_limit if joint_limit is None else joint_limit + torch.testing.assert_close(newton_vel_limit, torch.full_like(newton_vel_limit, expected_vel_limit)) + torch.testing.assert_close(newton_effort_limit, torch.full_like(newton_effort_limit, expected_effort_limit)) + # the actuator velocity limit keeps its configured value and is not pushed to the solver; + # when unset it falls back to the solver clamp + if actuator_limit is not None: + torch.testing.assert_close(actuator.actuator_velocity_limit, torch.full_like(newton_vel_limit, actuator_limit)) + assert not torch.allclose(actuator.actuator_velocity_limit, newton_vel_limit) else: - # When actuator_effort_limit is None, actuator should use USD default values - expected_actuator_effort_limit = torch.full_like(newton_effort_limit, usd_default_effort_limit) - torch.testing.assert_close(actuator_effort_limit_actual, expected_actuator_effort_limit) - - # the solver keeps the authored limit unless the user overrides it explicitly - if joint_effort_limit is not None: - limit = joint_effort_limit + torch.testing.assert_close(actuator.actuator_velocity_limit, newton_vel_limit) + + if articulation_type == "single_joint_implicit": + # without a separately configured rated limit, the implicit actuator limits track the solver clamp + torch.testing.assert_close(actuator.joint_effort_limit, newton_effort_limit) + torch.testing.assert_close(actuator.actuator_effort_limit, newton_effort_limit) + elif actuator_limit is not None: + torch.testing.assert_close(actuator.actuator_effort_limit, torch.full_like(newton_effort_limit, actuator_limit)) else: - limit = usd_default_effort_limit - # check physx internal value matches the expected sim value - expected_effort_limit = torch.full_like(newton_effort_limit, limit) - torch.testing.assert_close(articulation.data.joint_effort_limits.torch, expected_effort_limit) - torch.testing.assert_close(newton_effort_limit, expected_effort_limit) + # an unset explicit actuator effort limit falls back to the USD-authored value, not the solver clamp + torch.testing.assert_close( + actuator.actuator_effort_limit, torch.full_like(newton_effort_limit, usd_effort_limit) + ) @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["humanoid"]) def test_reset(sim, num_articulations, device, articulation_type, monkeypatch): - """Test that reset method works properly.""" + """Test that the actuator gains come from the configuration and that reset method works properly.""" articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation( articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device @@ -2983,6 +2040,14 @@ def test_reset(sim, num_articulations, device, articulation_type, monkeypatch): replicate(sim.get_clone_plan()) sim.reset() + # Check that gains are loaded from the configuration + expected_stiffness = torch.full( + (articulation.num_instances, articulation.num_joints), 10.0, device=articulation.device + ) + expected_damping = torch.full_like(expected_stiffness, 2.0) + torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) + torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) + # Now we are ready! # reset articulation actuator = next(iter(articulation.actuators.values())) @@ -2997,91 +2062,51 @@ def record_actuator_reset(env_ids=None): articulation.reset() assert reset_env_ids == [None] - # Reset should zero external forces and torques - assert not articulation._instantaneous_wrench_composer.active - assert not articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == 0 - - if num_articulations > 1: - num_bodies = articulation.num_bodies - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=torch.ones((num_articulations, num_bodies, 3), device=device), - torques=torch.ones((num_articulations, num_bodies, 3), device=device), - ) - articulation.instantaneous_wrench_composer.add_forces_and_torques_index( - forces=torch.ones((num_articulations, num_bodies, 3), device=device), - torques=torch.ones((num_articulations, num_bodies, 3), device=device), - ) - articulation.reset(env_ids=torch.tensor([0], device=device)) - assert articulation._instantaneous_wrench_composer.active - assert articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == num_bodies * 3 - + instantaneous_composer = articulation.instantaneous_wrench_composer + permanent_composer = articulation.permanent_wrench_composer -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_apply_joint_command(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test applying of joint position target functions correctly for a robotic arm.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device, add_ground_plane=True + # Reset should zero external forces and torques + assert not instantaneous_composer.active + assert not permanent_composer.active + assert torch.count_nonzero(instantaneous_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(instantaneous_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(permanent_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(permanent_composer.out_torque_b.torch) == 0 + + # A partial reset clears only the selected environment's wrenches + num_bodies = articulation.num_bodies + permanent_composer.set_forces_and_torques_index( + forces=torch.ones((num_articulations, num_bodies, 3), device=device), + torques=torch.ones((num_articulations, num_bodies, 3), device=device), ) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # reset dof state - joint_pos = articulation.data.default_joint_pos.torch.clone() - joint_pos[:, 3] = 0.0 - - # apply action to the articulation - articulation.set_joint_position_target_index(target=joint_pos) - articulation.write_data_to_sim() - - for _ in range(100): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # Check that current joint position is not the same as default joint position, meaning - # the articulation moved. We can't check that it reached its desired joint position as the gains - # are not properly tuned - assert not torch.allclose(articulation.data.joint_pos.torch, joint_pos) + instantaneous_composer.add_forces_and_torques_index( + forces=torch.ones((num_articulations, num_bodies, 3), device=device), + torques=torch.ones((num_articulations, num_bodies, 3), device=device), + ) + articulation.reset(env_ids=torch.tensor([0], device=device)) + assert instantaneous_composer.active + assert permanent_composer.active + assert torch.count_nonzero(instantaneous_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(instantaneous_composer.out_torque_b.torch) == num_bodies * 3 + assert torch.count_nonzero(permanent_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(permanent_composer.out_torque_b.torch) == num_bodies * 3 @pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) -def test_body_root_state(sim, num_articulations, device, with_offset, articulation_type): +def test_body_root_state(sim, num_articulations, device, articulation_type): """Test for reading the `body_state_w` property. This test verifies that: - 1. Body states can be read correctly - 2. States are correct with and without offsets - 3. States are consistent across different devices + 1. The single-joint articulation initializes as fixed base with correctly shaped buffers + 2. Body link and center-of-mass states match an analytic pendulum with a center-of-mass offset + 3. The fixed root holds its default state Args: sim: The simulation fixture num_articulations: Number of articulations to test device: The device to run the simulation on - with_offset: Whether to test with offset """ sim._app_control_on_stop_handle = None articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) @@ -3095,16 +2120,24 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati assert articulation.is_initialized, "Articulation is not initialized" # Check that fixed base assert articulation.is_fixed_base, "Articulation is not a fixed base" + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) + assert articulation.data.joint_pos.torch.shape == (num_articulations, 1) + assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) + + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg # Resolve body indices by name (ordering may differ across physics backends) root_idx = articulation.body_names.index("CenterPivot") arm_idx = articulation.body_names.index("Arm") # change center of mass offset from link frame - if with_offset: - offset = [0.5, 0.0, 0.0] - else: - offset = [0.0, 0.0, 0.0] + offset = [0.5, 0.0, 0.0] # create com offsets — apply offset to the Arm body num_bodies = articulation.num_bodies @@ -3127,8 +2160,14 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati # update buffers articulation.update(sim.cfg.dt) + # check that the root is at the correct state - its default state as it is fixed base + default_root_pose = articulation.data.default_root_pose.torch.clone() + default_root_vel = articulation.data.default_root_vel.torch.clone() + default_root_pose[:, :3] = default_root_pose[:, :3] + env_pos + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, default_root_pose) + torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) + # get state properties - root_link_pose_w = articulation.data.root_link_pose_w.torch root_link_vel_w = articulation.data.root_link_vel_w.torch root_com_pose_w = articulation.data.root_com_pose_w.torch root_com_vel_w = articulation.data.root_com_vel_w.torch @@ -3137,167 +2176,63 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati body_com_pose_w = articulation.data.body_com_pose_w.torch body_com_vel_w = articulation.data.body_com_vel_w.torch - if with_offset: - # get joint state - joint_pos = articulation.data.joint_pos.torch.unsqueeze(-1) - joint_vel = articulation.data.joint_vel.torch.unsqueeze(-1) - - # LINK state - # angular velocity should be the same for both COM and link frames - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # lin_vel arm - lin_vel_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) - vx = -(link_offset[0]) * joint_vel * torch.sin(joint_pos) - vy = torch.zeros(num_articulations, 1, 1, device=device) - vz = (link_offset[0]) * joint_vel * torch.cos(joint_pos) - lin_vel_gt[:, arm_idx, :] = torch.cat([vx, vy, vz], dim=-1).squeeze(-2) - - # linear velocity of root link should be zero - torch.testing.assert_close(lin_vel_gt[:, root_idx, :], root_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) - # linear velocity of pendulum link should be - torch.testing.assert_close(lin_vel_gt, body_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) - - # ang_vel - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - - # COM state - # position and orientation shouldn't match for the _state_com_w but everything else will - pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) - px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) - py = torch.zeros(num_articulations, 1, 1, device=device) - pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) - pos_gt[:, arm_idx, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) - pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) - torch.testing.assert_close(pos_gt[:, root_idx, :], root_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) - torch.testing.assert_close(pos_gt, body_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) - - # orientation - com_quat_b = articulation.data.body_com_quat_b.torch - com_quat_w = math_utils.quat_mul(body_link_pose_w[..., 3:], com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:]) - torch.testing.assert_close(com_quat_w[:, root_idx, :], root_com_pose_w[..., 3:]) - - # angular velocity should be the same for both COM and link frames - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) - else: - # single joint center of masses are at link frames so they will be the same - torch.testing.assert_close(root_link_pose_w, root_com_pose_w) - torch.testing.assert_close(root_com_vel_w, root_link_vel_w) - torch.testing.assert_close(body_link_pose_w, body_com_pose_w) - torch.testing.assert_close(body_com_vel_w, body_link_vel_w) - - -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_write_root_state( - sim, num_articulations, device, with_offset, state_location, gravity_enabled, articulation_type -): - """Test the setters for root_state using both the link frame and center of mass as reference frame. - - This test verifies that: - 1. Root states can be written correctly - 2. States are correct with and without offsets - 3. States can be written for both COM and link frames - 4. States are consistent across different devices - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - with_offset: Whether to test with offset - state_location: Whether to test COM or link frame - """ - sim._app_control_on_stop_handle = None - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) - env_idx = torch.tensor([x for x in range(num_articulations)], device=device, dtype=torch.int32) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Resolve root body index by name (ordering may differ across physics backends) - root_idx = articulation.find_bodies("base")[0][0] - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([1.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - - # create com offsets - com = wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())) - new_com = offset - com[:, 0, root_idx, :] = new_com.squeeze(-2) - articulation.root_view.set_attribute("body_com", SimulationManager.get_model(), wp.from_torch(com, dtype=wp.vec3f)) - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) - - # check they are set - torch.testing.assert_close( - wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())), com - ) - - rand_state = torch.zeros(num_articulations, 13, device=device) - rand_state[..., :7] = articulation.data.default_root_pose.torch - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_idx = env_idx.to(device) - for i in range(10): - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - if state_location == "com": - if i % 2 == 0: - articulation.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - articulation.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - articulation.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) - elif state_location == "link": - if i % 2 == 0: - articulation.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - articulation.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - articulation.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) - - if state_location == "com": - torch.testing.assert_close(rand_state[..., :7], articulation.data.root_com_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_com_vel_w.torch) - elif state_location == "link": - torch.testing.assert_close(rand_state[..., :7], articulation.data.root_link_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_link_vel_w.torch) + # get joint state + joint_pos = articulation.data.joint_pos.torch.unsqueeze(-1) + joint_vel = articulation.data.joint_vel.torch.unsqueeze(-1) + + # LINK state + # angular velocity should be the same for both COM and link frames + torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) + torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) + + # lin_vel arm + lin_vel_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) + vx = -(link_offset[0]) * joint_vel * torch.sin(joint_pos) + vy = torch.zeros(num_articulations, 1, 1, device=device) + vz = (link_offset[0]) * joint_vel * torch.cos(joint_pos) + lin_vel_gt[:, arm_idx, :] = torch.cat([vx, vy, vz], dim=-1).squeeze(-2) + + # linear velocity of root link should be zero + torch.testing.assert_close(lin_vel_gt[:, root_idx, :], root_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) + # linear velocity of pendulum link should be + torch.testing.assert_close(lin_vel_gt, body_link_vel_w[..., :3], atol=1e-3, rtol=1e-1) + + # COM state + # position and orientation shouldn't match for the _state_com_w but everything else will + pos_gt = torch.zeros(num_articulations, num_bodies, 3, device=device) + px = (link_offset[0] + offset[0]) * torch.cos(joint_pos) + py = torch.zeros(num_articulations, 1, 1, device=device) + pz = (link_offset[0] + offset[0]) * torch.sin(joint_pos) + pos_gt[:, arm_idx, :] = torch.cat([px, py, pz], dim=-1).squeeze(-2) + pos_gt += env_pos.unsqueeze(-2).repeat(1, num_bodies, 1) + torch.testing.assert_close(pos_gt[:, root_idx, :], root_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) + torch.testing.assert_close(pos_gt, body_com_pose_w[..., :3], atol=1e-3, rtol=1e-1) + + # orientation + com_quat_b = articulation.data.body_com_quat_b.torch + com_quat_w = math_utils.quat_mul(body_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:]) + torch.testing.assert_close(com_quat_w[:, root_idx, :], root_com_pose_w[..., 3:]) @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) @pytest.mark.parametrize("state_location", ["com", "link", "root"]) @pytest.mark.parametrize("gravity_enabled", [False]) @pytest.mark.parametrize("articulation_type", ["anymal"]) def test_write_root_state_functions_data_consistency( - sim, num_articulations, device, with_offset, state_location, gravity_enabled, articulation_type + sim, num_articulations, device, state_location, gravity_enabled, articulation_type ): - """A root pose/velocity write must refresh the derived cross-frame caches without a sim step. + """A root pose/velocity write must read back in the written frame and refresh the derived cross-frame caches. Regression coverage for the velocity invalidation cleanup: writing the root center-of-mass (or link) velocity must invalidate the derived root link (or com) velocity so the next read re-derives it. Linear velocity differs between the two frames, so - as in the rigid object test - we compare angular velocity, which is frame-independent and therefore only matches when the derived velocity was actually refreshed. + + The center-of-mass offset is set through ``set_coms_index``, which must update the body-frame CoM and + the world-frame CoM derived from it without a sim step. """ sim._app_control_on_stop_handle = None articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) @@ -3311,20 +2246,56 @@ def test_write_root_state_functions_data_consistency( root_idx = articulation.find_bodies("base")[0][0] # change center of mass offset from link frame on the root body - if with_offset: - offset = torch.tensor([1.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0]).repeat(num_articulations, 1, 1) - com = wp.to_torch(articulation.root_view.get_attribute("body_com", SimulationManager.get_model())) - com[:, 0, root_idx, :] = offset.squeeze(-2) - articulation.root_view.set_attribute("body_com", SimulationManager.get_model(), wp.from_torch(com, dtype=wp.vec3f)) - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) + original_com = articulation.data.body_com_pos_b.torch.clone() + # Populate the derived world-frame cache so a missing invalidation would surface as a stale read. + original_com_w = articulation.data.body_com_pos_w.torch.clone() + new_com = original_com.clone() + new_com[:, root_idx] = torch.tensor([1.0, 0.0, 0.0], device=device) + env_ids = torch.arange(num_articulations, device=device, dtype=torch.int32) + # Full poses are accepted too; Newton uses the position and ignores the orientation. + com_poses = articulation.data.body_com_pose_b.torch.clone() + com_poses[..., :3] = new_com + articulation.set_coms_index(coms=com_poses, env_ids=env_ids) + torch.testing.assert_close(articulation.data.body_com_pos_b.torch, new_com, atol=1e-5, rtol=1e-5) + articulation.set_coms_index(coms=original_com, env_ids=env_ids) + torch.testing.assert_close(articulation.data.body_com_pos_b.torch, original_com, atol=1e-5, rtol=1e-5) + _ = articulation.data.body_com_pos_w.torch + articulation.set_coms_index(coms=new_com, env_ids=env_ids) + + torch.testing.assert_close(articulation.data.body_com_pos_b.torch, new_com, atol=1e-5, rtol=1e-5) + # Without a sim step the links stay put, so the world-frame CoM is the link pose applied to the new offset. + link_pos_w = articulation.data.body_link_pos_w.torch + link_quat_w = articulation.data.body_link_quat_w.torch + expected_com_w = link_pos_w + math_utils.quat_apply(link_quat_w, new_com) + updated_com_w = articulation.data.body_com_pos_w.torch + torch.testing.assert_close(updated_com_w, expected_com_w, atol=1e-5, rtol=1e-5) + assert not torch.allclose(updated_com_w, original_com_w) + + def random_root_state(num_envs: int) -> torch.Tensor: + state = torch.rand(num_envs, 13, device=device) + state[..., :3] += env_pos[:num_envs] + # make quaternion a unit vector + state[..., 3:7] = torch.nn.functional.normalize(state[..., 3:7], dim=-1) + return state + + def write_root_state(state: torch.Tensor, env_ids: torch.Tensor | None = None): + if state_location == "com": + articulation.write_root_com_pose_to_sim_index(root_pose=state[..., :7], env_ids=env_ids) + articulation.write_root_com_velocity_to_sim_index(root_velocity=state[..., 7:], env_ids=env_ids) + elif state_location == "link": + articulation.write_root_link_pose_to_sim_index(root_pose=state[..., :7], env_ids=env_ids) + articulation.write_root_link_velocity_to_sim_index(root_velocity=state[..., 7:], env_ids=env_ids) + elif state_location == "root": + articulation.write_root_pose_to_sim_index(root_pose=state[..., :7], env_ids=env_ids) + articulation.write_root_velocity_to_sim_index(root_velocity=state[..., 7:], env_ids=env_ids) - rand_state = torch.rand(num_articulations, 13, device=device) - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + def read_written_root_state() -> torch.Tensor: + # the root pose is the link pose and the root velocity is the center-of-mass velocity + pose_w = articulation.data.root_com_pose_w if state_location == "com" else articulation.data.root_link_pose_w + vel_w = articulation.data.root_link_vel_w if state_location == "link" else articulation.data.root_com_vel_w + return torch.cat((pose_w.torch, vel_w.torch), dim=-1) + + rand_state = random_root_state(num_articulations) # perform a step then update the buffers sim.step() @@ -3338,15 +2309,7 @@ def test_write_root_state_functions_data_consistency( _ = articulation.data.root_link_vel_w.torch _ = articulation.data.root_com_vel_w.torch - if state_location == "com": - articulation.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "link": - articulation.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "root": - articulation.write_root_pose_to_sim_index(root_pose=rand_state[..., :7]) - articulation.write_root_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + write_root_state(rand_state) body_com_pose_b = articulation.data.body_com_pose_b.torch if state_location == "com": @@ -3384,6 +2347,16 @@ def test_write_root_state_functions_data_consistency( torch.testing.assert_close(expected_com_pose, root_com_pose_w) torch.testing.assert_close(root_link_vel_w[:, 3:], root_com_vel_w[:, 3:]) + # the written state reads back in the written frame + torch.testing.assert_close(read_written_root_state(), rand_state) + + # a partial write only changes the selected environment + partial_state = random_root_state(1) + write_root_state(partial_state, env_ids=torch.tensor([0], device=device, dtype=torch.int32)) + expected_state = rand_state.clone() + expected_state[0] = partial_state[0] + torch.testing.assert_close(read_written_root_state(), expected_state) + @pytest.mark.parametrize("device", ["cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) @@ -3401,9 +2374,10 @@ def test_setting_articulation_root_prim_path(sim, device, articulation_type, roo if root_prim_path == "/torso": replicate(sim.get_clone_plan()) sim.reset() - assert articulation._is_initialized + assert articulation.is_initialized else: - with pytest.raises((RuntimeError, KeyError)): + replicate(sim.get_clone_plan()) + with pytest.raises(KeyError, match="No articulations matching pattern"): sim.reset() @@ -3412,11 +2386,14 @@ def test_setting_articulation_root_prim_path(sim, device, articulation_type, roo @pytest.mark.parametrize("gravity_enabled", [False]) @pytest.mark.parametrize("articulation_type", ["anymal"]) def test_write_joint_state_data_consistency(sim, num_articulations, device, gravity_enabled, articulation_type): - """Test the setters for root_state using both the link frame and center of mass as reference frame. + """Joint limit and joint state writes update the joint buffers and refresh the body state without a step. + + This test verifies that: + 1. Joint position limits are written and keep in-limit default joint positions + 2. A partial joint state write with unsorted int64 selectors updates only the selected entries + 3. A joint state write moves the bodies and refreshes the derived body poses and velocities + 4. Indexed joint limits that exclude a default joint position clamp it into the new limits - This test verifies that after write_joint_state_to_sim operations: - 1. state, com_state, link_state value consistency - 2. body_pose, link Args: sim: The simulation fixture num_articulations: Number of articulations to test @@ -3431,11 +2408,33 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav replicate(sim.get_clone_plan()) sim.reset() + # Get current default joint pos + default_joint_pos = articulation.data.default_joint_pos.torch.clone() + limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) limits[..., 0] = (torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0) * -1.0 limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 articulation.write_joint_position_limit_to_sim_index(limits=limits) + # Check new limits are in place and the in-limit defaults are preserved + torch.testing.assert_close(articulation.data.joint_pos_limits.torch, limits) + torch.testing.assert_close(articulation.data.default_joint_pos.torch, default_joint_pos) + + # Write joint state with unsorted int64 selectors + env_ids = torch.tensor([1, 0], dtype=torch.int64, device=device) + joint_ids = torch.tensor([articulation.num_joints - 1, 0], dtype=torch.int64, device=device) + position = torch.tensor([[0.21, 0.11], [0.22, 0.12]], device=device) + velocity = torch.tensor([[1.21, 1.11], [1.22, 1.12]], device=device) + expected_position = articulation.data.joint_pos.torch.clone() + expected_velocity = articulation.data.joint_vel.torch.clone() + articulation.write_joint_state_to_sim_index( + position=position, velocity=velocity, env_ids=env_ids, joint_ids=joint_ids + ) + expected_position[env_ids[:, None], joint_ids[None, :]] = position + expected_velocity[env_ids[:, None], joint_ids[None, :]] = velocity + torch.testing.assert_close(articulation.data.joint_pos.torch, expected_position) + torch.testing.assert_close(articulation.data.joint_vel.torch, expected_velocity) + from torch.distributions import Uniform joint_pos_limits = articulation.data.joint_pos_limits.torch @@ -3459,11 +2458,9 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav assert torch.count_nonzero(original_body_states[:, 1:] != body_state_w[:, 1:]) > ( len(original_body_states[:, 1:]) / 2 ) - # validate body - link consistency - body_link_vel_w = articulation.data.body_link_vel_w.torch - torch.testing.assert_close(body_link_pose_w, articulation.data.body_link_pose_w.torch) # skip lin_vel because it differs from link frame, this should be fine because we are only checking # if velocity update is triggered, which can be determined by comparing angular velocity + body_link_vel_w = articulation.data.body_link_vel_w.torch torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) # validate link - com conistency @@ -3475,78 +2472,39 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav body_com_pos_b.view(-1, 3), body_com_quat_b.view(-1, 4), ) - body_com_pos_w = articulation.data.body_com_pos_w.torch - body_com_quat_w = articulation.data.body_com_quat_w.torch - torch.testing.assert_close(expected_com_pos.view(len(env_idx), -1, 3), body_com_pos_w) - torch.testing.assert_close(expected_com_quat.view(len(env_idx), -1, 4), body_com_quat_w) - - # validate body - com consistency - body_com_lin_vel_w = articulation.data.body_com_lin_vel_w.torch - body_com_ang_vel_w = articulation.data.body_com_ang_vel_w.torch - torch.testing.assert_close(body_com_vel_w[..., :3], body_com_lin_vel_w) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_com_ang_vel_w) - - # validate pos_w, quat_w, pos_b, quat_b is consistent with pose_w and pose_b - expected_com_pose_w = torch.cat((body_com_pos_w, body_com_quat_w), dim=2) - expected_com_pose_b = torch.cat((body_com_pos_b, body_com_quat_b), dim=2) - body_pos_w = articulation.data.body_pos_w.torch - body_quat_w = articulation.data.body_quat_w.torch - expected_body_pose_w = torch.cat((body_pos_w, body_quat_w), dim=2) - body_link_pos_w = articulation.data.body_link_pos_w.torch - body_link_quat_w = articulation.data.body_link_quat_w.torch - expected_body_link_pose_w = torch.cat((body_link_pos_w, body_link_quat_w), dim=2) - body_com_pose_w = articulation.data.body_com_pose_w.torch - body_com_pose_b = articulation.data.body_com_pose_b.torch - body_pose_w = articulation.data.body_pose_w.torch - body_link_pose_w_fresh = articulation.data.body_link_pose_w.torch - torch.testing.assert_close(body_com_pose_w, expected_com_pose_w) - torch.testing.assert_close(body_com_pose_b, expected_com_pose_b) - torch.testing.assert_close(body_pose_w, expected_body_pose_w) - torch.testing.assert_close(body_link_pose_w_fresh, expected_body_link_pose_w) - - # validate pose_w is consistent with individual properties - body_vel_w = articulation.data.body_vel_w.torch - body_com_vel_w_fresh = articulation.data.body_com_vel_w.torch - torch.testing.assert_close(body_pose_w, body_link_pose_w) - torch.testing.assert_close(body_vel_w, body_com_vel_w) - torch.testing.assert_close(body_link_pose_w_fresh, body_link_pose_w) - torch.testing.assert_close(body_com_pose_w, articulation.data.body_com_pose_w.torch) - torch.testing.assert_close(body_vel_w, body_com_vel_w_fresh) - - -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("articulation_type", ["panda"]) -def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test static joint friction writes propagate directly to the Newton model.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device, add_ground_plane=True - ) + torch.testing.assert_close(expected_com_pos.view(len(env_idx), -1, 3), articulation.data.body_com_pos_w.torch) + torch.testing.assert_close(expected_com_quat.view(len(env_idx), -1, 4), articulation.data.body_com_quat_w.torch) - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() + # Set new joint limits with indexing that invalidate the selected default joint positions + env_ids = torch.arange(1, device=device, dtype=torch.int32) + joint_ids = torch.nonzero(default_joint_pos[0].abs() > 0.1).squeeze(-1)[:2].to(torch.int32) + assert len(joint_ids) == 2 + limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) + limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 + limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 + articulation.write_joint_position_limit_to_sim_index(limits=limits, env_ids=env_ids, joint_ids=joint_ids) - friction = torch.rand(num_articulations, articulation.num_joints, device=device) - articulation.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=friction, + # Check new limits are in place and the defaults are clamped into them + torch.testing.assert_close(articulation.data.joint_pos_limits.torch[env_ids][:, joint_ids], limits) + default_joint_pos_torch = articulation.data.default_joint_pos.torch + within_bounds = (default_joint_pos_torch[env_ids][:, joint_ids] >= limits[..., 0]) & ( + default_joint_pos_torch[env_ids][:, joint_ids] <= limits[..., 1] ) - joint_friction_coeff_sim = wp.to_torch( - articulation.root_view.get_attribute("joint_friction", SimulationManager.get_model()) - )[:, 0, :] - torch.testing.assert_close(joint_friction_coeff_sim, friction) + assert torch.all(within_bounds) @pytest.mark.parametrize("selector_kind", ["index", "mask"]) +@pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("articulation_type", ["panda"]) @pytest.mark.parametrize("device", test_devices()) -def test_write_joint_viscous_friction_to_sim(sim, device, articulation_type, selector_kind): - """Test passive viscous joint damping is distinct from actuator derivative gains.""" +def test_write_joint_viscous_friction_to_sim(sim, num_articulations, device, articulation_type, selector_kind): + """Test passive viscous joint damping is distinct from actuator derivative gains. + + Static joint friction writes also propagate directly to the Newton model. + """ articulation_cfg = generate_articulation_cfg(articulation_type) articulation_cfg.actuators["panda_shoulder"].viscous_friction = 0.25 - articulation, _ = generate_articulation(articulation_cfg, 1, device) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) replicate(sim.get_clone_plan()) sim.reset() @@ -3587,6 +2545,15 @@ def test_write_joint_viscous_friction_to_sim(sim, device, articulation_type, sel values, ) + # Distinct per-env rows catch writers that ignore the env index + friction = torch.rand(articulation.num_instances, articulation.num_joints, device=device) + assert not torch.allclose(friction[0], friction[1]) + articulation.write_joint_friction_coefficient_to_sim_index(joint_friction_coeff=friction) + joint_friction_coeff_sim = wp.to_torch( + articulation.root_view.get_attribute("joint_friction", SimulationManager.get_model()) + )[:, 0, :] + torch.testing.assert_close(joint_friction_coeff_sim, friction) + @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @@ -3670,135 +2637,106 @@ def _patched_simulate(cls): ) -@pytest.mark.parametrize("num_articulations", [2]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_randomize_rigid_body_com(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that randomize_rigid_body_com modifies CoM and affects simulation dynamics.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - original_com = articulation.data.body_com_pos_b.torch.clone() - - com_offset = torch.zeros(num_articulations, articulation.num_bodies, 3, device=device) - com_offset[..., 0] = 0.5 - new_com = original_com + com_offset - env_ids = torch.arange(num_articulations, device=device, dtype=torch.int32) - articulation.set_coms_index(coms=new_com, env_ids=env_ids) - - updated_com = articulation.data.body_com_pos_b.torch - torch.testing.assert_close(updated_com, new_com, atol=1e-5, rtol=1e-5) - - # poses (position and quaternion) are accepted too, like on the other backends; the orientation is ignored - com_poses = articulation.data.body_com_pose_b.torch.clone() - com_poses[..., :3] = original_com - articulation.set_coms_index(coms=com_poses, env_ids=env_ids) - torch.testing.assert_close(articulation.data.body_com_pos_b.torch, original_com, atol=1e-5, rtol=1e-5) - - @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["anymal"]) -def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, add_ground_plane, articulation_type): - """Test that Newton collider offset randomization (shape_margin, shape_gap) takes effect.""" - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) - - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - model = SimulationManager.get_model() - original_margin = wp.to_torch(articulation.root_view.get_attribute("shape_margin", model)).clone() - original_gap = wp.to_torch(articulation.root_view.get_attribute("shape_gap", model)).clone() - - new_margin = original_margin.clone() - new_margin[:, 0] += 0.01 - articulation.root_view.set_attribute("shape_margin", model, wp.from_torch(new_margin, dtype=wp.float32)) - - new_gap = original_gap.clone() - new_gap[:, 0] += 0.005 - articulation.root_view.set_attribute("shape_gap", model, wp.from_torch(new_gap, dtype=wp.float32)) - - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.SHAPE_PROPERTIES) - - updated_margin = wp.to_torch(articulation.root_view.get_attribute("shape_margin", model)) - updated_gap = wp.to_torch(articulation.root_view.get_attribute("shape_gap", model)) - torch.testing.assert_close(updated_margin, new_margin) - torch.testing.assert_close(updated_gap, new_gap) - - -## -# Shape-contract regression tests for the new BaseArticulation accessors. -# These pin the public shape contract so future regressions (e.g., reverting -# to model-wide max sizing or to the wrong fixed-base row offset) fail fast. -## - - -@pytest.mark.parametrize("num_articulations", [4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) -@pytest.mark.isaacsim_ci -def test_dynamics_accessor_shapes(sim, num_articulations, device, add_ground_plane, articulation_type): - """Pin the per-articulation shapes of the Jacobian, mass matrix and gravity compensation accessors. - - Also checks that the mass matrix is symmetric and positive-definite. - - Fixed-base (panda): ``body_link_jacobian_w`` drops the fixed-root row, so its shape is - ``(N, num_bodies - 1, 6, num_joints)``; ``mass_matrix`` is ``(N, num_joints, num_joints)`` and - ``gravity_compensation_forces`` is ``(N, num_joints)``. - - Floating-base (anymal): every body row is kept and ``num_base_dofs`` floating-base columns/entries - are prepended on the DoF axis, matching the cross-library convention (Pinocchio, Drake, MuJoCo, - RBDL, OCS2, iDynTree). +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("body_subset", [False, True]) +def test_set_material_properties(sim, num_articulations, device, add_ground_plane, articulation_type, body_subset): + """Material and collider-offset randomization write through to the robot's shapes in the Newton model. - Both catch (a) the link_offset fix that drops Newton's row 0 for fixed-base and (b) per-articulation - output sizing: model-wide ``max_links``/``max_dofs`` would over-allocate in heterogeneous scenes and - surface as zero-padded mass-matrix diagonals, which is why the diagonal is asserted strictly positive - rather than checking a determinant (a well-formed 9x9 Franka mass matrix has det ~1e-13). + The event terms write the asset's view-level shape bindings; the assertions read the flat Newton model + arrays at the collision shapes of the selected bodies and environments. Visual shapes are ignored because + their materials and offsets have no physical effect. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device, add_ground_plane=True + ) + + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() - assert articulation.is_initialized - assert articulation.is_fixed_base == (articulation_type == "panda") - - num_dofs = articulation.num_joints + articulation.num_base_dofs - num_jacobian_bodies = articulation.num_bodies - 1 if articulation.is_fixed_base else articulation.num_bodies - - J = articulation.data.body_link_jacobian_w.torch - assert J.shape == torch.Size((num_articulations, num_jacobian_bodies, 6, num_dofs)), tuple(J.shape) - assert J.dtype == torch.float32 - g = articulation.data.gravity_compensation_forces.torch - assert g.shape == torch.Size((num_articulations, num_dofs)), tuple(g.shape) - assert g.dtype == torch.float32 + # Resolve the robot's shapes per environment and body from the flat Newton model. + model = SimulationManager.get_model() + body_world = model.body_world.numpy() + body_names = [label.rsplit("/", 1)[-1] for label in model.body_label] + shape_body = model.shape_body.numpy() + is_collision_shape = (model.shape_flags.numpy() & int(ShapeFlags.COLLIDE_SHAPES)) != 0 + + def robot_shapes(env_index: int, selected_body_names: list[str] | None = None) -> np.ndarray: + bodies = [ + body + for body in np.flatnonzero(body_world == env_index) + if selected_body_names is None or body_names[body] in selected_body_names + ] + return np.flatnonzero(np.isin(shape_body, bodies) & is_collision_shape) + + env = SimpleNamespace(scene={"robot": articulation}, sim=sim, device=device, num_envs=num_articulations) + env_ids = torch.tensor([num_articulations - 1], device=device) + + # Randomize the materials in the last environment, with degenerate ranges. + asset_cfg = SceneEntityCfg("robot") + selected_body_names = None + if body_subset: + asset_cfg.body_ids, selected_body_names = articulation.find_bodies(["panda_link3", "panda_hand"]) + selected_shapes = robot_shapes(num_articulations - 1, selected_body_names) + unselected_shapes = np.setdiff1d(robot_shapes(num_articulations - 1), selected_shapes) + other_env_shapes = robot_shapes(0) + assert len(selected_shapes) > 0 and len(other_env_shapes) > 0 + original_mu = model.shape_material_mu.numpy().copy() + original_restitution = model.shape_material_restitution.numpy().copy() + params = { + "static_friction_range": (0.55, 0.55), + "dynamic_friction_range": (0.55, 0.55), + "restitution_range": (0.15, 0.15), + "num_buckets": 1, + "asset_cfg": asset_cfg, + } + material_term = randomize_rigid_body_material( + EventTermCfg(func=randomize_rigid_body_material, mode="startup", params=params), env + ) + material_term(env, env_ids, **params) + + # Randomize the collider offsets of every shape in the last environment. + original_margin = model.shape_margin.numpy().copy() + original_gap = model.shape_gap.numpy().copy() + offset_params = { + "asset_cfg": SceneEntityCfg("robot"), + "rest_offset_distribution_params": (0.01, 0.01), + "contact_offset_distribution_params": (0.03, 0.03), + } + offset_term = randomize_rigid_body_collider_offsets( + EventTermCfg(func=randomize_rigid_body_collider_offsets, mode="startup", params=offset_params), env + ) + offset_term(env, env_ids, **offset_params) + # Simulate physics sim.step() articulation.update(sim.cfg.dt) - M = articulation.data.mass_matrix.torch - assert M.shape == torch.Size((num_articulations, num_dofs, num_dofs)), tuple(M.shape) - assert M.dtype == torch.float32 - diag = M.diagonal(dim1=-2, dim2=-1) - assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" + mu = model.shape_material_mu.numpy() + restitution = model.shape_material_restitution.numpy() + np.testing.assert_allclose(mu[selected_shapes], 0.55) + np.testing.assert_allclose(restitution[selected_shapes], 0.15) + for shapes in (unselected_shapes, other_env_shapes): + np.testing.assert_array_equal(mu[shapes], original_mu[shapes]) + np.testing.assert_array_equal(restitution[shapes], original_restitution[shapes]) - # The joint-space inertia is symmetric by construction; asymmetry means a wrong-axis gather or a - # half-populated buffer. OSC inverts ``J M^-1 J^T`` every step, so ``M`` must also be positive-definite. - asym = (M - M.transpose(-1, -2)).abs().max().item() - assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric" - # A tiny jitter tolerates the float32 eigenvalue floor without masking real non-PD bugs. - eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) - torch.linalg.cholesky(M + 1e-6 * eye) + # Newton maps the rest offset to the shape margin and the contact offset to margin + gap. + randomized_shapes = robot_shapes(num_articulations - 1) + np.testing.assert_allclose(model.shape_margin.numpy()[randomized_shapes], 0.01) + np.testing.assert_allclose(model.shape_gap.numpy()[randomized_shapes], 0.02, atol=1e-6) + np.testing.assert_array_equal(model.shape_margin.numpy()[other_env_shapes], original_margin[other_env_shapes]) + np.testing.assert_array_equal(model.shape_gap.numpy()[other_env_shapes], original_gap[other_env_shapes]) + + +## +# Shape-contract regression tests for the new BaseArticulation accessors. +# These pin the public shape contract so future regressions (e.g., reverting +# to model-wide max sizing or to the wrong fixed-base row offset) fail fast. +## @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @@ -3905,88 +2843,6 @@ def test_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, arti assert anymal_g.abs().max() > 1e-3, "Anymal gravity compensation is all-zero under heterogeneous scene" -@pytest.mark.parametrize("num_articulations", [4]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci -def test_get_jacobians_link_origin_contract(sim, num_articulations, device, articulation_type, gravity_enabled): - """``J · q_dot`` must encode the link-origin twist (after the COM->origin shift). - - The IsaacLab task-space controllers (IK / OSC / RMPFlow) silently - rely on :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` - returning a Jacobian whose linear rows reference each link's origin - (the body's USD prim transform), not its COM. Newton's ``eval_jacobian`` - natively produces COM-referenced rows; the wrapper applies a per-column - shift ``v_origin = v_com - omega x (R · body_com_pos_b)`` to honor the - contract. This test asserts the identity by computing both sides - independently: - - * Predicted by ``J · q_dot``: takes the (already-shifted) Jacobian - and the same ``q_dot`` Newton has after the kinematics refresh. Linear rows should - equal v_origin. - * Ground truth from ``state.body_qd``: read Newton's per-body spatial - twist directly via ``ArticulationView.get_link_velocities`` (which - returns ``(v_com_world, omega_world)``), then apply the same shift - in python and compare. - - Reading the velocity from the ArticulationView state rather than - ``data.body_com_lin_vel_w`` bypasses the IsaacLab lazy-buffer chain, - which is irrelevant to the contract being tested. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - # Reproducible non-trivial q_dot — large enough to drive omega well above - # the floor where COM offset effects would round into noise. - torch.manual_seed(0) - qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 - articulation.write_joint_velocity_to_sim_index(velocity=qdot) - # Refresh kinematics without integrating: for floating bases, an actuator step can - # introduce root motion that is intentionally absent from the actuated-only J slice. - sim.forward() - articulation.update(sim.cfg.dt) - - # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past - # them so the joint axis aligns with joint_vel (actuated-only). - J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :] - qdot_view = articulation.data.joint_vel.torch - v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) # (N, B_jac, 6) - v_pred_lin = v_pred[..., 0:3] - v_pred_ang = v_pred[..., 3:6] - - # Ground truth from Newton state. ``get_link_velocities`` returns shape - # (num_instances, 1, num_bodies, 6) — per-articulation grouping with - # one articulation per instance — so we squeeze the inner dim. - state = SimulationManager.get_state_0() - body_qd_view = wp.to_torch(articulation.root_view.get_link_velocities(state)).squeeze(1) - body_v_com = body_qd_view[..., :3] - body_omega = body_qd_view[..., 3:] - - # World-frame COM-to-origin offset, derived from already-computed - # data layer outputs (avoids quaternion-convention pitfalls). - body_com_pos_w = articulation.data.body_com_pos_w.torch # (N, num_bodies, 3) - body_link_pos_w = articulation.data.body_link_pos_w.torch # (N, num_bodies, 3) - c_world = body_com_pos_w - body_link_pos_w - - if articulation.is_fixed_base: - body_v_com = body_v_com[:, 1:] - body_omega = body_omega[:, 1:] - c_world = c_world[:, 1:] - - # Expected v_origin = v_com - omega x c_world. - v_origin_expected = body_v_com - torch.cross(body_omega, c_world, dim=-1) - - # Tolerance: 5 mm absolute. The COM-offset bug produces a ~3 cm bias - # on the panda hand under the 0.5-rad/s injected qdot, well above - # this floor; numerical noise from kernel ordering stays under 1 mm. - torch.testing.assert_close(v_pred_ang, body_omega, atol=5e-3, rtol=1e-2) - torch.testing.assert_close(v_pred_lin, v_origin_expected, atol=5e-3, rtol=1e-2) - - @pytest.mark.parametrize("num_articulations", [4]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -4000,12 +2856,11 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( Newton computes the gravity compensation force through an RNEA pass (``eval_inverse_dynamics_passive``); the static identity above derives the same - quantity independently from the (already contract-validated) COM-referenced - Jacobian and the per-body masses, pinning the sign convention, the DoF - ordering (including the 6 floating-base entries), and the flat-buffer view - gather in one assertion. Non-default joint positions and — for - floating-base — a rotated, lifted root pose guard the corner fixed - upstream in newton#2625 (wrong gravity compensation under non-identity + quantity independently from the COM-referenced Jacobian and the per-body masses, + pinning the sign convention, the DoF ordering (including the 6 floating-base + entries), and the flat-buffer view gather in one assertion. Non-default joint + positions and — for floating-base — a rotated, lifted root pose guard the corner + fixed upstream in newton#2625 (wrong gravity compensation under non-identity root pose). With ``ordering_mode="reversed"`` a nonidentity joint ordering is active and @@ -4013,15 +2868,82 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( Jacobian gather applies the user->backend permutation, so a ``gather_dof_force_rows`` that skips it returns backend-ordered forces and breaks the identity row-wise. + + The same fixture also pins: + + * the per-articulation shapes of the Jacobian, mass matrix and gravity compensation + accessors, and a symmetric, positive-definite mass matrix. Fixed-base (panda): + ``body_link_jacobian_w`` drops the fixed-root row, so its shape is + ``(N, num_bodies - 1, 6, num_joints)``. Floating-base (anymal): every body row is kept and + ``num_base_dofs`` floating-base columns/entries are prepended on the DoF axis, matching the + cross-library convention (Pinocchio, Drake, MuJoCo, RBDL, OCS2, iDynTree). A zero-padded + model-wide sizing would surface as non-positive mass-matrix diagonals; + * that every dynamics accessor reflects a manual joint write without a sim step (the FK + trigger before ``eval_jacobian``, ``eval_mass_matrix`` and the RNEA pass); + * the link-origin Jacobian contract: ``J · q_dot`` must encode the link-origin twist + ``v_origin = v_com - omega x (R · body_com_pos_b)``, which the IsaacLab task-space + controllers (IK / OSC / RMPFlow) rely on. Newton's ``eval_jacobian`` natively produces + COM-referenced rows, so the ground truth reads Newton's per-body twist directly from the + ArticulationView state. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) if ordering_mode == "reversed": joint_names = PANDA_JOINT_NAMES if articulation_type == "panda" else ANYMAL_C_PHYSX_JOINT_NAMES articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(joint_names))) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device, add_ground_plane=True) + + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(articulation) < 10 + replicate(sim.get_clone_plan()) sim.reset() assert articulation.is_initialized + assert articulation.is_fixed_base == (articulation_type == "panda") + # Check buffers that exists and have correct shapes + assert articulation.data.root_pos_w.torch.shape == (num_articulations, 3) + assert articulation.data.root_quat_w.torch.shape == (num_articulations, 4) + assert articulation.data.joint_pos.torch.shape == (num_articulations, 9 if articulation_type == "panda" else 12) + assert articulation.data.body_mass.torch.shape == (num_articulations, articulation.num_bodies) + assert articulation.data.body_inertia.torch.shape == (num_articulations, articulation.num_bodies, 9) + + # -- actuator type + for actuator_name, actuator in articulation.actuators.items(): + is_implicit_model_cfg = isinstance(articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg) + assert getattr(actuator, "is_implicit_model", False) == is_implicit_model_cfg + + num_dofs = articulation.num_joints + articulation.num_base_dofs + num_jacobian_bodies = articulation.num_bodies - 1 if articulation.is_fixed_base else articulation.num_bodies + + J = articulation.data.body_link_jacobian_w.torch + assert J.shape == torch.Size((num_articulations, num_jacobian_bodies, 6, num_dofs)), tuple(J.shape) + assert J.dtype == torch.float32 + + g = articulation.data.gravity_compensation_forces.torch + assert g.shape == torch.Size((num_articulations, num_dofs)), tuple(g.shape) + assert g.dtype == torch.float32 + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch + assert M.shape == torch.Size((num_articulations, num_dofs, num_dofs)), tuple(M.shape) + assert M.dtype == torch.float32 + diag = M.diagonal(dim1=-2, dim2=-1) + assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" + + # The joint-space inertia is symmetric by construction; asymmetry means a wrong-axis gather or a + # half-populated buffer. OSC inverts ``J M^-1 J^T`` every step, so ``M`` must also be positive-definite. + asym = (M - M.transpose(-1, -2)).abs().max().item() + assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric" + # A tiny jitter tolerates the float32 eigenvalue floor without masking real non-PD bugs. + eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) + torch.linalg.cholesky(M + 1e-6 * eye) + + # Read every accessor at the stepped joint state. + J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() + J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() + M_0 = articulation.data.mass_matrix.torch.clone() + g_0 = articulation.data.gravity_compensation_forces.torch.clone() # Non-trivial configuration via manual writes (no sim step, so the assert # compares both quantities at exactly this state): random joint offsets, @@ -4042,6 +2964,21 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( # agree trivially, voiding the newton#2625 rotated-root coverage. torch.testing.assert_close(articulation.data.root_link_pose_w.torch, root_pose, atol=1e-5, rtol=0.0) + # With the FK trigger, forward() refreshes body_q to the written state before each accessor evaluates. + # Without it, body_q stays at the previous state and every accessor returns its previous value. + assert not torch.allclose(J_link_0, articulation.data.body_link_jacobian_w.torch, atol=1e-3), ( + "body_link_jacobian_w did not change after manual joint write; FK trigger likely missing" + ) + assert not torch.allclose(J_com_0, articulation.data.body_com_jacobian_w.torch, atol=1e-3), ( + "body_com_jacobian_w did not change after manual joint write; FK trigger likely missing" + ) + assert not torch.allclose(M_0, articulation.data.mass_matrix.torch, atol=1e-3), ( + "mass_matrix did not change after manual joint write; FK trigger likely missing" + ) + assert not torch.allclose(g_0, articulation.data.gravity_compensation_forces.torch, atol=1e-3), ( + "gravity_compensation_forces did not change after manual joint write; FK trigger likely missing" + ) + g_meas = articulation.data.gravity_compensation_forces.torch # Independent derivation: generalized gravity load tau_g = sum_b J_lin_b^T (m_b g_w); @@ -4060,55 +2997,53 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( torch.testing.assert_close(g_meas, g_expected, atol=1e-2, rtol=1e-3) - -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) -@pytest.mark.isaacsim_ci -def test_dynamics_accessors_refresh_after_manual_joint_write(sim, num_articulations, device, articulation_type): - """After ``write_joint_position_to_sim_index`` (no sim step), every dynamics accessor must reflect the - new joint state, not the previous one. - - Catches a missing FK trigger before ``eval_jacobian`` / the COM->origin shift kernel - (:attr:`body_com_jacobian_w`, :attr:`body_link_jacobian_w`), before ``eval_mass_matrix`` - (``compute_body_spatial_inertia`` reads ``state.body_q``) and before the RNEA pass in - ``eval_inverse_dynamics_passive`` (:attr:`gravity_compensation_forces`). Gravity stays enabled - (the default): with gravity off ``g(q)`` is identically zero and its assertion would be vacuous. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - sim.step() + # Link-origin Jacobian contract. Reproducible non-trivial q_dot — large enough to drive omega + # well above the floor where COM offset effects would round into noise. The root is at rest so + # the actuated-only J slice predicts the whole body twist. + qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 + articulation.write_joint_velocity_to_sim_index(velocity=qdot) + if not articulation.is_fixed_base: + articulation.write_root_velocity_to_sim_index(root_velocity=torch.zeros(num_articulations, 6, device=device)) + # Refresh kinematics without integrating: for floating bases, an actuator step can + # introduce root motion that is intentionally absent from the actuated-only J slice. + sim.forward() articulation.update(sim.cfg.dt) - # Read every accessor at the baseline joint state. - q_baseline = articulation.data.joint_pos.torch.clone() - J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() - J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() - M_0 = articulation.data.mass_matrix.torch.clone() - g_0 = articulation.data.gravity_compensation_forces.torch.clone() + # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past + # them so the joint axis aligns with joint_vel (actuated-only). + J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :] + qdot_view = articulation.data.joint_vel.torch + v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) # (N, B_jac, 6) + v_pred_lin = v_pred[..., 0:3] + v_pred_ang = v_pred[..., 3:6] - # Manually write a different joint state (large delta so the change is visible) without a sim step - # or update, which marks FK stale (write_joint_position_to_sim sets _fk_timestamp = -1). - q_target = q_baseline + 0.5 - env_ids = wp.array([0], dtype=wp.int32, device=device) - articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + # Ground truth from Newton state. ``get_link_velocities`` returns shape + # (num_instances, 1, num_bodies, 6) — per-articulation grouping with + # one articulation per instance — so we squeeze the inner dim. + state = SimulationManager.get_state_0() + body_qd_view = wp.to_torch(articulation.root_view.get_link_velocities(state)).squeeze(1) + body_v_com = body_qd_view[..., :3] + body_omega = body_qd_view[..., 3:] - # With the FK trigger, forward() refreshes body_q to q_target before each accessor evaluates. - # Without it, body_q stays at the baseline and every accessor returns its previous value. - assert not torch.allclose(J_link_0, articulation.data.body_link_jacobian_w.torch, atol=1e-3), ( - "body_link_jacobian_w did not change after manual joint write; FK trigger likely missing" - ) - assert not torch.allclose(J_com_0, articulation.data.body_com_jacobian_w.torch, atol=1e-3), ( - "body_com_jacobian_w did not change after manual joint write; FK trigger likely missing" - ) - assert not torch.allclose(M_0, articulation.data.mass_matrix.torch, atol=1e-3), ( - "mass_matrix did not change after manual joint write; FK trigger likely missing" - ) - assert not torch.allclose(g_0, articulation.data.gravity_compensation_forces.torch, atol=1e-3), ( - "gravity_compensation_forces did not change after manual joint write; FK trigger likely missing" - ) + # World-frame COM-to-origin offset, derived from already-computed + # data layer outputs (avoids quaternion-convention pitfalls). + body_com_pos_w = articulation.data.body_com_pos_w.torch # (N, num_bodies, 3) + body_link_pos_w = articulation.data.body_link_pos_w.torch # (N, num_bodies, 3) + c_world = body_com_pos_w - body_link_pos_w + + if articulation.is_fixed_base: + body_v_com = body_v_com[:, 1:] + body_omega = body_omega[:, 1:] + c_world = c_world[:, 1:] + + # Expected v_origin = v_com - omega x c_world. + v_origin_expected = body_v_com - torch.cross(body_omega, c_world, dim=-1) + + # Tolerance: 5 mm absolute. The COM-offset bug produces a ~3 cm bias + # on the panda hand under the 0.5-rad/s injected qdot, well above + # this floor; numerical noise from kernel ordering stays under 1 mm. + torch.testing.assert_close(v_pred_ang, body_omega, atol=5e-3, rtol=1e-2) + torch.testing.assert_close(v_pred_lin, v_origin_expected, atol=5e-3, rtol=1e-2) @pytest.mark.parametrize("num_articulations", [1]) @@ -4197,83 +3132,6 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio ) -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci -def test_franka_ik_tracking_accuracy(sim, device, articulation_type, gravity_enabled): - """Newton-side IK convergence sentinel. - - Runs a full IK tracking loop end-to-end through the new - ``robot.data.body_link_jacobian_w`` accessor and records the steady-state EE - pose error. With the robot teleported to its configured init_state - home pose and scene gravity off, Newton's IK converges to - machine-precision tracking (sub-mm). A bridge regression - (wrong-reference-frame Jacobian, missing COM->origin shift, DoF - mis-ordering) would push the steady-state error well above the - threshold below. - - The pose teleport is deliberate: the standalone test path does not - invoke a manager-based env reset (which is what normally pushes - :attr:`~isaaclab.assets.ArticulationData.default_joint_pos` to sim). - Without it, the robot starts at the URDF-neutral pose where the - Franka wrist axes nearly align (rank-deficient Jacobian) and DLS - plateaus at multi-cm error -- a kinematic-singularity artifact, not - a bridge or Newton issue. - - See ``test_get_jacobians_link_origin_contract`` (above) for the - sharper unit-level pin on the Jacobian's reference-point contract. - """ - robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim) - - sim.step() - robot.update(sim.cfg.dt) - target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device) - - ik = DifferentialIKController( - DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), - num_envs=1, - device=device, - ) - ik.set_command(target_pose_b) - - pos_history: list[float] = [] - rot_history: list[float] = [] - for _ in range(800): - jacobian = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) - ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) - joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids] - - joint_pos_des = ik.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) - - robot.set_joint_position_target(joint_pos_des, joint_ids=arm_joint_ids) - robot.write_data_to_sim() - sim.step() - robot.update(sim.cfg.dt) - - pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) - pos_history.append(pos_error.norm(dim=-1).max().item()) - rot_history.append(rot_error.norm(dim=-1).max().item()) - - pos_min, pos_mean = _summarize_history(pos_history) - rot_min, rot_mean = _summarize_history(rot_history) - - # Print metrics every run for stress-test capture. - print(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") - - # Regression sentinel: assert on tail mean rather than min. Tail - # min is the bottom of any oscillation envelope and can be tiny - # while the actual tracking error is much larger. With the - # configured home pose and scene gravity off, Newton converges to - # machine precision (sub-mm). The 5 mm bound absorbs any CUDA- - # kernel-ordering noise while remaining well below the "totally - # broken" regime: a bridge regression (wrong-frame Jacobian, - # missing COM->origin shift, DoF mis-ordering) would push the - # steady-state error well past this bound. - assert pos_mean < 5e-3, f"IK pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" - assert rot_mean < 5e-2, f"IK rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" - - @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["panda"]) @pytest.mark.parametrize("gravity_enabled", [False]) @@ -4283,8 +3141,7 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en Mirror of the existing PhysX-side OSC tests in :mod:`isaaclab.test.controllers.test_operational_space`, scoped to - Franka pose-abs tracking on Newton. Like the IK sentinel above, this - test exercises the full controller-bridge pipeline + Franka pose-abs tracking on Newton. This test exercises the full controller-bridge pipeline (:attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`) end-to-end and asserts a loose regression bound rather than a tight correctness @@ -4346,15 +3203,13 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en pos_history.append(pos_error.norm(dim=-1).max().item()) rot_history.append(rot_error.norm(dim=-1).max().item()) - pos_min, pos_mean = _summarize_history(pos_history) - rot_min, rot_mean = _summarize_history(rot_history) - - print(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + pos_mean = _tail_mean(pos_history) + rot_mean = _tail_mean(rot_history) # Regression sentinel: assert on tail mean rather than min. With # ``current_ee_vel_b = J · q_dot`` providing OSC's damping term and # the actuator PD zeroed, the impedance settles to machine - # precision -- same ballpark as the IK test. The 5 mm bound is a + # precision. The 5 mm bound is a # bridge regression sentinel: a wrong J, wrong mass matrix, or # DoF mis-ordering pushes the steady-state error well past it # because OSC consumes both ``body_link_jacobian_w`` and @@ -4472,8 +3327,6 @@ def _stationary_tail_mean(history, label): pos_off = _stationary_tail_mean(hist_off, "phase-1 sag") pos_on = _stationary_tail_mean(hist_on, "phase-2 hold") - print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.6f}") - # Re-validated on newton 81cdcfc2 / mujoco-warp 3.10.0.2 with 4 substeps: # pos_off ~= 0.024, pos_on ~= 1e-6. assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" diff --git a/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py b/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py index 06d516c12137..fcb4e7759dee 100644 --- a/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py +++ b/source/isaaclab_newton/test/assets/test_articulation_ordering_kernels.py @@ -16,8 +16,7 @@ def _selector(values: list[int], dtype: type) -> wp.array: return wp.array(values, dtype=dtype, device="cpu") -@pytest.mark.parametrize("env_dtype", [wp.int32, wp.int64]) -@pytest.mark.parametrize("joint_dtype", [wp.int32, wp.int64]) +@pytest.mark.parametrize(("env_dtype", "joint_dtype"), [(wp.int32, wp.int32), (wp.int64, wp.int64)]) def test_write_joint_limit_data_to_user_and_backend_index_accepts_index_dtypes( env_dtype: type, joint_dtype: type ) -> None: diff --git a/source/isaaclab_newton/test/assets/test_joint_coordinates.py b/source/isaaclab_newton/test/assets/test_joint_coordinates.py index 216ca4b7c5ee..74d437408edf 100644 --- a/source/isaaclab_newton/test/assets/test_joint_coordinates.py +++ b/source/isaaclab_newton/test/assets/test_joint_coordinates.py @@ -33,16 +33,6 @@ def _map() -> BallJointCoordinateMap: return build_ball_joint_coordinate_map(COORD_COUNTS, DOF_COUNTS, "cpu") -def test_tables_cover_every_dof() -> None: - """Every DOF of every joint must be tabulated exactly once.""" - m = _map() - covered = list(m.single_dof.numpy()) + [b + k for b in m.ball_dof.numpy() for k in range(3)] - assert sorted(covered) == list(range(sum(DOF_COUNTS))) - assert sorted(list(m.single_coord.numpy()) + [b + k for b in m.ball_coord.numpy() for k in range(4)]) == list( - range(sum(COORD_COUNTS)) - ) - - def test_two_ball_tables_cover_every_dof() -> None: """A second ball joint's offsets are not a simple repeat of the first's.""" m = build_ball_joint_coordinate_map(TWO_BALL_COORD_COUNTS, TWO_BALL_DOF_COUNTS, "cpu") @@ -155,9 +145,9 @@ def test_unsupported_layout_is_rejected() -> None: build_ball_joint_coordinate_map([7], [6], "cpu") -@pytest.mark.parametrize("num_envs", [1, 2]) -def test_scatter_then_gather_round_trips(num_envs: int) -> None: - """DOF values survive a trip through coordinate space, at one environment and at two.""" +def test_scatter_then_gather_round_trips() -> None: + """DOF values survive a trip through coordinate space.""" + num_envs = 2 m = _map() n_dofs, n_coords = sum(DOF_COUNTS), sum(COORD_COUNTS) rng = np.random.default_rng(0) diff --git a/source/isaaclab_newton/test/assets/test_mpm_object.py b/source/isaaclab_newton/test/assets/test_mpm_object.py index bc54cd73ccfe..57d0551989ff 100644 --- a/source/isaaclab_newton/test/assets/test_mpm_object.py +++ b/source/isaaclab_newton/test/assets/test_mpm_object.py @@ -13,7 +13,7 @@ newton = pytest.importorskip("newton") -from isaaclab_newton.assets.mpm_object import MPMObjectCfg +from isaaclab_newton.assets.mpm_object import MPMObject, MPMObjectCfg from isaaclab_newton.physics import MPMSolverCfg, NewtonCfg, NewtonMPMManager from isaaclab_newton.sim.spawners.mpm import MPMGridCfg @@ -50,6 +50,7 @@ class MPMSceneCfg(InteractiveSceneCfg): sim.reset() media = scene["media"] + assert isinstance(media, MPMObject) assert media.num_instances == 2 assert media.particles_per_object == 1 assert media.data.particle_pos_w.torch.shape == (2, 1, 3) diff --git a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py index 264ce94b4498..f338a3425504 100644 --- a/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py +++ b/source/isaaclab_newton/test/assets/test_newton_actuators_newton.py @@ -34,7 +34,7 @@ from isaaclab_newton.physics import NewtonManager as SimulationManager import isaaclab.sim as sim_utils -from isaaclab.actuators import IdealPDActuatorCfg +from isaaclab.actuators import DCMotorCfg, DelayedPDActuatorCfg, IdealPDActuatorCfg from isaaclab.actuators.newton import read_group_parameter from isaaclab.actuators.newton.kernels import sync_torque_telemetry from isaaclab.assets import AssetBaseCfg @@ -42,7 +42,6 @@ from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.test.utils.actuator_equivalence import ( CARTPOLE_EXPLICIT_ACTUATORS, - DC_MOTOR_ACTUATORS, DELAYED_PD_ACTUATORS, IDEAL_PD_ACTUATORS, IMPLICIT_ONLY_ACTUATORS, @@ -82,6 +81,32 @@ use_cuda_graph=False, ) +# The PD demand (~kp * TARGET_OFFSET = 4 N·m) exceeds the 2 N·m saturation effort, so the DC-motor +# torque-speed clamp binds and Newton must author it for the paths to match. +SATURATING_DC_MOTOR_ACTUATORS = { + "legs": DCMotorCfg( + joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], + saturation_effort=2.0, + actuator_effort_limit=80.0, + actuator_velocity_limit=7.5, + stiffness=40.0, + damping=5.0, + ), +} + +# Newton authors ``max_delay`` as a fixed delay, while the Lab path samples a lag in +# ``[min_delay, max_delay]`` on reset; a fixed delay makes both paths apply the same lag. +FIXED_DELAYED_PD_ACTUATORS = { + "legs": DelayedPDActuatorCfg( + joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], + stiffness=40.0, + damping=5.0, + actuator_effort_limit=80.0, + min_delay=2, + max_delay=2, + ), +} + # --------------------------------------------------------------------------- # Simulation runner # --------------------------------------------------------------------------- @@ -98,6 +123,7 @@ def _run_simulation( feedforward: float | None = None, joint_ordering: tuple[str, ...] | None = None, permutation_sensitive_commands: bool = False, + ramp_targets: bool = False, ) -> dict: """Run ANYmal-C and return recorded trajectories + telemetry. @@ -119,10 +145,11 @@ def _run_simulation( joint_ordering: Optional explicit public joint-name order. permutation_sensitive_commands: Whether to command distinct position, velocity, and effort values by physical joint name. + ramp_targets: Whether to ramp the position target over the rollout so command delays are observable. Returns: - Recorded joint-name metadata, commands, public trajectories and torque telemetry, and backend-order - adapter effort traces. + Recorded joint-name metadata, commands, public trajectories and torque telemetry, backend-order + adapter effort traces, and the Newton actuators built from the model. """ sim_cfg = SimulationCfg(dt=dt, physics=newton_cfg, use_newton_actuators=use_newton_actuators) with build_simulation_context( @@ -149,6 +176,8 @@ def _run_simulation( replicate(sim.get_clone_plan()) sim.reset() assert articulation.is_initialized + # Reset samples the Lab-path actuator delay; without it the Lab lag stays at zero. + articulation.reset() if use_newton_actuators and decimation > 1: SimulationManager.set_decimation(decimation) @@ -199,7 +228,10 @@ def _run_simulation( recorded_pos, recorded_vel = [], [] recorded_computed_effort, recorded_applied_effort = [], [] recorded_adapter_applied = [] - for _ in range(num_steps): + for step in range(num_steps): + if ramp_targets: + target_pos = init_pos + TARGET_OFFSET * (step + 1) / num_steps + articulation.set_joint_position_target_index(target=target_pos) if handles_dec: articulation.write_data_to_sim() sim.step() @@ -216,6 +248,17 @@ def _run_simulation( if use_newton_actuators: recorded_adapter_applied.append(wp.to_torch(articulation.data._sim_bind_joint_effort).clone()) + actuator_info = [] + if use_newton_actuators: + for act in SimulationManager.get_model().actuators: + actuator_info.append( + { + "controller_type": type(act.controller).__name__, + "clamping_types": sorted(type(c).__name__ for c in (act.clamping or [])), + "has_delay": act.delay is not None, + } + ) + return { "joint_names": joint_names, "backend_joint_names": backend_joint_names, @@ -229,19 +272,24 @@ def _run_simulation( "target_pos": target_pos.clone(), "target_vel": target_vel.clone(), "effort_target": None if effort_target is None else effort_target.clone(), + "actuator_info": actuator_info, } def test_newton_actuator_rollout_matches_reversed_joint_ordering() -> None: - """Match Newton-backend actuator traces under reversed public joint ordering.""" + """Match Newton-backend actuator traces under reversed public joint ordering. + + The implicit hip group reads its torque telemetry from backend-order effort buffers, so the + permutation-sensitive feedforward effort also checks that gather. + """ identity_result = _run_simulation( - IDEAL_PD_ACTUATORS, + MIXED_WITH_IMPLICIT_ACTUATORS, use_newton_actuators=True, permutation_sensitive_commands=True, ) requested_joint_names = tuple(reversed(identity_result["joint_names"])) reversed_result = _run_simulation( - IDEAL_PD_ACTUATORS, + MIXED_WITH_IMPLICIT_ACTUATORS, use_newton_actuators=True, joint_ordering=requested_joint_names, permutation_sensitive_commands=True, @@ -274,6 +322,7 @@ class _EquivalenceTestBase(EquivalenceAssertionsMixin, unittest.TestCase): newton_cfg: NewtonCfg = NEWTON_CFG num_steps: int = NUM_STEPS decimation: int = 1 + ramp_targets: bool = False @classmethod def setUpClass(cls): @@ -283,6 +332,7 @@ def setUpClass(cls): newton_cfg=cls.newton_cfg, num_steps=cls.num_steps, decimation=cls.decimation, + ramp_targets=cls.ramp_targets, ) cls.lab_result = _run_simulation(cls.actuators, use_newton_actuators=False, **kwargs) cls.newton_result = _run_simulation(cls.actuators, use_newton_actuators=True, **kwargs) @@ -293,18 +343,18 @@ def setUpClass(cls): # --------------------------------------------------------------------------- -class TestIdealPDEquivalence(_EquivalenceTestBase): - """IdealPDActuator on all 12 joints: Lab vs Newton.""" - - __test__ = True - actuators = IDEAL_PD_ACTUATORS - - class TestDCMotorEquivalence(_EquivalenceTestBase): - """DCMotor actuator on all 12 joints: Lab vs Newton.""" + """Saturating DCMotor actuator on all 12 joints: Lab vs Newton.""" __test__ = True - actuators = DC_MOTOR_ACTUATORS + actuators = SATURATING_DC_MOTOR_ACTUATORS + + def test_dc_motor_clamp_binds(self): + for result in (self.lab_result, self.newton_result): + assert any( + not torch.allclose(applied, computed) + for applied, computed in zip(result["applied_effort"], result["computed_effort"]) + ), "the DC-motor clamp never bound, so the equivalence cannot detect missing clamping" class TestMixedWithImplicitEquivalence(_EquivalenceTestBase): @@ -457,72 +507,6 @@ class TestRandomizeActuatorGainsViaEventsNewton(unittest.TestCase): ``K`` — so the assertions are deterministic. """ - def test_single_articulation(self): - sim_cfg = SimulationCfg(dt=DT, physics=NEWTON_CFG, use_newton_actuators=True) - with build_simulation_context( - device="cuda:0", - gravity_enabled=True, - add_ground_plane=True, - sim_cfg=sim_cfg, - ) as sim: - sim._app_control_on_stop_handle = None - sim_utils.create_prim("/World/Env_0", "Xform") - art_cfg = ANYMAL_C_CFG.replace( - actuators=IDEAL_PD_ACTUATORS, - prim_path="/World/Env_[^/]*/Robot", - ) - clone_plan_from_env_0( - CloneCfg(clone_template="/World/Env_{}"), - (art_cfg, AssetBaseCfg(prim_path="/World/defaultGroundPlane")), - NUM_ENVS, - 3.0, - positions=np.asarray([(i * 3.0, 0.0, 0.0) for i in range(NUM_ENVS)]), - ) - anymal = Articulation(art_cfg) - replicate(sim.get_clone_plan()) - sim.reset() - - adapter = SimulationManager._adapter - self.assertIsNotNone(adapter, "Newton adapter should exist with use_newton_actuators=True") - read = functools.partial(read_group_parameter, anymal.actuators) - n = anymal.num_joints - # Before DR, native gain reads must return the configured values for - # *every* env. IDEAL_PD_ACTUATORS covers all 12 joints with constant - # gains, so every cell of both env rows must equal the configured - # value. This is also the regression check for the env-major DOF - # stride decoding on floating-base articulations (ANYmal-C has 6 - # free-root DOFs + 12 joints -> a per-env stride of 18 vs. - # ``num_joints == 12``): a wrong stride corrupts every env past the - # first. - legs_stiffness_before = read("legs", "controller", "kp").clone() - legs_damping_before = read("legs", "controller", "kd").clone() - torch.testing.assert_close(legs_stiffness_before, torch.full((NUM_ENVS, n), 40.0, device=anymal.device)) - torch.testing.assert_close(legs_damping_before, torch.full((NUM_ENVS, n), 5.0, device=anymal.device)) - - env = MockEnv({"robot": anymal}, NUM_ENVS, anymal.device) - term, asset_cfg = build_dr_term(env, "robot") - env_ids = torch.tensor([0], device=anymal.device, dtype=torch.long) - - term( - env, - env_ids=env_ids, - asset_cfg=asset_cfg, - stiffness_distribution_params=(100.0, 100.0), - damping_distribution_params=(5.0, 5.0), - operation="abs", - distribution="uniform", - ) - - # Named native-group reads project the controller values immediately. - torch.testing.assert_close( - read("legs", "controller", "kp")[0], torch.full((n,), 100.0, device=anymal.device) - ) - torch.testing.assert_close(read("legs", "controller", "kd")[0], torch.full((n,), 5.0, device=anymal.device)) - # Other envs untouched. - for env_idx in range(1, NUM_ENVS): - torch.testing.assert_close(read("legs", "controller", "kp")[env_idx], legs_stiffness_before[env_idx]) - torch.testing.assert_close(read("legs", "controller", "kd")[env_idx], legs_damping_before[env_idx]) - def test_two_articulations(self): from isaaclab_assets import CARTPOLE_CFG # noqa: PLC0415 @@ -560,6 +544,13 @@ def test_two_articulations(self): cartpole_read = functools.partial(read_group_parameter, cartpole.actuators) anymal_stiffness_before = anymal_read("legs", "controller", "kp").clone() anymal_damping_before = anymal_read("legs", "controller", "kd").clone() + # Before DR, native gain reads must return the configured values for *every* env. This is + # also the regression check for the env-major DOF stride decoding on floating-base + # articulations (ANYmal-C has 6 free-root DOFs + 12 joints): a wrong stride corrupts every + # env past the first. + n = anymal.num_joints + torch.testing.assert_close(anymal_stiffness_before, torch.full((NUM_ENVS, n), 40.0, device=anymal.device)) + torch.testing.assert_close(anymal_damping_before, torch.full((NUM_ENVS, n), 5.0, device=anymal.device)) cartpole_stiffness_before = cartpole_read("all_joints", "controller", "kp").clone() cartpole_damping_before = cartpole_read("all_joints", "controller", "kd").clone() @@ -598,6 +589,31 @@ def test_two_articulations(self): cartpole_read("all_joints", "controller", "kd")[env_idx], cartpole_damping_before[env_idx] ) + # DR scoped to the floating-base ANYmal updates only its selected env. + anymal_term, anymal_asset_cfg = build_dr_term(env, "anymal") + anymal_term( + env, + env_ids=env_ids, + asset_cfg=anymal_asset_cfg, + stiffness_distribution_params=(100.0, 100.0), + damping_distribution_params=(5.0, 5.0), + operation="abs", + distribution="uniform", + ) + torch.testing.assert_close( + anymal_read("legs", "controller", "kp")[0], torch.full((n,), 100.0, device=anymal.device) + ) + torch.testing.assert_close( + anymal_read("legs", "controller", "kd")[0], torch.full((n,), 5.0, device=anymal.device) + ) + for env_idx in range(1, NUM_ENVS): + torch.testing.assert_close( + anymal_read("legs", "controller", "kp")[env_idx], anymal_stiffness_before[env_idx] + ) + torch.testing.assert_close( + anymal_read("legs", "controller", "kd")[env_idx], anymal_damping_before[env_idx] + ) + # --------------------------------------------------------------------------- # DelayedPD equivalence: PD with actuator command delay @@ -608,11 +624,19 @@ class TestDelayedPDEquivalence(_EquivalenceTestBase): """DelayedPDActuator on all 12 joints: Lab vs Newton. Verifies that actuator command delays are correctly authored as - ``NewtonActuatorDelayAPI`` and produce matching trajectories. + ``NewtonActuatorDelayAPI`` and produce matching trajectories. The ramped + position target makes a missing delay change the trajectory. """ __test__ = True - actuators = DELAYED_PD_ACTUATORS + actuators = FIXED_DELAYED_PD_ACTUATORS + ramp_targets = True + + def test_newton_actuators_are_delayed_pd(self): + self.assertTrue(self.newton_result["actuator_info"], "No Newton actuators were created") + for a in self.newton_result["actuator_info"]: + self.assertTrue(a["has_delay"], "Delay not found on delayed PD actuator") + self.assertEqual(a["controller_type"], "DrivePD") # --------------------------------------------------------------------------- @@ -644,14 +668,6 @@ class _DecimationMixin: decimation = 2 -class TestDecimationDCMotor(_DecimationMixin, TestDCMotorEquivalence): - """DCMotor — same equivalence checks, with decimation=2 + CUDA graph.""" - - -class TestDecimationDelayedPD(_DecimationMixin, TestDelayedPDEquivalence): - """DelayedPD — decimation=2 + CUDA graph (delay queue stepped inside the captured graph).""" - - # --------------------------------------------------------------------------- # Per-env reset: actuator state isolation # --------------------------------------------------------------------------- @@ -705,126 +721,31 @@ def _remotized_pd_actuators() -> dict: stiffness=60.0, damping=1.5, actuator_effort_limit=80.0, + min_delay=3, max_delay=3, joint_parameter_lookup=SPOT_KNEE_LOOKUP, ), } -def _run_authoring_introspection(actuator_cfgs: dict) -> dict: - """Instantiate Newton simulation, return Newton actuator introspection. - - Verifies that Lab configs are correctly authored to Newton USD schemas - and that Newton creates the expected controller/clamping/delay objects. - - Returns: - Dict with ``num_actuators``, ``actuator_info`` (list of per-actuator - dicts), and ``joint_pos`` (recorded trajectories). - """ - sim_cfg = SimulationCfg(dt=DT, physics=NEWTON_CFG, use_newton_actuators=True) - - with build_simulation_context( - device="cuda:0", - gravity_enabled=True, - add_ground_plane=True, - sim_cfg=sim_cfg, - ) as sim: - sim._app_control_on_stop_handle = None - - sim_utils.create_prim("/World/Env_0", "Xform") - - art_cfg = ANYMAL_C_CFG.replace( - actuators=actuator_cfgs, - prim_path="/World/Env_[^/]*/Robot", - ) - clone_plan_from_env_0( - CloneCfg(clone_template="/World/Env_{}"), - (art_cfg, AssetBaseCfg(prim_path="/World/defaultGroundPlane")), - NUM_ENVS, - 3.0, - positions=np.asarray([(i * 3.0, 0.0, 0.0) for i in range(NUM_ENVS)]), - ) - articulation = Articulation(art_cfg) - replicate(sim.get_clone_plan()) - sim.reset() - assert articulation.is_initialized - - model = SimulationManager.get_model() - - actuator_info = [] - for act in model.actuators: - ctrl_type = type(act.controller).__name__ - clamp_types = sorted(type(c).__name__ for c in (act.clamping or [])) - actuator_info.append( - { - "controller_type": ctrl_type, - "clamping_types": clamp_types, - "has_delay": act.delay is not None, - "num_indices": len(act.indices), - } - ) - - init_pos = wp.to_torch(articulation.data.joint_pos).clone() - target_pos = init_pos + TARGET_OFFSET - target_vel = torch.zeros_like(init_pos) - articulation.set_joint_position_target_index(target=target_pos) - articulation.set_joint_velocity_target_index(target=target_vel) - - recorded_pos = [] - for _ in range(NUM_STEPS): - articulation.write_data_to_sim() - sim.step() - articulation.update(DT) - recorded_pos.append(wp.to_torch(articulation.data.joint_pos).clone()) - - return { - "num_actuators": len(model.actuators), - "actuator_info": actuator_info, - "joint_pos": recorded_pos, - } - - -class TestRemotizedPDAuthoring(unittest.TestCase): - """Verify RemotizedPDActuatorCfg is authored as Newton PD + delay + - position-based clamping. - - Uses the Spot knee lookup table on ANYmal's KFE joints, with IdealPD - on HAA and HFE joints. - """ - - @classmethod - def setUpClass(cls): - cls.result = _run_authoring_introspection(_remotized_pd_actuators()) - - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_kfe_controller_is_pd(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - self.assertTrue(len(kfe_acts) > 0, "No actuator with position-based clamping found") - for a in kfe_acts: - self.assertEqual(a["controller_type"], "DrivePD") - - def test_kfe_has_position_based_clamping(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - self.assertTrue(len(kfe_acts) > 0, "Position-based clamping not found") - - def test_kfe_has_delay(self): - kfe_acts = [a for a in self.result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] - for a in kfe_acts: - self.assertTrue(a["has_delay"], "Delay not found on remotized KFE actuator") - - class TestRemotizedPDEquivalence(_EquivalenceTestBase): """RemotizedPD (PD + delay + position-based clamping): Lab vs Newton.""" __test__ = True + ramp_targets = True @classmethod def setUpClass(cls): cls.actuators = _remotized_pd_actuators() super().setUpClass() + def test_newton_knee_actuators_are_delayed_position_clamped_pd(self): + kfe_acts = [a for a in self.newton_result["actuator_info"] if "ClampingPositionBased" in a["clamping_types"]] + self.assertTrue(len(kfe_acts) > 0, "No actuator with position-based clamping found") + for a in kfe_acts: + self.assertEqual(a["controller_type"], "DrivePD") + self.assertTrue(a["has_delay"], "Delay not found on remotized KFE actuator") + class TestDecimationRemotizedPD(_DecimationMixin, TestRemotizedPDEquivalence): """RemotizedPD — decimation=2 + CUDA graph.""" @@ -835,17 +756,18 @@ class TestDecimationRemotizedPD(_DecimationMixin, TestRemotizedPDEquivalence): # --------------------------------------------------------------------------- -class TestNeuralMLPAuthoring(unittest.TestCase): - """Verify ActuatorNetMLPCfg is authored as Newton NeuralMLP controller - with DC motor clamping. +class TestNeuralActuatorAuthoring(unittest.TestCase): + """Verify ActuatorNetMLPCfg and ActuatorNetLSTMCfg are authored as Newton neural + controllers with DC motor clamping and run on the Newton backend. """ @classmethod def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetMLPCfg # noqa: PLC0415 + from isaaclab.actuators.actuator_net_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg # noqa: PLC0415 cls.mlp_path = make_dummy_mlp_checkpoint() - cls.result = _run_authoring_introspection( + cls.lstm_path = make_dummy_lstm_checkpoint() + cls.result = _run_simulation( { "mlp_legs": ActuatorNetMLPCfg( joint_names_expr=[".*HAA"], @@ -859,124 +781,43 @@ def setUpClass(cls): input_order="pos_vel", input_idx=[0, 1, 2], ), - "pd_legs": IdealPDActuatorCfg( - joint_names_expr=[".*HFE", ".*KFE"], - stiffness=40.0, - damping=5.0, - actuator_effort_limit=80.0, - ), - } - ) - - @classmethod - def tearDownClass(cls): - os.unlink(cls.mlp_path) - - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_has_neural_mlp_controller(self): - mlp_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralMLP"] - self.assertTrue(len(mlp_acts) > 0, "No NeuralMLP controller found") - - def test_mlp_has_dc_motor_clamping(self): - mlp_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralMLP"] - for a in mlp_acts: - self.assertIn("ClampingDCMotor", a["clamping_types"]) - - -class TestNeuralLSTMAuthoring(unittest.TestCase): - """Verify ActuatorNetLSTMCfg is authored as Newton NeuralLSTM controller - with DC motor clamping. - """ - - @classmethod - def setUpClass(cls): - from isaaclab.actuators.actuator_net_cfg import ActuatorNetLSTMCfg # noqa: PLC0415 - - cls.lstm_path = make_dummy_lstm_checkpoint() - cls.result = _run_authoring_introspection( - { "lstm_legs": ActuatorNetLSTMCfg( - joint_names_expr=[".*HAA"], + joint_names_expr=[".*HFE"], network_file=cls.lstm_path, saturation_effort=120.0, actuator_effort_limit=80.0, actuator_velocity_limit=7.5, ), "pd_legs": IdealPDActuatorCfg( - joint_names_expr=[".*HFE", ".*KFE"], + joint_names_expr=[".*KFE"], stiffness=40.0, damping=5.0, actuator_effort_limit=80.0, ), - } + }, + use_newton_actuators=True, ) @classmethod def tearDownClass(cls): + os.unlink(cls.mlp_path) os.unlink(cls.lstm_path) - def test_num_actuators(self): - self.assertGreaterEqual(self.result["num_actuators"], 2) - - def test_has_neural_lstm_controller(self): - lstm_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralLSTM"] - self.assertTrue(len(lstm_acts) > 0, "No NeuralLSTM controller found") + def test_mlp_has_dc_motor_clamping(self): + mlp_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralMLP"] + self.assertTrue(len(mlp_acts) > 0, "No NeuralMLP controller found") + for a in mlp_acts: + self.assertIn("ClampingDCMotor", a["clamping_types"]) def test_lstm_has_dc_motor_clamping(self): lstm_acts = [a for a in self.result["actuator_info"] if a["controller_type"] == "DriveNeuralLSTM"] + self.assertTrue(len(lstm_acts) > 0, "No NeuralLSTM controller found") for a in lstm_acts: self.assertIn("ClampingDCMotor", a["clamping_types"]) - -def test_sync_torque_telemetry_reads_backend_effort_buffers_in_user_order() -> None: - """Report torque telemetry in public joint order from backend-order effort buffers.""" - joint_pos = wp.zeros((1, 3), dtype=wp.float32, device="cpu") - joint_vel = wp.zeros_like(joint_pos) - joint_pos_target = wp.zeros_like(joint_pos) - joint_vel_target = wp.zeros_like(joint_pos) - joint_stiffness = wp.zeros_like(joint_pos) - joint_damping = wp.zeros_like(joint_pos) - effort_limit = wp.full((1, 3), 1000.0, dtype=wp.float32, device="cpu") - joint_modes = wp.array(np.asarray([0, 1, 0], dtype=np.int32), dtype=wp.int32, device="cpu") - user_to_backend = wp.array(np.asarray([2, 0, 1], dtype=np.int32), dtype=wp.int32, device="cpu") - sim_bind_joint_effort = wp.array( - np.asarray([[100.0, 200.0, 300.0]], dtype=np.float32), - dtype=wp.float32, - device="cpu", - ) - actuator_computed_effort = wp.array( - np.asarray([[10.0, 20.0, 30.0]], dtype=np.float32), - dtype=wp.float32, - device="cpu", - ) - computed = wp.zeros_like(joint_pos) - applied = wp.zeros_like(joint_pos) - - wp.launch( - sync_torque_telemetry, - dim=joint_pos.shape, - inputs=[ - joint_pos, - joint_vel, - joint_pos_target, - joint_vel_target, - joint_stiffness, - joint_damping, - effort_limit, - joint_modes, - sim_bind_joint_effort, - actuator_computed_effort, - user_to_backend, - True, - ], - outputs=[computed, applied], - device="cpu", - ) - - np.testing.assert_allclose(computed.numpy(), np.asarray([[30.0, 100.0, 20.0]], dtype=np.float32)) - np.testing.assert_allclose(applied.numpy(), np.asarray([[300.0, 100.0, 200.0]], dtype=np.float32)) + def test_positions_finite(self): + for step_i, pos in enumerate(self.result["joint_pos"]): + self.assertTrue(torch.isfinite(pos).all(), f"Non-finite positions at step {step_i}") def test_sync_torque_telemetry_keeps_user_order_effort_buffers_unmapped() -> None: diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 01a4de488607..4bd2d9aacad9 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -10,7 +10,7 @@ """Launch Isaac Sim Simulator first.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices +from isaaclab.test.utils import DeviceScope, resolve_test_sim_device, test_devices # launch omniverse app simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app @@ -18,6 +18,7 @@ """Rest everything follows.""" import sys +from types import SimpleNamespace from typing import Literal import numpy as np @@ -34,11 +35,12 @@ import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg, RigidObjectCfg from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate +from isaaclab.envs.mdp.events import randomize_rigid_body_material +from isaaclab.managers import EventTermCfg, SceneEntityCfg from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab.utils.math import ( combine_frame_transforms, - default_orientation, quat_apply_inverse, quat_inv, quat_mul, @@ -127,41 +129,6 @@ def generate_cubes_scene( return cube_object, torch.as_tensor(origins, device=device) -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [1, 2]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization(num_cubes, device): - """Test initialization for prim with rigid body API at the provided prim path.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Check that the framework doesn't hold excessive strong references. - assert sys.getrefcount(cube_object) < 10 - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Check if object is initialized - assert cube_object.is_initialized - assert len(cube_object.body_names) == 1 - - # Check buffers that exists and have correct shapes - assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) - assert cube_object.data.root_quat_w.torch.shape == (num_cubes, 4) - assert cube_object.data.body_mass.torch.shape == (num_cubes, 1) - assert cube_object.data.body_inertia.torch.shape == (num_cubes, 1, 9) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("api", ["none", "articulation_root"]) def test_initialization_rejects_non_rigid_body_prims(api): @@ -174,121 +141,78 @@ def test_initialization_rejects_non_rigid_body_prims(api): assert sys.getrefcount(cube_object) < 10 replicate(sim.get_clone_plan()) - with pytest.raises(RuntimeError): + with pytest.raises(RuntimeError, match="Expected 1 prims at"): sim.reset() -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(device): - """Test if external force buffer correctly updates in the force value is zero case. - - In this test, we apply a non-zero force, then a zero force, then finally a non-zero force - to an object. We check if the force buffer is properly updated at each step. - """ - - # Generate cubes scene - with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=1, device=device, add_ground_plane=True) - - # play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # find bodies to apply the force - body_ids, body_names = cube_object.find_bodies(".*") - - # reset object - cube_object.reset() - - # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) - - if step == 0 or step == 3: - # set a non-zero force - force = 1 - else: - # set a zero force - force = 0 - - # set force value - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - # apply force - cube_object.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - - # check if the cube's force and torque buffers are correctly updated - for i in range(cube_object.num_instances): - assert cube_object._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force - assert cube_object._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force - - # Check if the instantaneous wrench is correctly added to the permanent wrench - cube_object.permanent_wrench_composer.add_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=body_ids, - ) - - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [4]) @pytest.mark.parametrize("device", test_devices()) def test_external_force_on_single_body(num_cubes, device): - """Test application of external force on the base of the object. + """Test initialization and external forces on the base of the object. - In this test, we apply a force equal to the weight of an object on the base of + In the first phase, we apply a force equal to the weight of an object on the base of one of the objects. We check that the object does not move. For the other object, - we do not apply any force and check that it falls down. + we do not apply any force and check that it falls down. In the second phase, the force + is applied at 1m in the Y direction and the object must rotate around its X axis. - We validate that this works when we apply the force in the global frame and in the local frame. + We validate that this works when we apply the force in the global frame and in the local frame, + and that resetting the object clears the wrench applied in the previous iteration. """ # Generate cubes scene with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device, add_ground_plane=True) + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(cube_object) < 10 + # Play the simulator replicate(sim.get_clone_plan()) sim.reset() + # Check if object is initialized + assert cube_object.is_initialized + assert len(cube_object.body_names) == 1 + + # Check buffers that exists and have correct shapes + assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) + assert cube_object.data.root_quat_w.torch.shape == (num_cubes, 4) + assert cube_object.data.body_mass.torch.shape == (num_cubes, 1) + assert cube_object.data.body_inertia.torch.shape == (num_cubes, 1, 9) + # Find bodies to apply the force body_ids, body_names = cube_object.find_bodies(".*") + def reset_cubes(): + # reset root state; shift the cubes to their origins so they are not on top of each other + root_pose = cube_object.data.default_root_pose.torch.clone() + root_pose[:, :3] = origins + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + cube_object.write_root_velocity_to_sim_index(root_velocity=cube_object.data.default_root_vel.torch.clone()) + cube_object.reset() + + # Reset should zero external forces and torques + assert not cube_object.instantaneous_wrench_composer.active + assert not cube_object.permanent_wrench_composer.active + assert torch.count_nonzero(cube_object.instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object.instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(cube_object.permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object.permanent_wrench_composer.out_torque_b.torch) == 0 + + def simulate(): + for _ in range(5): + cube_object.write_data_to_sim() + sim.step() + cube_object.update(sim.cfg.dt) + # Sample a force equal to the weight of the object external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) # Every 2nd cube should have a force applied to it external_wrench_b[0::2, :, 2] = 9.81 * cube_object.data.body_mass.torch[0] - # Now we are ready! - for i in range(5): - # reset root state - root_pose = cube_object.data.default_root_pose.torch.clone() - root_vel = cube_object.data.default_root_vel.torch.clone() - - # need to shift the position of the cubes otherwise they will be on top of each other - root_pose[:, :3] = origins - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # reset object - cube_object.reset() + for i in range(2): + reset_cubes() is_global = False if i % 2 == 0: @@ -305,16 +229,7 @@ def test_external_force_on_single_body(num_cubes, device): body_ids=body_ids, is_global=is_global, ) - # perform simulation - for _ in range(5): - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) + simulate() # First object should still be at the same Z position (1.0) torch.testing.assert_close( @@ -323,50 +238,14 @@ def test_external_force_on_single_body(num_cubes, device): # Second object should have fallen, so it's Z height should be less than initial height of 1.0 assert torch.all(cube_object.data.root_pos_w.torch[1::2, 2] < 1.0) - -@pytest.mark.parametrize("num_cubes", [4]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(num_cubes, device): - """Test application of external force on the base of the object at a specific position. - - In this test, we apply a force equal to the weight of an object on the base of - one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. - For the other object, we do not apply any force and check that it falls down. - - We validate that this works when we apply the force in the global frame and in the local frame. - """ - # Generate cubes scene - with _newton_sim_context(device, add_ground_plane=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device, add_ground_plane=True) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - # Find bodies to apply the force - body_ids, body_names = cube_object.find_bodies(".*") - - # Sample a force equal to the weight of the object + # Apply a force at 1m in the Y direction on every 2nd cube external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) external_wrench_positions_b = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) - # Every 2nd cube should have a force applied to it external_wrench_b[0::2, :, 2] = 50.0 external_wrench_positions_b[0::2, :, 1] = 1.0 - # Now we are ready! - for i in range(5): - # reset root state - root_pose = cube_object.data.default_root_pose.torch.clone() - root_vel = cube_object.data.default_root_vel.torch.clone() - - # need to shift the position of the cubes otherwise they will be on top of each other - root_pose[:, :3] = origins - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - # reset object - cube_object.reset() + for i in range(2): + reset_cubes() is_global = False if i % 2 == 0: @@ -396,16 +275,7 @@ def test_external_force_on_single_body_at_position(num_cubes, device): body_ids=body_ids, is_global=is_global, ) - # perform simulation - for _ in range(5): - # apply action to the object - cube_object.write_data_to_sim() - - # perform step - sim.step() - - # update buffers - cube_object.update(sim.cfg.dt) + simulate() # The first object should be rotating around it's X axis assert torch.all(torch.abs(cube_object.data.root_ang_vel_b.torch[0::2, 0]) > 0.1) @@ -416,114 +286,54 @@ def test_external_force_on_single_body_at_position(num_cubes, device): @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -def test_set_rigid_object_state(num_cubes, device): - """Test setting the state of the rigid object. - - In this test, we set the state of the rigid object to a random state and check - that the object is in that state after simulation. We set gravity to zero as - we don't want any external forces acting on the object to ensure state remains static. - """ - # Turn off gravity for this test as we don't want any external forces acting on the object - # to ensure state remains static - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: +def test_rigid_body_set_material_properties(num_cubes, device): + """Material randomization writes friction and restitution into the Newton model shapes of the selected envs.""" + with _newton_sim_context(device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator - replicate(sim.get_clone_plan()) - sim.reset() - - state_types = ["root_pos_w", "root_quat_w", "root_lin_vel_w", "root_ang_vel_w"] - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in state_types: - state_dict = { - "root_pos_w": torch.zeros_like(cube_object.data.root_pos_w.torch, device=sim.device), - "root_quat_w": default_orientation(num=num_cubes, device=sim.device), - "root_lin_vel_w": torch.zeros_like(cube_object.data.root_lin_vel_w.torch, device=sim.device), - "root_ang_vel_w": torch.zeros_like(cube_object.data.root_ang_vel_w.torch, device=sim.device), - } + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device, add_ground_plane=True) - # Now we are ready! - for _ in range(5): - # reset object - cube_object.reset() - - # Set random state - if state_type_to_randomize == "root_quat_w": - state_dict[state_type_to_randomize] = random_orientation(num=num_cubes, device=sim.device) - else: - state_dict[state_type_to_randomize] = torch.randn(num_cubes, 3, device=sim.device) - - # perform simulation - for _ in range(5): - root_pose = torch.cat( - [state_dict["root_pos_w"], state_dict["root_quat_w"]], - dim=-1, - ) - root_vel = torch.cat( - [state_dict["root_lin_vel_w"], state_dict["root_ang_vel_w"]], - dim=-1, - ) - # reset root state - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - - sim.step() - - # assert that set root quantities are equal to the ones set in the state_dict - for key, expected_value in state_dict.items(): - value = getattr(cube_object.data, key).torch - # Newton reads state directly from sim (not cached), so post-step drift - # from velocity integration causes larger differences than PhysX - torch.testing.assert_close(value, expected_value, rtol=1e-1, atol=1e-1) - - cube_object.update(sim.cfg.dt) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset_rigid_object(num_cubes, device): - """Test resetting the state of the rigid object.""" - with _newton_sim_context(device, gravity_enabled=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Generate cubes scene - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Play the simulator + # Play sim replicate(sim.get_clone_plan()) sim.reset() - for i in range(5): - # perform rendering - sim.step() - - # update object - cube_object.update(sim.cfg.dt) - - # Move the object to a random position - root_pose = cube_object.data.default_root_pose.torch.clone() - root_pose[:, :3] = torch.randn(num_cubes, 3, device=sim.device) - - # Random orientation - root_pose[:, 3:7] = random_orientation(num=num_cubes, device=sim.device) - cube_object.write_root_pose_to_sim_index(root_pose=root_pose) - root_vel = cube_object.data.default_root_vel.torch.clone() - cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) + # Resolve each cube's shapes from the flat Newton model, independent of the asset's view binding. + model = SimulationManager.get_model() + body_world = model.body_world.numpy() + shape_body = model.shape_body.numpy() + cube_shapes = [ + np.flatnonzero(np.isin(shape_body, np.flatnonzero(body_world == index))) for index in range(num_cubes) + ] + assert all(len(shapes) > 0 for shapes in cube_shapes) + original_mu = model.shape_material_mu.numpy().copy() + original_restitution = model.shape_material_restitution.numpy().copy() + + # Randomize the materials of the last cube through the event term, with degenerate ranges. + env = SimpleNamespace(scene={"cube": cube_object}, sim=sim, device=device, num_envs=num_cubes) + params = { + "static_friction_range": (0.55, 0.55), + "dynamic_friction_range": (0.55, 0.55), + "restitution_range": (0.15, 0.15), + "num_buckets": 1, + "asset_cfg": SceneEntityCfg("cube"), + } + term = randomize_rigid_body_material( + EventTermCfg(func=randomize_rigid_body_material, mode="startup", params=params), env + ) + term(env, torch.tensor([num_cubes - 1], device=device), **params) - if i % 2 == 0: - # reset object - cube_object.reset() + # Simulate physics + sim.step() + cube_object.update(sim.cfg.dt) - # Reset should zero external forces and torques - assert not cube_object._instantaneous_wrench_composer.active - assert not cube_object._permanent_wrench_composer.active - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_torque_b.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_torque_b.torch) == 0 + mu = model.shape_material_mu.numpy() + restitution = model.shape_material_restitution.numpy() + np.testing.assert_allclose(mu[cube_shapes[-1]], 0.55) + np.testing.assert_allclose(restitution[cube_shapes[-1]], 0.15) + # Shapes of the other cubes are untouched. + for shapes in cube_shapes[:-1]: + np.testing.assert_array_equal(mu[shapes], original_mu[shapes]) + np.testing.assert_array_equal(restitution[shapes], original_restitution[shapes]) @pytest.mark.isaacsim_ci @@ -607,48 +417,9 @@ def test_rigid_body_set_mass(num_cubes, device): torch.testing.assert_close(masses, masses_to_check) -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [True, False]) -def test_gravity_vec_w(num_cubes, device, gravity_enabled): - """Test that gravity vector direction is set correctly for the rigid object.""" - with _newton_sim_context(device, gravity_enabled=gravity_enabled) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) - - # Obtain gravity direction - if gravity_enabled: - expected_g = (0.0, 0.0, -9.81) - else: - expected_g = (0.0, 0.0, 0.0) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Check that gravity is set correctly - torch.testing.assert_close(cube_object.data.GRAVITY_VEC_W.torch[0], torch.tensor(expected_g, device=device)) - - # Simulate physics - for _ in range(2): - # perform rendering - sim.step() - # update object - cube_object.update(sim.cfg.dt) - - # Expected gravity value is the acceleration of the body - gravity = torch.zeros(num_cubes, 1, 6, device=device) - if gravity_enabled: - gravity[:, :, 2] = -9.81 - # Check the body accelerations are correct - torch.testing.assert_close(cube_object.data.body_acc_w.torch, gravity) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [3]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_gravity_vec_w_tracks_model_gravity(num_cubes, device): """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. @@ -662,6 +433,19 @@ def test_gravity_vec_w_tracks_model_gravity(num_cubes, device): replicate(sim.get_clone_plan()) sim.reset() + # Check that gravity is set correctly + torch.testing.assert_close( + cube_object.data.GRAVITY_VEC_W.torch[0], torch.tensor((0.0, 0.0, -9.81), device=device) + ) + + # The free-falling cubes accelerate with gravity and keep their identity orientation. + for _ in range(2): + sim.step() + cube_object.update(sim.cfg.dt) + gravity = torch.zeros(num_cubes, 1, 6, device=device) + gravity[:, :, 2] = -9.81 + torch.testing.assert_close(cube_object.data.body_acc_w.torch, gravity) + # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. model = SimulationManager.get_model() model_gravity_arr = model.gravity[: model.world_count] @@ -692,9 +476,8 @@ def test_gravity_vec_w_tracks_model_gravity(num_cubes, device): @pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) @flaky(max_runs=3, min_passes=1) -def test_body_root_state_properties(num_cubes, device, with_offset): +def test_body_root_state_properties(num_cubes, device): """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -709,10 +492,7 @@ def test_body_root_state_properties(num_cubes, device, with_offset): assert cube_object.is_initialized # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) # Set center of mass offset via Newton API (position only, no quaternion) com_pos = offset.unsqueeze(1) # (N, 1, 3) @@ -746,68 +526,61 @@ def test_body_root_state_properties(num_cubes, device, with_offset): body_com_pose_w = cube_object.data.body_com_pose_w.torch body_com_vel_w = cube_object.data.body_com_vel_w.torch - # if offset is [0,0,0] all root_state_%_w will match and all body_%_w will match - if not with_offset: - torch.testing.assert_close(root_link_pose_w, root_com_pose_w) - torch.testing.assert_close(root_com_vel_w, root_link_vel_w) - torch.testing.assert_close(root_link_pose_w, body_link_pose_w.squeeze(-2)) - torch.testing.assert_close(root_com_vel_w, root_link_vel_w) - torch.testing.assert_close(body_link_pose_w, body_com_pose_w) - torch.testing.assert_close(body_com_vel_w, body_link_vel_w) - torch.testing.assert_close(root_com_pose_w, body_com_pose_w.squeeze(-2)) - torch.testing.assert_close(body_com_vel_w, body_link_vel_w) - else: - # cubes are spinning around center of mass - # position will not match - # center of mass position will be constant (i.e. spinning around com) - _tol = dict(atol=2e-3, rtol=2e-3) - torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3], **_tol) - torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2), **_tol) - # link position will be moving but should stay constant away from center of mass - root_link_state_pos_rel_com = quat_apply_inverse( - root_link_pose_w[..., 3:], - root_link_pose_w[..., :3] - root_com_pose_w[..., :3], - ) - torch.testing.assert_close(-offset, root_link_state_pos_rel_com, **_tol) - body_link_state_pos_rel_com = quat_apply_inverse( - body_link_pose_w[..., 3:], - body_link_pose_w[..., :3] - body_com_pose_w[..., :3], - ) - torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2), **_tol) - - # orientation of com will be a constant rotation from link orientation - com_quat_b = cube_object.data.body_com_quat_b.torch - com_quat_w = quat_mul(body_link_pose_w[..., 3:], com_quat_b) - torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:], **_tol) - torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:], **_tol) - - # root and body link orientations describe the same rigid body - torch.testing.assert_close(root_link_pose_w[..., 3:], body_link_pose_w[..., 3:].squeeze(-2), **_tol) - - # lin_vel will not match - # center of mass vel will be constant (i.e. spinning around com) - torch.testing.assert_close(torch.zeros_like(root_com_vel_w[..., :3]), root_com_vel_w[..., :3], **_tol) - torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3], **_tol) - # link frame will be moving, and should be equal to input angular velocity cross offset - lin_vel_rel_root_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_link_vel_w[..., :3]) - lin_vel_rel_body_gt = quat_apply_inverse(body_link_pose_w[..., 3:], body_link_vel_w[..., :3]) - lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_cubes, 1)[..., 3:], -offset) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, **_tol) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), **_tol) - - # ang_vel will always match - torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) - torch.testing.assert_close(root_com_vel_w[..., 3:], body_com_vel_w[..., 3:].squeeze(-2)) - torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + _tol = dict(atol=2e-3, rtol=2e-3) + torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3], **_tol) + torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2), **_tol) + # link position will be moving but should stay constant away from center of mass + root_link_state_pos_rel_com = quat_apply_inverse( + root_link_pose_w[..., 3:], + root_link_pose_w[..., :3] - root_com_pose_w[..., :3], + ) + torch.testing.assert_close(-offset, root_link_state_pos_rel_com, **_tol) + body_link_state_pos_rel_com = quat_apply_inverse( + body_link_pose_w[..., 3:], + body_link_pose_w[..., :3] - body_com_pose_w[..., :3], + ) + torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2), **_tol) + + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.body_com_quat_b.torch + com_quat_w = quat_mul(body_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:], **_tol) + torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:], **_tol) + + # root and body link orientations describe the same rigid body + torch.testing.assert_close(root_link_pose_w[..., 3:], body_link_pose_w[..., 3:].squeeze(-2), **_tol) + + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close(torch.zeros_like(root_com_vel_w[..., :3]), root_com_vel_w[..., :3], **_tol) + torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3], **_tol) + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_root_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_link_vel_w[..., :3]) + lin_vel_rel_body_gt = quat_apply_inverse(body_link_pose_w[..., 3:], body_link_vel_w[..., :3]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, **_tol) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), **_tol) + + # ang_vel will always match + torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) + torch.testing.assert_close(root_com_vel_w[..., 3:], body_com_vel_w[..., 3:].squeeze(-2)) + torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) @pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) +@pytest.mark.parametrize( + ("num_cubes", "state_location"), [(2, "com"), (2, "link"), (2, "root"), (1, "root")] +) # num_cubes=1 covers single-instance initialization @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -def test_write_root_state(num_cubes, device, with_offset, state_location): - """Test the setters for root_state using both the link frame and center of mass as reference frame.""" +def test_write_root_state(num_cubes, device, state_location): + """Test the root state setters in the center-of-mass frame, the link frame, and the default root frames. + + A write must be readable in the written frame and refresh the derived frame and the body-frame caches + without a sim step, and the written state must persist into the solver across a step. + """ with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Create a scene with random cubes @@ -822,10 +595,7 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): assert cube_object.is_initialized # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) # Set center of mass offset via Newton API (position only) com_pos = offset.unsqueeze(1) # (N, 1, 3) @@ -836,34 +606,38 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): # check center of mass has been set torch.testing.assert_close(cube_object.data.body_com_pos_b.torch.squeeze(1), offset) - rand_state = torch.zeros(num_cubes, 13, device=device) - rand_state[..., :7] = cube_object.data.default_root_pose.torch - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - for i in range(10): + for i in range(2): # perform step sim.step() # update buffers cube_object.update(sim.cfg.dt) + # A target state distinct from the current one in position, orientation, and velocity. + target_pose = torch.cat( + [env_pos + 0.3 * torch.rand(num_cubes, 3, device=device), random_orientation(num_cubes, device)], + dim=-1, + ) + target_vel = torch.randn(num_cubes, 6, device=device) + + # Prime the lazily-derived caches at the current sim timestamp. Without this they would + # recompute on first access after the write regardless of invalidation; priming them makes a + # missing reset_pose/reset_velocity observable as a stale read in the assertions below. + _ = cube_object.data.root_link_pose_w.torch + _ = cube_object.data.root_com_pose_w.torch + _ = cube_object.data.root_link_vel_w.torch + _ = cube_object.data.root_com_vel_w.torch + + # Alternate between the default and explicit environment selectors. + env_ids = None if i == 0 else env_idx if state_location == "com": - if i % 2 == 0: - cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) + cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + cube_object.write_root_com_velocity_to_sim_index(root_velocity=target_vel, env_ids=env_ids) elif state_location == "link": - if i % 2 == 0: - cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - else: - cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) - cube_object.write_root_link_velocity_to_sim_index( - root_velocity=rand_state[..., 7:], env_ids=env_idx - ) + cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + cube_object.write_root_link_velocity_to_sim_index(root_velocity=target_vel, env_ids=env_ids) + elif state_location == "root": + cube_object.write_root_pose_to_sim_index(root_pose=target_pose, env_ids=env_ids) + cube_object.write_root_velocity_to_sim_index(root_velocity=target_vel, env_ids=env_ids) # Snapshot the body-frame caches *before* reading the root-frame caches: touching a # root cache lazily recomputes the shared buffer and would mask a stale body cache. @@ -871,150 +645,64 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): # body_com_pose_w returned the pre-write buffer after a link-frame pose write). body_link_pose_w = cube_object.data.body_link_pose_w.torch.squeeze(1).clone() body_com_pose_w = cube_object.data.body_com_pose_w.torch.squeeze(1).clone() - body_link_vel_w = cube_object.data.body_link_vel_w.torch.squeeze(1).clone() body_com_vel_w = cube_object.data.body_com_vel_w.torch.squeeze(1).clone() + root_link_pose_w = cube_object.data.root_link_pose_w.torch + root_com_pose_w = cube_object.data.root_com_pose_w.torch + root_link_vel_w = cube_object.data.root_link_vel_w.torch + root_com_vel_w = cube_object.data.root_com_vel_w.torch + body_com_pose_b = cube_object.data.body_com_pose_b.torch if state_location == "com": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.root_com_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.root_com_vel_w.torch) - elif state_location == "link": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.root_link_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.root_link_vel_w.torch) + torch.testing.assert_close(target_pose, root_com_pose_w) + torch.testing.assert_close(target_vel, root_com_vel_w) + # the com pose was written, so the derived link pose must be refreshed + expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( + root_com_pose_w[:, :3], + root_com_pose_w[:, 3:], + quat_rotate(quat_inv(body_com_pose_b[:, 0, 3:7]), -body_com_pose_b[:, 0, :3]), + quat_inv(body_com_pose_b[:, 0, 3:7]), + ) + expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) + torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) + else: + torch.testing.assert_close(target_pose, root_link_pose_w) + # the root velocity is the center-of-mass velocity + written_vel_w = root_link_vel_w if state_location == "link" else root_com_vel_w + torch.testing.assert_close(target_vel, written_vel_w) + # the link pose was written, so the derived com pose must be refreshed + expected_com_pos, expected_com_quat = combine_frame_transforms( + root_link_pose_w[:, :3], + root_link_pose_w[:, 3:], + body_com_pose_b[:, 0, :3], + body_com_pose_b[:, 0, 3:7], + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) + torch.testing.assert_close(expected_com_pose, root_com_pose_w) + # skip lin_vel because it differs between the frames; angular velocity is frame-independent + # and only matches when the derived velocity was actually refreshed after the write + torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) # For a single-body rigid object the body-frame caches are exactly the root-frame # caches reshaped, so they must stay consistent after a write without a sim step. - torch.testing.assert_close(cube_object.data.root_link_pose_w.torch, body_link_pose_w) - torch.testing.assert_close(cube_object.data.root_com_pose_w.torch, body_com_pose_w) - torch.testing.assert_close(cube_object.data.root_link_vel_w.torch, body_link_vel_w) - torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, body_com_vel_w) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) -@pytest.mark.parametrize("state_location", ["com", "link", "root"]) -def test_write_state_functions_data_consistency(num_cubes, device, with_offset, state_location): - """Test the setters for root_state using both the link frame and center of mass as reference frame.""" - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - - # Play sim - replicate(sim.get_clone_plan()) - sim.reset() - - # Check if cube_object is initialized - assert cube_object.is_initialized - - # change center of mass offset from link frame - if with_offset: - offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) - else: - offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) - - # Set center of mass offset via Newton API (position only) - com_pos = offset.unsqueeze(1) # (N, 1, 3) - cube_object.set_coms_index(coms=wp.from_torch(com_pos, dtype=wp.vec3f)) - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) - - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pos_b.torch.squeeze(1), offset) - - rand_state = torch.rand(num_cubes, 13, device=device) - rand_state[..., :3] += env_pos - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - # perform step + torch.testing.assert_close(root_link_pose_w, body_link_pose_w) + torch.testing.assert_close(root_com_pose_w, body_com_pose_w) + torch.testing.assert_close(root_link_vel_w, cube_object.data.body_link_vel_w.torch.squeeze(1)) + torch.testing.assert_close(root_com_vel_w, body_com_vel_w) + + # The written state persists into the solver: with gravity off, one step only integrates the + # written velocity. + written_pose_w = (root_com_pose_w if state_location == "com" else root_link_pose_w).clone() + written_com_vel_w = root_com_vel_w.clone() sim.step() - # update buffers cube_object.update(sim.cfg.dt) - - # Prime the lazily-derived caches at the current sim timestamp. Without this they would - # recompute on first access after the write regardless of invalidation; priming them makes a - # missing reset_pose/reset_velocity observable as a stale read in the assertions below. - _ = cube_object.data.root_link_pose_w.torch - _ = cube_object.data.root_com_pose_w.torch - _ = cube_object.data.root_link_vel_w.torch - _ = cube_object.data.root_com_vel_w.torch - - if state_location == "com": - cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "link": - cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - elif state_location == "root": - cube_object.write_root_pose_to_sim_index(root_pose=rand_state[..., :7]) - cube_object.write_root_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) - - if state_location == "com": - root_com_pose_w = cube_object.data.root_com_pose_w.torch - root_com_vel_w = cube_object.data.root_com_vel_w.torch - body_com_pose_b = cube_object.data.body_com_pose_b.torch - expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( - root_com_pose_w[:, :3], - root_com_pose_w[:, 3:], - quat_rotate(quat_inv(body_com_pose_b[:, 0, 3:7]), -body_com_pose_b[:, 0, :3]), - quat_inv(body_com_pose_b[:, 0, 3:7]), - ) - expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - # test both root_pose and root_link successfully updated when root_com updates - torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) - torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) - torch.testing.assert_close(root_com_vel_w[:, 3:], cube_object.data.root_com_vel_w.torch[:, 3:]) - elif state_location == "link": - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - body_com_pose_b = cube_object.data.body_com_pose_b.torch - expected_com_pos, expected_com_quat = combine_frame_transforms( - root_link_pose_w[:, :3], - root_link_pose_w[:, 3:], - body_com_pose_b[:, 0, :3], - body_com_pose_b[:, 0, 3:7], - ) - expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) - root_com_pose_w = cube_object.data.root_com_pose_w.torch - root_com_vel_w = cube_object.data.root_com_vel_w.torch - # test both root_pose and root_com successfully updated when root_link updates - torch.testing.assert_close(expected_com_pose, root_com_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(root_link_vel_w[:, 3:], root_com_vel_w[:, 3:]) - torch.testing.assert_close(root_link_pose_w, cube_object.data.root_link_pose_w.torch) - torch.testing.assert_close(root_link_vel_w[:, 3:], cube_object.data.root_com_vel_w.torch[:, 3:]) - elif state_location == "root": - root_link_pose_w = cube_object.data.root_link_pose_w.torch - root_com_vel_w = cube_object.data.root_com_vel_w.torch - body_com_pose_b = cube_object.data.body_com_pose_b.torch - expected_com_pos, expected_com_quat = combine_frame_transforms( - root_link_pose_w[:, :3], - root_link_pose_w[:, 3:], - body_com_pose_b[:, 0, :3], - body_com_pose_b[:, 0, 3:7], - ) - expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) - root_com_pose_w = cube_object.data.root_com_pose_w.torch - root_link_vel_w = cube_object.data.root_link_vel_w.torch - # test both root_com and root_link successfully updated when root_pose updates - torch.testing.assert_close(expected_com_pose, root_com_pose_w) - torch.testing.assert_close(root_com_vel_w, cube_object.data.root_com_vel_w.torch) - torch.testing.assert_close(root_link_pose_w, cube_object.data.root_link_pose_w.torch) - torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) + pose_w = cube_object.data.root_com_pose_w if state_location == "com" else cube_object.data.root_link_pose_w + torch.testing.assert_close(pose_w.torch, written_pose_w, rtol=1e-1, atol=1e-1) + torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, written_com_vel_w, rtol=1e-1, atol=1e-1) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"]) @pytest.mark.isaacsim_ci -def test_body_link_pose_w_fresh_after_root_pose_write(device, writer): +def test_body_link_pose_w_fresh_after_root_pose_write(device): """Regression: ``body_link_pose_w`` must reflect a freshly written root pose without an intervening sim step. After ``write_root_{link,com}_pose_to_sim_{index,mask}``, the cached ``_sim_bind_body_link_pose_w`` @@ -1042,45 +730,46 @@ def _fk_reset_mask_dirty() -> bool: sim.step() cube_object.update(sim.cfg.dt) - # Prime the body_link_pose_w cache with the current pose. - pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone().view(num_cubes, 7) - - # Clear the dirty flag so we can observe that the write sets it. - SimulationManager.forward() - assert not _fk_reset_mask_dirty() - - # Build a target pose clearly distinct from the current one in both translation and orientation. - # Quaternion in (x, y, z, w) for 90° about z: [0, 0, sin(pi/4), cos(pi/4)] = [0, 0, sqrt(0.5), sqrt(0.5)]. - target_pose = wp.to_torch(cube_object.data.root_link_pose_w).clone() - target_pose[..., 0] += 10.0 - target_pose[..., 1] += 5.0 - target_pose[..., 2] += 2.0 - sqrt_half = 0.7071067811865476 - target_pose[..., 3] = 0.0 - target_pose[..., 4] = 0.0 - target_pose[..., 5] = sqrt_half - target_pose[..., 6] = sqrt_half - - if writer == "link_index": - cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose) - elif writer == "link_mask": - cube_object.write_root_link_pose_to_sim_mask(root_pose=target_pose) - elif writer == "com_index": - cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose) - elif writer == "com_mask": - cube_object.write_root_com_pose_to_sim_mask(root_pose=target_pose) - - # The simulator-side dirty flag must be set before any property read clears it via forward(). - assert _fk_reset_mask_dirty(), "pose write must call SimulationManager.invalidate_fk()" - - # Read without stepping: getter must trigger forward kinematics and return the fresh pose. - body_link = wp.to_torch(cube_object.data.body_link_pose_w).view(num_cubes, 7) - # Defeat alias accidents: the property must not still return the pre-write value. - assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( - "body_link_pose_w returned the pre-write cached pose; forward() was not invoked" - ) - # Translation must match the write. - torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) - # Orientation: compare via |q1 · q2| ≈ 1 to account for the q ≡ -q double cover. - quat_dot = torch.abs((body_link[..., 3:7] * target_pose[..., 3:7]).sum(dim=-1)) - torch.testing.assert_close(quat_dot, torch.ones_like(quat_dot), rtol=1e-4, atol=1e-4) + for writer in ("link_index", "link_mask", "com_index", "com_mask"): + # Prime the body_link_pose_w cache with the current pose. + pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone().view(num_cubes, 7) + + # Clear the dirty flag so we can observe that the write sets it. + SimulationManager.forward() + assert not _fk_reset_mask_dirty() + + # Build a target pose clearly distinct from the current one in both translation and orientation. + # Quaternion in (x, y, z, w) for 90° about z: [0, 0, sin(pi/4), cos(pi/4)] = [0, 0, sqrt(0.5), sqrt(0.5)]. + target_pose = wp.to_torch(cube_object.data.root_link_pose_w).clone() + target_pose[..., 0] += 10.0 + target_pose[..., 1] += 5.0 + target_pose[..., 2] += 2.0 + sqrt_half = 0.7071067811865476 + target_pose[..., 3] = 0.0 + target_pose[..., 4] = 0.0 + target_pose[..., 5] = sqrt_half + target_pose[..., 6] = sqrt_half + + if writer == "link_index": + cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose) + elif writer == "link_mask": + cube_object.write_root_link_pose_to_sim_mask(root_pose=target_pose) + elif writer == "com_index": + cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose) + elif writer == "com_mask": + cube_object.write_root_com_pose_to_sim_mask(root_pose=target_pose) + + # The simulator-side dirty flag must be set before any property read clears it via forward(). + assert _fk_reset_mask_dirty(), f"{writer} pose write must call SimulationManager.invalidate_fk()" + + # Read without stepping: getter must trigger forward kinematics and return the fresh pose. + body_link = wp.to_torch(cube_object.data.body_link_pose_w).view(num_cubes, 7) + # Defeat alias accidents: the property must not still return the pre-write value. + assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( + f"body_link_pose_w returned the pre-write cached pose after {writer}; forward() was not invoked" + ) + # Translation must match the write. + torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) + # Orientation: compare via |q1 · q2| ≈ 1 to account for the q ≡ -q double cover. + quat_dot = torch.abs((body_link[..., 3:7] * target_pose[..., 3:7]).sum(dim=-1)) + torch.testing.assert_close(quat_dot, torch.ones_like(quat_dot), rtol=1e-4, atol=1e-4) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py index 183aae9276f6..51c08a2a0a48 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -10,7 +10,7 @@ """Launch Isaac Sim Simulator first.""" from isaaclab.app import AppLauncher -from isaaclab.test.utils import resolve_test_sim_device, test_devices +from isaaclab.test.utils import DeviceScope, resolve_test_sim_device, test_devices # launch omniverse app simulation_app = AppLauncher(headless=True, device=resolve_test_sim_device()).app @@ -36,7 +36,6 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.math import ( combine_frame_transforms, - default_orientation, quat_apply_inverse, quat_inv, quat_mul, @@ -131,36 +130,23 @@ def generate_cubes_scene( return cube_object_collection, torch.as_tensor(origins, device=device) +@pytest.mark.parametrize(("num_envs", "num_cubes", "spawn_unrelated_sibling"), [(1, 1, False), (2, 3, True)]) @pytest.mark.parametrize("device", test_devices()) -def test_initialization_ignores_unrelated_sibling_rigid_objects(device): - """Test that a collection view selects only its configured rigid objects.""" - num_envs = 2 - num_cubes = 3 +def test_initialization(num_envs, num_cubes, spawn_unrelated_sibling, device): + """Test initialization for prim with rigid body API at the provided prim path. + + With an unrelated rigid body next to the cubes in each environment, the collection view must still + select only its configured rigid objects. + """ with _newton_sim_context(device, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, _ = generate_cubes_scene( num_envs=num_envs, num_cubes=num_cubes, device=device, - spawn_unrelated_sibling=True, + spawn_unrelated_sibling=spawn_unrelated_sibling, ) - replicate(sim.get_clone_plan()) - sim.reset() - - assert object_collection.num_instances == num_envs - assert object_collection.root_view.count == num_envs * num_cubes - assert object_collection.data.default_body_pose.torch.shape == (num_envs, num_cubes, 7) - - -@pytest.mark.parametrize(("num_envs", "num_cubes"), [(1, 1), (2, 3)]) -@pytest.mark.parametrize("device", test_devices()) -def test_initialization(num_envs, num_cubes, device): - """Test initialization for prim with rigid body API at the provided prim path.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - # Check that the framework doesn't hold excessive strong references. assert sys.getrefcount(object_collection) < 10 @@ -170,19 +156,17 @@ def test_initialization(num_envs, num_cubes, device): # Check if object is initialized assert object_collection.is_initialized + assert object_collection.num_instances == num_envs + assert object_collection.root_view.count == num_envs * num_cubes assert len(object_collection.body_names) == num_cubes # Check buffers that exist and have correct shapes + assert object_collection.data.default_body_pose.torch.shape == (num_envs, num_cubes, 7) assert object_collection.data.body_link_pos_w.torch.shape == (num_envs, num_cubes, 3) assert object_collection.data.body_link_quat_w.torch.shape == (num_envs, num_cubes, 4) assert object_collection.data.body_mass.torch.shape == (num_envs, num_cubes) assert object_collection.data.body_inertia.torch.shape == (num_envs, num_cubes, 9) - # Simulate physics - for _ in range(2): - sim.step() - object_collection.update(sim.cfg.dt) - @pytest.mark.parametrize("device", test_devices()) def test_set_body_inertial_properties_updates_inverses(device): @@ -268,70 +252,21 @@ def test_initialization_with_no_rigid_body(): # Play sim replicate(sim.get_clone_plan()) - with pytest.raises(RuntimeError): + with pytest.raises(RuntimeError, match="Expected 1 prims at"): sim.reset() -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_buffer(device): - """Test if external force buffer correctly updates in the force value is zero case.""" - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - num_envs = 2 - num_cubes = 1 - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - - # find objects to apply the force - object_ids, object_names = object_collection.find_bodies(".*") - # reset object - object_collection.reset() - - # perform simulation - for step in range(5): - # initiate force tensor - external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) - - # decide if zero or non-zero force - if step == 0 or step == 3: - force = 1.0 - else: - force = 0.0 - - # apply force to the object - external_wrench_b[:, :, 0] = force - external_wrench_b[:, :, 3] = force - - object_collection.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - body_ids=object_ids, - env_ids=None, - ) - - # check if the object collection's force and torque buffers are correctly updated - for i in range(num_envs): - assert object_collection._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force - assert object_collection._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force - - object_collection.instantaneous_wrench_composer.add_forces_and_torques_index( - body_ids=object_ids, - forces=external_wrench_b[..., :3], - torques=external_wrench_b[..., 3:], - ) - - # apply action to the object collection - object_collection.write_data_to_sim() - sim.step() - object_collection.update(sim.cfg.dt) - - @pytest.mark.parametrize("num_envs", [2]) @pytest.mark.parametrize("num_cubes", [4]) @pytest.mark.parametrize("device", test_devices()) def test_external_force_on_single_body(num_envs, num_cubes, device): - """Test application of external force on the base of the object.""" + """Test application of external force on the base of the object. + + In the first phase, a force equal to the weight of every 2nd object keeps it in place while the others + fall. In the second phase, the force is applied at 1m in the Y direction and the object must rotate + around its X axis. Both phases run in the global and the local frame, and resetting the collection + must clear the wrench applied in the previous iteration. + """ with _newton_sim_context(device, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) @@ -341,22 +276,35 @@ def test_external_force_on_single_body(num_envs, num_cubes, device): # find objects to apply the force object_ids, object_names = object_collection.find_bodies(".*") - # Sample a force equal to the weight of the object - external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) - # Every 2nd cube should have a force applied to it - external_wrench_b[:, 0::2, 2] = 9.81 * object_collection.data.body_mass.torch[:, 0::2] - - for i in range(5): - # reset object state + def reset_objects(): + # reset object state; shift the cubes to their origins so they are not on top of each other body_pose = object_collection.data.default_body_pose.torch.clone() body_vel = object_collection.data.default_body_vel.torch.clone() - # need to shift the position of the cubes otherwise they will be on top of each other body_pose[..., :2] += origins.unsqueeze(1)[..., :2] object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - # reset object object_collection.reset() + # Reset should zero external forces and torques + assert torch.count_nonzero(object_collection.instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection.instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(object_collection.permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection.permanent_wrench_composer.out_torque_b.torch) == 0 + + def simulate(): + for _ in range(10): + object_collection.write_data_to_sim() + sim.step() + object_collection.update(sim.cfg.dt) + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[:, 0::2, 2] = 9.81 * object_collection.data.body_mass.torch[:, 0::2] + + for i in range(2): + reset_objects() + is_global = False if i % 2 == 0: positions = object_collection.data.body_link_pos_w.torch[:, object_ids, :3] @@ -373,13 +321,7 @@ def test_external_force_on_single_body(num_envs, num_cubes, device): env_ids=None, is_global=is_global, ) - for _ in range(10): - # write data to sim - object_collection.write_data_to_sim() - # step sim - sim.step() - # update object collection - object_collection.update(sim.cfg.dt) + simulate() # First object should still be at the same Z position (1.0) torch.testing.assert_close( @@ -389,46 +331,16 @@ def test_external_force_on_single_body(num_envs, num_cubes, device): # Second object should have fallen, so it's Z height should be less than initial height of 1.0 assert torch.all(object_collection.data.body_link_pos_w.torch[:, 1::2, 2] < 1.0) - -@pytest.mark.parametrize("num_envs", [2]) -@pytest.mark.parametrize("num_cubes", [4]) -@pytest.mark.parametrize("device", test_devices()) -def test_external_force_on_single_body_at_position(num_envs, num_cubes, device): - """Test application of external force on the base of the object at a specific position. - - In this test, we apply a force equal to the weight of an object on the base of - one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. - For the other object, we do not apply any force and check that it falls down. - """ - with _newton_sim_context(device, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - - # find objects to apply the force - object_ids, object_names = object_collection.find_bodies(".*") - - # Sample a force equal to the weight of the object + # Apply a force at 1m in the Y direction on every 2nd cube external_wrench_b = torch.zeros(object_collection.num_instances, len(object_ids), 6, device=sim.device) external_wrench_positions_b = torch.zeros( object_collection.num_instances, len(object_ids), 3, device=sim.device ) - # Every 2nd cube should have a force applied to it external_wrench_b[:, 0::2, 2] = 50.0 external_wrench_positions_b[:, 0::2, 1] = 1.0 - # Desired force and torque - for i in range(5): - # reset object state - body_pose = object_collection.data.default_body_pose.torch.clone() - body_vel = object_collection.data.default_body_vel.torch.clone() - # need to shift the position of the cubes otherwise they will be on top of each other - body_pose[..., :2] += origins.unsqueeze(1)[..., :2] - object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) - object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - # reset object - object_collection.reset() + for i in range(2): + reset_objects() is_global = False if i % 2 == 0: @@ -459,14 +371,7 @@ def test_external_force_on_single_body_at_position(num_envs, num_cubes, device): body_ids=object_ids, is_global=is_global, ) - - for _ in range(10): - # write data to sim - object_collection.write_data_to_sim() - # step sim - sim.step() - # update object collection - object_collection.update(sim.cfg.dt) + simulate() # First object should be rotating around it's X axis assert torch.all(object_collection.data.body_com_ang_vel_b.torch[:, 0::2, 0] > 0.1) @@ -474,168 +379,35 @@ def test_external_force_on_single_body_at_position(num_envs, num_cubes, device): assert torch.all(object_collection.data.body_link_pos_w.torch[:, 1::2, 2] < 1.0) +@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_envs", [3]) @pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_set_object_state(num_envs, num_cubes, device): - """Test setting the state of the object. +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +def test_gravity_vec_w_tracks_model_gravity(num_envs, num_cubes, device): + """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - .. note:: - Turn off gravity for this test as we don't want any external forces acting on the object - to ensure state remains static + Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's + gravity broadcast to every env and body, hiding per-env gravity + randomization (e.g. :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). """ - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, origins = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() - - state_types = ["body_link_pos_w", "body_link_quat_w", "body_com_lin_vel_w", "body_com_ang_vel_w"] - - # Set each state type individually as they are dependent on each other - for state_type_to_randomize in state_types: - state_dict = { - "body_link_pos_w": torch.zeros_like(object_collection.data.body_link_pos_w.torch, device=sim.device), - "body_link_quat_w": default_orientation(num=num_cubes * num_envs, device=sim.device).view( - num_envs, num_cubes, 4 - ), - "body_com_lin_vel_w": torch.zeros_like( - object_collection.data.body_com_lin_vel_w.torch, device=sim.device - ), - "body_com_ang_vel_w": torch.zeros_like( - object_collection.data.body_com_ang_vel_w.torch, device=sim.device - ), - } - - for _ in range(5): - # reset object - object_collection.reset() - - # Set random state - if state_type_to_randomize == "body_link_quat_w": - state_dict[state_type_to_randomize] = random_orientation( - num=num_cubes * num_envs, device=sim.device - ).view(num_envs, num_cubes, 4) - else: - state_dict[state_type_to_randomize] = torch.randn(num_envs, num_cubes, 3, device=sim.device) - # make sure objects do not overlap - if state_type_to_randomize == "body_link_pos_w": - state_dict[state_type_to_randomize][..., :2] += origins.unsqueeze(1)[..., :2] - - # perform simulation - for _ in range(5): - body_pose = torch.cat( - [state_dict["body_link_pos_w"], state_dict["body_link_quat_w"]], - dim=-1, - ) - body_vel = torch.cat( - [state_dict["body_com_lin_vel_w"], state_dict["body_com_ang_vel_w"]], - dim=-1, - ) - # reset object state - object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) - object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - sim.step() - - # assert that set object quantities are equal to the ones set in the state_dict - for key, expected_value in state_dict.items(): - value = getattr(object_collection.data, key).torch - # Newton reads state directly from sim (not cached), so post-step drift - # from velocity integration causes larger differences than PhysX - torch.testing.assert_close(value, expected_value, rtol=1e-1, atol=1e-1) - - object_collection.update(sim.cfg.dt) - - -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -def test_reset_object_collection(num_envs, num_cubes, device): - """Test resetting the state of the rigid object.""" with _newton_sim_context(device, gravity_enabled=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) replicate(sim.get_clone_plan()) sim.reset() - for i in range(5): - sim.step() - object_collection.update(sim.cfg.dt) - - # Move the object to a random position - body_pose = object_collection.data.default_body_pose.torch.clone() - body_pose[..., :3] = torch.randn(num_envs, num_cubes, 3, device=sim.device) - # Random orientation - body_pose[..., 3:7] = random_orientation(num=num_cubes, device=sim.device) - object_collection.write_body_link_pose_to_sim_index(body_poses=body_pose) - body_vel = object_collection.data.default_body_vel.torch.clone() - object_collection.write_body_com_velocity_to_sim_index(body_velocities=body_vel) - - if i % 2 == 0: - object_collection.reset() - - # Reset should zero external forces and torques - assert not object_collection._instantaneous_wrench_composer.active - assert not object_collection._permanent_wrench_composer.active - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_torque_b.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_force_b.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_torque_b.torch) == 0 - - -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("gravity_enabled", [True, False]) -def test_gravity_vec_w(num_envs, num_cubes, device, gravity_enabled): - """Test that gravity vector direction is set correctly for the rigid object.""" - with _newton_sim_context(device, gravity_enabled=gravity_enabled, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - - # GRAVITY_VEC_W now binds to Newton's per-env gravity array directly, - # so it carries full m/s^2 values and is shaped per-instance (not - # per-instance-per-body). - expected_g = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) - - replicate(sim.get_clone_plan()) - sim.reset() - # Check if gravity vector is set correctly torch.testing.assert_close( - object_collection.data.GRAVITY_VEC_W.torch[0], torch.tensor(expected_g, device=device) + object_collection.data.GRAVITY_VEC_W.torch[0], torch.tensor((0.0, 0.0, -9.81), device=device) ) - # Perform simulation + # The free-falling cubes accelerate with gravity and keep their identity orientation. for _ in range(2): sim.step() object_collection.update(sim.cfg.dt) - - # Expected gravity value is the acceleration of the body - gravity = torch.zeros(num_envs, num_cubes, 6, device=device) - if gravity_enabled: - gravity[..., 2] = -9.81 - - # Check the body accelerations are correct - torch.testing.assert_close(object_collection.data.body_com_acc_w.torch, gravity) - - -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_gravity_vec_w_tracks_model_gravity(num_envs, num_cubes, device): - """Per-env mutations to Newton's ``model.gravity`` reach ``GRAVITY_VEC_W`` and ``projected_gravity_b``. - - Regression for the pre-fix snapshot: ``GRAVITY_VEC_W`` used to be env 0's - gravity broadcast to every env and body, hiding per-env gravity - randomization (e.g. :class:`~isaaclab.envs.mdp.randomize_physics_scene_gravity`). - """ - with _newton_sim_context(device, gravity_enabled=True, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) - replicate(sim.get_clone_plan()) - sim.reset() + gravity = torch.zeros(num_envs, num_cubes, 6, device=device) + gravity[..., 2] = -9.81 + torch.testing.assert_close(object_collection.data.body_com_acc_w.torch, gravity) # GRAVITY_VEC_W must share storage with Newton's per-env gravity array. model = SimulationManager.get_model() @@ -665,8 +437,7 @@ def test_gravity_vec_w_tracks_model_gravity(num_envs, num_cubes, device): @pytest.mark.parametrize("num_envs", [4]) @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -def test_object_state_properties(num_envs, num_cubes, device, with_offset): +def test_object_state_properties(num_envs, num_cubes, device): """Test the object_com_state_w and object_link_state_w properties.""" with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -679,11 +450,7 @@ def test_object_state_properties(num_envs, num_cubes, device, with_offset): assert cube_object.is_initialized # change center of mass offset from link frame - offset = ( - torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - if with_offset - else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - ) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) # Set center of mass offset via Newton API (position only, shape (E, B, 3)) cube_object.set_coms_index(coms=wp.from_torch(offset, dtype=wp.vec3f)) @@ -713,60 +480,58 @@ def test_object_state_properties(num_envs, num_cubes, device, with_offset): object_com_pose_w = cube_object.data.body_com_pose_w.torch object_com_vel_w = cube_object.data.body_com_vel_w.torch - # if offset is [0,0,0] all object_state_%_w will match and all body_%_w will match - if not with_offset: - torch.testing.assert_close(object_link_pose_w, object_com_pose_w) - torch.testing.assert_close(object_com_vel_w, object_link_vel_w) - else: - _tol = dict(atol=2e-3, rtol=2e-3) - # cubes are spinning around center of mass - # position will not match - # center of mass position will be constant (i.e. spinning around com) - torch.testing.assert_close(init_com, object_com_pose_w[..., :3], **_tol) - - # link position will be moving but should stay constant away from center of mass - object_link_state_pos_rel_com = quat_apply_inverse( - object_link_pose_w[..., 3:], - object_link_pose_w[..., :3] - object_com_pose_w[..., :3], - ) + _tol = dict(atol=2e-3, rtol=2e-3) + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + torch.testing.assert_close(init_com, object_com_pose_w[..., :3], **_tol) - torch.testing.assert_close(-offset, object_link_state_pos_rel_com, **_tol) + # link position will be moving but should stay constant away from center of mass + object_link_state_pos_rel_com = quat_apply_inverse( + object_link_pose_w[..., 3:], + object_link_pose_w[..., :3] - object_com_pose_w[..., :3], + ) - # orientation of com will be a constant rotation from link orientation - com_quat_b = cube_object.data.body_com_quat_b.torch - com_quat_w = quat_mul(object_link_pose_w[..., 3:], com_quat_b) - torch.testing.assert_close(com_quat_w, object_com_pose_w[..., 3:], **_tol) + torch.testing.assert_close(-offset, object_link_state_pos_rel_com, **_tol) - # lin_vel will not match - # center of mass vel will be constant (i.e. spinning around com) - torch.testing.assert_close( - torch.zeros_like(object_com_vel_w[..., :3]), - object_com_vel_w[..., :3], - **_tol, - ) + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.body_com_quat_b.torch + com_quat_w = quat_mul(object_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, object_com_pose_w[..., 3:], **_tol) - # link frame will be moving, and should be equal to input angular velocity cross offset - lin_vel_rel_object_gt = quat_apply_inverse(object_link_pose_w[..., 3:], object_link_vel_w[..., :3]) - lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_envs, num_cubes, 1)[..., 3:], -offset) - torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_object_gt, **_tol) + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close( + torch.zeros_like(object_com_vel_w[..., :3]), + object_com_vel_w[..., :3], + **_tol, + ) - # ang_vel will always match - torch.testing.assert_close(object_com_vel_w[..., 3:], object_link_vel_w[..., 3:]) + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_object_gt = quat_apply_inverse(object_link_pose_w[..., 3:], object_link_vel_w[..., :3]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_envs, num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_object_gt, **_tol) + + # ang_vel will always match + torch.testing.assert_close(object_com_vel_w[..., 3:], object_link_vel_w[..., 3:]) @pytest.mark.parametrize("num_envs", [3]) @pytest.mark.parametrize("num_cubes", [2]) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.parametrize("state_location", ["com", "link"]) -def test_write_object_state(num_envs, num_cubes, device, with_offset, state_location): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" +@pytest.mark.parametrize("state_location", ["com", "link", "root"]) +def test_write_object_state(num_envs, num_cubes, device, state_location): + """Test the object state setters in the center-of-mass frame, the link frame, and the default root frames. + + A write must be readable in the written frame and refresh the derived frame without a sim step, and the + written state must persist into the solver across a step. + """ with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Create a scene with random cubes cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) - env_ids = torch.tensor([x for x in range(num_envs)], dtype=torch.int32) - object_ids = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) + env_ids = torch.tensor([x for x in range(num_envs)], dtype=torch.int32, device=device) + object_ids = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32, device=device) replicate(sim.get_clone_plan()) sim.reset() @@ -775,11 +540,7 @@ def test_write_object_state(num_envs, num_cubes, device, with_offset, state_loca assert cube_object.is_initialized # change center of mass offset from link frame - offset = ( - torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - if with_offset - else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - ) + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) # Set center of mass offset via Newton API (position only, shape (E, B, 3)) cube_object.set_coms_index(coms=wp.from_torch(offset, dtype=wp.vec3f)) @@ -790,193 +551,88 @@ def test_write_object_state(num_envs, num_cubes, device, with_offset, state_loca # check center of mass has been set torch.testing.assert_close(cube_object.data.body_com_pos_b.torch, offset) - rand_state = torch.zeros(num_envs, num_cubes, 13, device=device) - rand_state[..., :7] = cube_object.data.default_body_pose.torch - rand_state[..., :3] += cube_object.data.body_link_pos_w.torch - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_ids = env_ids.to(device) - object_ids = object_ids.to(device) - for i in range(10): + for i in range(2): sim.step() cube_object.update(sim.cfg.dt) - if state_location == "com": - if i % 2 == 0: - cube_object.write_body_com_pose_to_sim_index(body_poses=rand_state[..., :7]) - cube_object.write_body_com_velocity_to_sim_index(body_velocities=rand_state[..., 7:]) - else: - cube_object.write_body_com_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_com_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - elif state_location == "link": - if i % 2 == 0: - cube_object.write_body_link_pose_to_sim_index(body_poses=rand_state[..., :7]) - cube_object.write_body_link_velocity_to_sim_index(body_velocities=rand_state[..., 7:]) - else: - cube_object.write_body_link_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_link_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) + body_link_pose_w = cube_object.data.body_link_pose_w.torch + body_com_pose_w = cube_object.data.body_com_pose_w.torch + object_link_to_com_pos, object_link_to_com_quat = subtract_frame_transforms( + body_link_pose_w[..., :3].view(-1, 3), + body_link_pose_w[..., 3:7].view(-1, 4), + body_com_pose_w[..., :3].view(-1, 3), + body_com_pose_w[..., 3:7].view(-1, 4), + ) + + # A target state distinct from the current one in position, orientation, and velocity. + target_pose = torch.cat( + [ + body_link_pose_w[..., :3] + 0.3 * torch.rand(num_envs, num_cubes, 3, device=device), + random_orientation(num_envs * num_cubes, device).view(num_envs, num_cubes, 4), + ], + dim=-1, + ) + target_vel = torch.randn(num_envs, num_cubes, 6, device=device) + # Alternate between the default and explicit environment and body selectors. + selectors = {} if i == 0 else {"env_ids": env_ids, "body_ids": object_ids} if state_location == "com": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.body_com_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.body_com_vel_w.torch) + cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose, **selectors) + cube_object.write_body_com_velocity_to_sim_index(body_velocities=target_vel, **selectors) elif state_location == "link": - torch.testing.assert_close(rand_state[..., :7], cube_object.data.body_link_pose_w.torch) - torch.testing.assert_close(rand_state[..., 7:], cube_object.data.body_link_vel_w.torch) + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose, **selectors) + cube_object.write_body_link_velocity_to_sim_index(body_velocities=target_vel, **selectors) + elif state_location == "root": + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose, **selectors) + cube_object.write_body_com_velocity_to_sim_index(body_velocities=target_vel, **selectors) - -@pytest.mark.parametrize("num_envs", [3]) -@pytest.mark.parametrize("num_cubes", [2]) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("with_offset", [True]) -@pytest.mark.parametrize("state_location", ["com", "link", "root"]) -def test_write_object_state_functions_data_consistency(num_envs, num_cubes, device, with_offset, state_location): - """Test the setters for object_state using both the link frame and center of mass as reference frame.""" - with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - # Create a scene with random cubes - cube_object, env_pos = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device) - env_ids = torch.tensor([x for x in range(num_envs)], dtype=torch.int32) - object_ids = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) - - replicate(sim.get_clone_plan()) - sim.reset() - - # Check if cube_object is initialized - assert cube_object.is_initialized - - # change center of mass offset from link frame - offset = ( - torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - if with_offset - else torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_envs, num_cubes, 1) - ) - - # Set center of mass offset via Newton API (position only, shape (E, B, 3)) - cube_object.set_coms_index(coms=wp.from_torch(offset, dtype=wp.vec3f)) - # Flush the model change immediately so it takes effect before the next step - with wp.ScopedDevice(device): - SimulationManager._solver.notify_model_changed(ModelFlags.BODY_INERTIAL_PROPERTIES) - - # check center of mass has been set - torch.testing.assert_close(cube_object.data.body_com_pos_b.torch, offset) - - rand_state = torch.rand(num_envs, num_cubes, 13, device=device) - rand_state[..., :3] += cube_object.data.body_link_pos_w.torch - # make quaternion a unit vector - rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) - - env_ids = env_ids.to(device) - object_ids = object_ids.to(device) - sim.step() - cube_object.update(sim.cfg.dt) - - body_link_pose_w = cube_object.data.body_link_pose_w.torch - body_com_pose_w = cube_object.data.body_com_pose_w.torch - object_link_to_com_pos, object_link_to_com_quat = subtract_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:7].view(-1, 4), - body_com_pose_w[..., :3].view(-1, 3), - body_com_pose_w[..., 3:7].view(-1, 4), - ) - - if state_location == "com": - cube_object.write_body_com_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_com_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - elif state_location == "link": - cube_object.write_body_link_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_link_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - elif state_location == "root": - cube_object.write_body_link_pose_to_sim_index( - body_poses=rand_state[..., :7], env_ids=env_ids, body_ids=object_ids - ) - cube_object.write_body_com_velocity_to_sim_index( - body_velocities=rand_state[..., 7:], env_ids=env_ids, body_ids=object_ids - ) - - if state_location == "com": - com_pose_w = cube_object.data.body_com_pose_w.torch - com_vel_w = cube_object.data.body_com_vel_w.torch - expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( - com_pose_w[..., :3].view(-1, 3), - com_pose_w[..., 3:].view(-1, 4), - quat_rotate(quat_inv(object_link_to_com_quat), -object_link_to_com_pos), - quat_inv(object_link_to_com_quat), - ) - expected_object_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1).view( - num_envs, -1, 7 - ) - link_pose_w = cube_object.data.body_link_pose_w.torch - link_vel_w = cube_object.data.body_link_vel_w.torch - # test both root_pose and root_link successfully updated when root_com updates - torch.testing.assert_close(expected_object_link_pose, link_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(com_vel_w[..., 3:], link_vel_w[..., 3:]) - torch.testing.assert_close(expected_object_link_pose, link_pose_w) - torch.testing.assert_close(com_vel_w[..., 3:], cube_object.data.body_com_vel_w.torch[..., 3:]) - elif state_location == "link": link_pose_w = cube_object.data.body_link_pose_w.torch link_vel_w = cube_object.data.body_link_vel_w.torch - expected_com_pos, expected_com_quat = combine_frame_transforms( - link_pose_w[..., :3].view(-1, 3), - link_pose_w[..., 3:].view(-1, 4), - object_link_to_com_pos, - object_link_to_com_quat, - ) - expected_object_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1).view(num_envs, -1, 7) com_pose_w = cube_object.data.body_com_pose_w.torch com_vel_w = cube_object.data.body_com_vel_w.torch - # test both root_pose and root_com successfully updated when root_link updates - torch.testing.assert_close(expected_object_com_pose, com_pose_w) - # skip lin_vel because it differs from link frame, this should be fine because we are only checking - # if velocity update is triggered, which can be determined by comparing angular velocity - torch.testing.assert_close(link_vel_w[..., 3:], com_vel_w[..., 3:]) - torch.testing.assert_close(link_pose_w, cube_object.data.body_link_pose_w.torch) - torch.testing.assert_close(link_vel_w[..., 3:], cube_object.data.body_com_vel_w.torch[..., 3:]) - elif state_location == "root": - body_link_pose_w = cube_object.data.body_link_pose_w.torch - body_com_vel_w = cube_object.data.body_com_vel_w.torch - expected_object_com_pos, expected_object_com_quat = combine_frame_transforms( - body_link_pose_w[..., :3].view(-1, 3), - body_link_pose_w[..., 3:].view(-1, 4), - object_link_to_com_pos, - object_link_to_com_quat, - ) - expected_object_com_pose = torch.cat((expected_object_com_pos, expected_object_com_quat), dim=1).view( - num_envs, -1, 7 - ) - com_pose_w = cube_object.data.body_com_pose_w.torch - com_vel_w = cube_object.data.body_com_vel_w.torch - link_pose_w = cube_object.data.body_link_pose_w.torch - link_vel_w = cube_object.data.body_link_vel_w.torch - # test both root_com and root_link successfully updated when root_pose updates - torch.testing.assert_close(expected_object_com_pose, com_pose_w) - torch.testing.assert_close(body_com_vel_w, com_vel_w) - torch.testing.assert_close(body_link_pose_w, link_pose_w) - torch.testing.assert_close(body_com_vel_w[..., 3:], link_vel_w[..., 3:]) + if state_location == "com": + torch.testing.assert_close(target_pose, com_pose_w) + torch.testing.assert_close(target_vel, com_vel_w) + # the com pose was written, so the derived link pose must be refreshed + expected_link_pos, expected_link_quat = combine_frame_transforms( + com_pose_w[..., :3].view(-1, 3), + com_pose_w[..., 3:].view(-1, 4), + quat_rotate(quat_inv(object_link_to_com_quat), -object_link_to_com_pos), + quat_inv(object_link_to_com_quat), + ) + expected_link_pose = torch.cat((expected_link_pos, expected_link_quat), dim=1).view(num_envs, -1, 7) + torch.testing.assert_close(expected_link_pose, link_pose_w) + else: + torch.testing.assert_close(target_pose, link_pose_w) + written_vel_w = link_vel_w if state_location == "link" else com_vel_w + torch.testing.assert_close(target_vel, written_vel_w) + # the link pose was written, so the derived com pose must be refreshed + expected_com_pos, expected_com_quat = combine_frame_transforms( + link_pose_w[..., :3].view(-1, 3), + link_pose_w[..., 3:].view(-1, 4), + object_link_to_com_pos, + object_link_to_com_quat, + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1).view(num_envs, -1, 7) + torch.testing.assert_close(expected_com_pose, com_pose_w) + # skip lin_vel because it differs between the frames; angular velocity is frame-independent + # and only matches when the derived velocity was actually refreshed after the write + torch.testing.assert_close(com_vel_w[..., 3:], link_vel_w[..., 3:]) + + # The written state persists into the solver: with gravity off, one step only integrates the + # written velocity. + written_pose_w = (com_pose_w if state_location == "com" else link_pose_w).clone() + written_com_vel_w = com_vel_w.clone() + sim.step() + cube_object.update(sim.cfg.dt) + pose_w = cube_object.data.body_com_pose_w if state_location == "com" else cube_object.data.body_link_pose_w + torch.testing.assert_close(pose_w.torch, written_pose_w, rtol=1e-1, atol=1e-1) + torch.testing.assert_close(cube_object.data.body_com_vel_w.torch, written_com_vel_w, rtol=1e-1, atol=1e-1) @pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"]) @pytest.mark.isaacsim_ci -def test_body_pose_write_marks_fk_reset_mask(device, writer): +def test_body_pose_write_marks_fk_reset_mask(device): """Regression: ``write_body_{link,com}_pose_to_sim_{index,mask}`` must mark FK dirty. For a collection, ``_sim_bind_body_link_pose_w`` is bound directly to the simulator's root-transforms @@ -1005,33 +661,34 @@ def _fk_reset_mask_dirty() -> bool: sim.step() cube_object.update(sim.cfg.dt) - # Clear the dirty flag so we can observe that the write sets it. - SimulationManager.forward() - assert not _fk_reset_mask_dirty() - - pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() - - target_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() - target_pose[..., 0] += 10.0 - target_pose[..., 1] += 5.0 - target_pose[..., 2] += 2.0 - - if writer == "link_index": - cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose) - elif writer == "link_mask": - cube_object.write_body_link_pose_to_sim_mask(body_poses=target_pose) - elif writer == "com_index": - cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose) - elif writer == "com_mask": - cube_object.write_body_com_pose_to_sim_mask(body_poses=target_pose) - - assert _fk_reset_mask_dirty(), "pose write must call SimulationManager.invalidate_fk()" - - # body_link_pose_w must reflect the write immediately — its underlying buffer is the write - # target. A regression that moves this property to a separate cached buffer (mirroring the - # single-object case) would silently break this invariant. - body_link = wp.to_torch(cube_object.data.body_link_pose_w) - assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( - "body_link_pose_w still aliases the pre-write pose; the underlying buffer was not written" - ) - torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) + for writer in ("link_index", "link_mask", "com_index", "com_mask"): + # Clear the dirty flag so we can observe that the write sets it. + SimulationManager.forward() + assert not _fk_reset_mask_dirty() + + pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() + + target_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() + target_pose[..., 0] += 10.0 + target_pose[..., 1] += 5.0 + target_pose[..., 2] += 2.0 + + if writer == "link_index": + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose) + elif writer == "link_mask": + cube_object.write_body_link_pose_to_sim_mask(body_poses=target_pose) + elif writer == "com_index": + cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose) + elif writer == "com_mask": + cube_object.write_body_com_pose_to_sim_mask(body_poses=target_pose) + + assert _fk_reset_mask_dirty(), f"{writer} pose write must call SimulationManager.invalidate_fk()" + + # body_link_pose_w must reflect the write immediately — its underlying buffer is the write + # target. A regression that moves this property to a separate cached buffer (mirroring the + # single-object case) would silently break this invariant. + body_link = wp.to_torch(cube_object.data.body_link_pose_w) + assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( + f"body_link_pose_w still aliases the pre-write pose after {writer}; the buffer was not written" + ) + torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) diff --git a/source/isaaclab_newton/test/assets/test_wrench_kernels.py b/source/isaaclab_newton/test/assets/test_wrench_kernels.py deleted file mode 100644 index 4dc272cc9417..000000000000 --- a/source/isaaclab_newton/test/assets/test_wrench_kernels.py +++ /dev/null @@ -1,73 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Regression tests for Newton external-wrench packing kernels.""" - -import numpy as np -import warp as wp -from isaaclab_newton.assets import kernels as shared_kernels -from isaaclab_newton.assets.articulation import kernels as articulation_kernels - -_IDENTITY_QUAT = (0.0, 0.0, 0.0, 1.0) -_QUARTER_TURN_Z_QUAT = (0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)) - - -def _body_link_poses_w( - poses: list[tuple[tuple[float, float, float], tuple[float, float, float, float]]], -) -> wp.array: - """Create one-environment body-link poses from translations and quaternions.""" - poses = np.asarray([[[*translation, *quaternion] for translation, quaternion in poses]], dtype=np.float32) - return wp.array(poses, dtype=wp.transformf, device="cpu") - - -def test_update_wrench_array_rotates_body_wrenches_to_world_frame() -> None: - """Rotate body-frame force and torque into the world-frame wrench.""" - forces = wp.array(np.asarray([[[2.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - torques = wp.array(np.asarray([[[3.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - # A COM wrench rotation must not add a link-origin moment from this translation. - body_link_pose_w = _body_link_poses_w([((7.0, -5.0, 11.0), _QUARTER_TURN_Z_QUAT)]) - wrench = wp.zeros((1, 1), dtype=wp.spatial_vectorf, device="cpu") - env_mask = wp.array(np.asarray([True]), dtype=wp.bool, device="cpu") - body_mask = wp.array(np.asarray([True]), dtype=wp.bool, device="cpu") - - wp.launch( - shared_kernels.update_wrench_array_with_force_and_torque, - dim=(1, 1), - inputs=[forces, torques, body_link_pose_w, wrench, env_mask, body_mask], - device="cpu", - ) - - np.testing.assert_allclose( - wrench.numpy(), np.asarray([[[0.0, 2.0, 0.0, 0.0, 3.0, 0.0]]], dtype=np.float32), atol=1e-6 - ) - - -def test_update_wrench_array_ordered_rotates_and_scatter_wrenches_to_backend_order() -> None: - """Rotate public-order body wrenches and scatter them into backend order.""" - forces = wp.array(np.asarray([[[1.0, 0.0, 0.0], [3.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - torques = wp.array(np.asarray([[[2.0, 0.0, 0.0], [4.0, 0.0, 0.0]]], dtype=np.float32), dtype=wp.vec3f, device="cpu") - # Link poses are public ordered: public body 0 is rotated, then maps to - # backend body 1. The rotated body's nonzero translation must not add a - # p x f moment. - body_link_pose_w = _body_link_poses_w( - [((2.0, -3.0, 4.0), _QUARTER_TURN_Z_QUAT), ((-1.0, 6.0, 8.0), _IDENTITY_QUAT)] - ) - user_to_backend = wp.array(np.asarray([1, 0], dtype=np.int32), dtype=wp.int32, device="cpu") - wrench = wp.zeros((1, 2), dtype=wp.spatial_vectorf, device="cpu") - env_mask = wp.array(np.asarray([True]), dtype=wp.bool, device="cpu") - body_mask = wp.array(np.asarray([True, True]), dtype=wp.bool, device="cpu") - - wp.launch( - articulation_kernels.update_wrench_array_with_force_and_torque_ordered, - dim=(1, 2), - inputs=[forces, torques, body_link_pose_w, user_to_backend, wrench, env_mask, body_mask], - device="cpu", - ) - - np.testing.assert_allclose( - wrench.numpy(), - np.asarray([[[3.0, 0.0, 0.0, 4.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 2.0, 0.0]]], dtype=np.float32), - atol=1e-6, - ) diff --git a/source/isaaclab_newton/test/cloner/test_collision_approximation.py b/source/isaaclab_newton/test/cloner/test_collision_approximation.py index 854730f6faf9..b80b73fc68b9 100644 --- a/source/isaaclab_newton/test/cloner/test_collision_approximation.py +++ b/source/isaaclab_newton/test/cloner/test_collision_approximation.py @@ -107,6 +107,10 @@ def _make_mixed_visual_stage() -> Usd.Stage: UsdGeom.Sphere.Define(stage, f"{_SOURCE}/StaticAuthored/visual") static_collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/StaticAuthored/collider") UsdPhysics.CollisionAPI.Apply(static_collider.GetPrim()) + + mesh_body = UsdGeom.Xform.Define(stage, f"{_SOURCE}/MeshOnly") + UsdPhysics.RigidBodyAPI.Apply(mesh_body.GetPrim()) + _add_l_prism(stage, f"{_SOURCE}/MeshOnly/geom", None, offset=8.0) return stage @@ -153,11 +157,6 @@ def test_authored_approximation_produces_expected_shape(self, approximation, exp shapes = _collision_shapes(_build(_make_stage(approximation))) assert list(shapes.values()) == [expected] - def test_unauthored_mesh_is_never_approximated(self): - """The cloner approximates nothing on its own: USD defaults ``physics:approximation`` to ``none``.""" - shapes = _collision_shapes(_build(_make_stage(None))) - assert list(shapes.values()) == [GeoType.MESH] - def test_only_the_authored_mesh_is_remeshed_in_a_mixed_stage(self): """A sibling that authors nothing keeps its trimesh while the authored one is remeshed.""" stage = _make_stage("boundingCube") @@ -182,15 +181,6 @@ def test_differing_sources_each_keep_their_own_authored_mode(self): assert list(_collision_shapes(builders[sources[0]]).values()) == [GeoType.SPHERE] assert list(_collision_shapes(builders[sources[1]]).values()) == [GeoType.MESH] - def test_heterogeneous_sources_with_equal_sequences_stay_honored(self): - """Identically authored sources keep their authored modes (no fallback).""" - stage, sources = _make_two_source_stage("boundingSphere", "boundingSphere") - - builders = _build_sources(stage, sources) - - for source in sources: - assert list(_collision_shapes(builders[source]).values()) == [GeoType.SPHERE] - def test_sdf_collider_is_never_remeshed(self): """``physics:approximation`` is ignored on an SDF collider, matching Newton's importer. @@ -205,7 +195,11 @@ def test_sdf_collider_is_never_remeshed(self): assert list(shapes.values()) == [GeoType.MESH] def test_primitive_collider_remains_visible_in_mixed_visual_model(self): - """Colliders remain visible only when their body or static parent has no visual shape.""" + """Colliders remain visible only when their body or static parent has no visual shape. + + A lone default-purpose collision mesh also stays visible: assets that author a single mesh as + both collider and render geometry have no visual-only shape to fall back on. + """ builder = _build(_make_mixed_visual_stage()) flags_by_label = dict(zip(builder.shape_label, builder.shape_flags, strict=True)) @@ -213,21 +207,4 @@ def test_primitive_collider_remains_visible_in_mixed_visual_model(self): assert not flags_by_label[f"{_SOURCE}/Authored/collider"] & ShapeFlags.VISIBLE assert flags_by_label[f"{_SOURCE}/StaticPrimitive/geometry"] & ShapeFlags.VISIBLE assert not flags_by_label[f"{_SOURCE}/StaticAuthored/collider"] & ShapeFlags.VISIBLE - - def test_mesh_collider_remains_visible_in_mixed_visual_model(self): - """A lone default-purpose collision mesh renders even when other bodies have visuals. - - Assets that author a single mesh as both collider and render geometry have no - visual-only shape to fall back on, so hiding their collider makes the body - disappear from the viewer. - """ - stage = _make_mixed_visual_stage() - mesh_body = UsdGeom.Xform.Define(stage, f"{_SOURCE}/MeshOnly") - UsdPhysics.RigidBodyAPI.Apply(mesh_body.GetPrim()) - _add_l_prism(stage, f"{_SOURCE}/MeshOnly/geom", None, offset=8.0) - - builder = _build(stage) - flags_by_label = dict(zip(builder.shape_label, builder.shape_flags, strict=True)) - assert flags_by_label[f"{_SOURCE}/MeshOnly/geom"] & ShapeFlags.VISIBLE - assert not flags_by_label[f"{_SOURCE}/Authored/collider"] & ShapeFlags.VISIBLE diff --git a/source/isaaclab_newton/test/cloner/test_visual_shape_import.py b/source/isaaclab_newton/test/cloner/test_visual_shape_import.py index 9487a65ed8c2..c42705d57710 100644 --- a/source/isaaclab_newton/test/cloner/test_visual_shape_import.py +++ b/source/isaaclab_newton/test/cloner/test_visual_shape_import.py @@ -8,6 +8,7 @@ from types import SimpleNamespace import newton +import pytest from isaaclab_newton.cloner import newton_clone_utils from isaaclab_newton.cloner.newton_clone_utils import build_source_builders from newton import ShapeFlags @@ -62,17 +63,12 @@ def _build(stage: Usd.Stage, **kwargs) -> newton.ModelBuilder: class TestClonerVisualShapeImport: """``build_source_builders`` must keep colliders regardless of the visual-shape flag.""" - def test_load_visual_shapes_imports_visual_only_geometry(self): - """The default import keeps the visual-only cube alongside the collider.""" - colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=True)) + @pytest.mark.parametrize("load_visual_shapes", [True, False]) + def test_visual_shape_flag_gates_only_visual_only_geometry(self, load_visual_shapes): + """The visual-only cube is imported only with visual shapes; the collider is always kept.""" + colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=load_visual_shapes)) assert colliding == 1 - assert visual_only == 1 - - def test_skipping_visual_shapes_keeps_colliders(self): - """Disabling visual shapes drops the visual-only cube and nothing else.""" - colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=False)) - assert colliding == 1 - assert visual_only == 0 + assert visual_only == int(load_visual_shapes) def test_skipping_visual_shapes_skips_collider_visibility_resolution(self): """Without visual shapes no collider is hidden, so the restore pass must not run. @@ -123,24 +119,6 @@ def test_nested_static_collider_uses_rigid_body_root_for_visual_lookup(self): assert not builder.shape_flags[0] & ShapeFlags.VISIBLE - def test_static_collider_without_rigid_body_or_visual_remains_visible(self): - """A standalone static collider retains its authored viewport visibility.""" - stage = Usd.Stage.CreateInMemory() - collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/collision") - UsdPhysics.CollisionAPI.Apply(collider.GetPrim()) - builder = SimpleNamespace( - shape_body=[-1], - shape_flags=[ShapeFlags.COLLIDE_SHAPES], - shape_label=[str(collider.GetPrim().GetPath())], - shape_type=[newton.GeoType.BOX], - ) - - newton_clone_utils._restore_visible_colliders_without_visual_shapes( - builder, stage, {str(collider.GetPrim().GetPath()): 0} - ) - - assert builder.shape_flags[0] & ShapeFlags.VISIBLE - def test_generated_proxy_collider_visual_remains_hidden(self): """Newton's unauthored visual companion must not expose a proxy collider.""" stage = Usd.Stage.CreateInMemory() diff --git a/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py b/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py index 4f37651410e3..7ea6e8733cce 100644 --- a/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py +++ b/source/isaaclab_newton/test/controllers/test_newton_ik_solver.py @@ -104,20 +104,6 @@ def test_solve_writes_output_buffer(monkeypatch): assert torch.allclose(wp.to_torch(result), torch.tensor([[2.0, 3.0], [4.0, 5.0]])) -def test_constraint_objectives_carry_no_target_or_action(monkeypatch): - _patch_newton_ik(monkeypatch) - solver = _pose_solver( - objectives=[ - NewtonIKPoseObjectiveCfg(body_name="ee"), - NewtonIKJointLimitObjectiveCfg(weight=0.1), - ] - ) - # Only the pose objective is named and command-driven; the joint limit is a - # pure constraint (no name, no action dimensions). - assert list(solver.objectives_by_name) == ["ee"] - assert [obj.action_dim for obj in solver.objectives] == [6, 0] - - def test_multiple_pose_objectives_register_distinct_targets(monkeypatch): _patch_newton_ik(monkeypatch) solver = _pose_solver( @@ -127,7 +113,10 @@ def test_multiple_pose_objectives_register_distinct_targets(monkeypatch): NewtonIKJointLimitObjectiveCfg(weight=0.1), ] ) + # Only pose objectives are named and command-driven; the joint limit is a + # pure constraint (no name, no action dimensions). assert list(solver.objectives_by_name) == ["ee", "torso"] + assert [obj.action_dim for obj in solver.objectives] == [6, 6, 0] assert solver.objectives_by_name["ee"].link_index == 0 assert solver.objectives_by_name["torso"].link_index == 1 diff --git a/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py b/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py index ff406ced13ef..93c6413fb639 100644 --- a/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py +++ b/source/isaaclab_newton/test/physics/test_mjwarp_tendon_control.py @@ -108,7 +108,7 @@ def test_a_view_without_the_actuator_frequency_resolves_to_nothing(): def test_a_tendon_no_actuator_drives_gets_no_column(): - """A passive tendon keeps column -1; the other tendons still find their actuators.""" + """A passive tendon gets no column; the other tendons still find their actuators.""" view, model = _make_view_and_model(tendon_count=3) columns, tendon_ids = resolve_fixed_tendon_actuator_columns(view, model) @@ -140,7 +140,7 @@ def test_tendons_no_actuator_transmits_to_are_named_once(caplog): with caplog.at_level(logging.WARNING): MjWarpTendonControl(articulation, _pair([1, 2], [0, 2]), articulation.root_view) - assert "passive" in caplog.text + assert caplog.text.count("passive") == 1 assert "rh_FFJ0" not in caplog.text diff --git a/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py b/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py index 18a5fd0d1bb8..c22ba0b0d7e7 100644 --- a/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py +++ b/source/isaaclab_newton/test/physics/test_mpm_reset_mask_contract.py @@ -46,20 +46,15 @@ def cpu_mpm_solver_and_state(): return solver, model.state() -def test_mpm_reset_accepts_global_sentinel_mask(cpu_mpm_solver_and_state): - """The manager factory uses Newton's current global-sentinel mask contract.""" - solver, state = cpu_mpm_solver_and_state - - solver.reset(state, world_mask=wp.array([False, True, False], dtype=wp.bool, device="cpu"), flags=0) - - def test_mpm_manager_explicit_reset_accepts_canonical_mask(cpu_mpm_solver_and_state, monkeypatch): - """The explicit task reset forwards Newton's canonical mask unchanged.""" + """The explicit task reset forwards Newton's canonical global-sentinel mask to the real solver.""" solver, state = cpu_mpm_solver_and_state calls = [] + newton_reset = SolverImplicitMPM.reset def record_reset(self, state, world_mask=None, flags=None): calls.append((state, world_mask, flags)) + newton_reset(self, state, world_mask=world_mask, flags=flags) monkeypatch.setattr(SolverImplicitMPM, "reset", record_reset) monkeypatch.setattr(NewtonManager, "_solver", solver) diff --git a/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py b/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py index 7002b3973bfc..239b6f8a7376 100644 --- a/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py +++ b/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py @@ -160,6 +160,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): transforms: write asset state, then render without an intervening physics step or asset-data read. The assertion reads Fabric's world matrix, which is the transform consumed by Kit/RTX; USD is intentionally not written back. + The synchronized pose must also keep the body's authored USD scale. """ device = "cuda:0" sim_cfg = SimulationCfg( @@ -174,16 +175,21 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): scene = InteractiveScene(_RenderSceneCfg(num_envs=1, env_spacing=2.0)) sim.register_interactive_scene(scene) try: + body_path = "/World/envs/env_0/Cube" + authored_scale = torch.tensor([0.25, 0.5, 0.75]) + body_prim = sim_utils.get_current_stage().GetPrimAtPath(body_path) + body_prim.GetAttribute("xformOp:scale").Set(UsdGf.Vec3d(*authored_scale.tolist())) + sim.reset() scene.reset() _render(sim, scene) + torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) fabric = sim.get_or_create_backend(FabricBackendCfg(stage=sim.stage, device=sim.device)) assert sim.visualizers[0]._fabric is scene["camera"]._renderer._fabric is fabric assert sum(isinstance(resource, FabricBackend) for _, resource in sim._backend_registry) == 1 cube = scene["cube"] - body_path = "/World/envs/env_0/Cube" target_pose = torch.tensor( [[1.5, -0.75, 2.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, @@ -238,6 +244,7 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): rtol=0.0, atol=1.0e-4, ) + torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) finally: sim.register_interactive_scene(None) @@ -246,8 +253,8 @@ def test_root_pose_write_is_visible_on_next_render_without_step(): @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") @pytest.mark.parametrize( ("device", "renderer_cfg"), - [("cpu", IsaacRtxRendererCfg()), ("cuda:0", IsaacRtxRendererCfg()), ("cuda:0", NewtonWarpRendererCfg())], - ids=["rtx-cpu", "rtx-cuda", "newton-warp"], + [("cpu", IsaacRtxRendererCfg()), ("cuda:0", NewtonWarpRendererCfg())], + ids=["rtx-cpu", "newton-warp"], ) def test_root_pose_sync_preserves_authored_scale(device, renderer_cfg): """Newton body pose synchronization must preserve authored USD scale in Kit/RTX.""" @@ -527,35 +534,15 @@ def test_frame_view_pose_write_reaches_fabric_when_the_scope_raises(): _assert_position(_fabric_position(frame_path), target_position) -@pytest.mark.isaacsim_ci -@pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") -def test_frame_view_pose_write_on_body_child_survives_body_motion(): - """A body-attached frame renders at the written pose and keeps tracking the body.""" - device = "cuda:0" - body_path = "/World/envs/env_0/Cube" - frame_path = f"{body_path}/Frame" - - with _frame_scene(frame_path, (0.0, 0.0, 0.35), device) as (sim, scene, view): - body_start = torch.tensor([0.0, 0.0, 1.0]) - written_position = body_start + torch.tensor([0.5, 0.0, 0.0]) - _write_frame_world_position(view, written_position.to(device)) - _render(sim, scene) - - _assert_position(_fabric_position(frame_path), written_position) - - body_pose = torch.tensor([[1.5, -0.75, 2.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device) - scene["cube"].write_root_link_pose_to_sim_index(root_pose=body_pose) - _render(sim, scene) - - expected = body_pose[0, :3].cpu() + (written_position - body_start) - _assert_position(_reported_position(view), expected) - _assert_position(_fabric_position(frame_path), expected) - - @pytest.mark.isaacsim_ci @pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): - """Building the mirror must not reseed its prims from USD, which would unrender the moved body.""" + """A body-attached frame renders at written poses while the body moves. + + Building the mirror on the first frame write must not reseed its prims from USD, which would + unrender the moved body. The written frame then keeps tracking later body writes, and a frame + write after unrendered physics steps composes against the live body pose. + """ device = "cuda:0" body_path = "/World/envs/env_0/Cube" frame_path = f"{body_path}/Frame" @@ -567,6 +554,7 @@ def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): _render(sim, scene) _assert_position(_fabric_position(body_path), body_target) + # The first frame write must be the one that builds the mirror. late_child = f"{frame_path}/LateChild" offset = torch.tensor([0.0, 0.0, 0.25]) sim_utils.create_prim(late_child, "Xform", translation=tuple(offset.tolist())) @@ -578,22 +566,22 @@ def test_first_frame_pose_write_after_body_move_leaves_the_body_rendered(): _assert_position(_fabric_position(frame_path), written_position) _assert_position(_fabric_position(late_child), written_position + offset) + # The written frame keeps tracking its body. + moved_pose = torch.tensor([[-1.0, 0.5, 1.0, 0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device) + scene["cube"].write_root_link_pose_to_sim_index(root_pose=moved_pose) + _render(sim, scene) -@pytest.mark.isaacsim_ci -@pytest.mark.skipif(not wp.get_cuda_device_count(), reason="CUDA is unavailable") -def test_frame_view_pose_write_after_unrendered_steps_reaches_fabric(): - """A pose write renders correctly even when the body moved since the last render.""" - device = "cuda:0" - body_path = "/World/envs/env_0/Cube" - frame_path = f"{body_path}/Frame" + expected = moved_pose[0, :3].cpu() + (written_position - body_target) + _assert_position(_reported_position(view), expected) + _assert_position(_fabric_position(frame_path), expected) - with _frame_scene(frame_path, (0.0, 0.0, 0.35), device) as (sim, scene, view): + # Move the body without rendering, then write the frame. velocity = torch.zeros((1, 6), dtype=torch.float32, device=device) velocity[0, 0] = 5.0 scene["cube"].write_root_com_velocity_to_sim_index(root_velocity=velocity) for _ in range(30): sim.step(render=False) - assert _fabric_position(body_path)[0].item() == pytest.approx(0.0, abs=1.0e-4) + _assert_position(_fabric_position(body_path), moved_pose[0, :3].cpu()) target_position = torch.tensor([0.0, 0.0, 1.5]) _write_frame_world_position(view, target_position.to(device)) diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index c5cf56892cd8..d93a1229a019 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -7,16 +7,11 @@ Covers: -* :attr:`NewtonSolverCfg.class_type` resolves to the matching manager subclass. * :meth:`NewtonCfg.__post_init__` propagates ``solver_cfg.class_type`` onto :attr:`NewtonCfg.class_type` so that ``SimulationContext`` picks the right manager. -* Each leaf manager subclasses :class:`NewtonManager` and implements - :meth:`_build_solver` (with the abstract base raising ``NotImplementedError``). * The cross-config validation in :meth:`NewtonMJWarpManager._build_solver` rejects the ``MJWarp + use_mujoco_contacts=True + collision_cfg`` combination. -* Manager name dispatch (used by :class:`InteractiveScene` and the various - factory dispatchers) still starts with ``"newton"``. * Fixed-root pose writes refresh MuJoCo's solver-owned root transform. * End-to-end: spinning up a simulation with each solver builds the correct solver, sets the right ``_use_single_state`` / ``_needs_collision_pipeline`` @@ -60,7 +55,6 @@ NewtonMJWarpManager, NewtonMPMManager, NewtonShapeCfg, - NewtonSolverCfg, NewtonVBDManager, NewtonXPBDManager, VBDSolverCfg, @@ -165,21 +159,6 @@ # --------------------------------------------------------------------------- -@pytest.mark.parametrize( - "solver_cfg_factory, expected_manager, _solver_cls, _single_state, _pipeline", - SOLVER_MATRIX, -) -def test_solver_cfg_class_type_resolves_to_subclass( - solver_cfg_factory, expected_manager, _solver_cls, _single_state, _pipeline -): - """Each ``*SolverCfg.class_type`` resolves to its matching manager subclass.""" - solver_cfg = solver_cfg_factory() - # ``class_type`` is a lazy ``"module:Class"`` reference; calling its - # ``_resolve()`` returns the actual class. ``__name__`` works without - # forcing import (LazyType caches metadata) and is sufficient identity. - assert solver_cfg.class_type.__name__ == expected_manager.__name__ - - @pytest.mark.parametrize( "solver_cfg_factory, expected_manager, _solver_cls, _single_state, _pipeline", SOLVER_MATRIX, @@ -196,11 +175,9 @@ def test_newton_cfg_post_init_propagates_class_type( "num_substeps, collision_decimation, should_warn", [ (8, 0, False), # Default: feature disabled, no warning. - (8, 1, False), # Valid: re-collide every substep. - (8, 2, False), # Valid: re-collide every 2 substeps. (8, 7, False), # Valid edge: one mid-loop re-collide at i=6. (8, 8, True), # Equal to num_substeps: gate never fires. - (8, 16, True), # Larger than num_substeps: gate never fires. + (8, 16, True), # Above num_substeps: pins the ``>=`` rather than ``==`` boundary. ], ) def test_newton_cfg_collision_decimation_warning(num_substeps, collision_decimation, should_warn, caplog): @@ -462,27 +439,24 @@ def test_mpm_solver_cfg_maps_only_newton_solver_fields(): @pytest.mark.parametrize( - "deprecated_value, replacement", + "mode, expected, deprecated", [ - ("instantaneous", "forward"), - ("finite_difference", "backward"), + ("instantaneous", "forward", True), + ("finite_difference", "backward", True), + ("forward", "forward", False), + ("backward", "backward", False), ], ) -def test_mpm_solver_cfg_translates_deprecated_collider_velocity_modes(deprecated_value, replacement): - """Deprecated collider velocity modes warn and map to Newton's current values.""" - with pytest.warns(DeprecationWarning, match=f"use {replacement!r}"): - newton_cfg = _make_solver_config(MPMSolverCfg(collider_velocity_mode=deprecated_value)) - - assert newton_cfg.collider_velocity_mode == replacement - - -@pytest.mark.parametrize("mode", ["forward", "backward"]) -def test_mpm_solver_cfg_preserves_canonical_collider_velocity_modes(mode, recwarn): - """Canonical collider velocity modes pass through without deprecation warnings.""" +def test_mpm_solver_cfg_translates_deprecated_collider_velocity_modes(mode, expected, deprecated, recwarn): + """Deprecated collider velocity modes warn and map to Newton's values; canonical modes pass through silently.""" newton_cfg = _make_solver_config(MPMSolverCfg(collider_velocity_mode=mode)) - assert newton_cfg.collider_velocity_mode == mode - assert not [warning for warning in recwarn if issubclass(warning.category, DeprecationWarning)] + assert newton_cfg.collider_velocity_mode == expected + deprecations = [str(w.message) for w in recwarn if issubclass(w.category, DeprecationWarning)] + if deprecated: + assert any(f"use {expected!r}" in message for message in deprecations) + else: + assert not deprecations # Tuples of ``(field_name, non_default_value)`` covering every solver-tunable @@ -514,19 +488,19 @@ def test_mpm_solver_cfg_preserves_canonical_collider_velocity_modes(mode, recwar ] -@pytest.mark.parametrize("field_name, value", _MPM_FIELD_VALUES) -def test_mpm_solver_cfg_forwards_every_solver_field(field_name, value): +def test_mpm_solver_cfg_forwards_every_solver_field(): """Every tunable MPM cfg field round-trips into ``SolverImplicitMPM.Config``. Guards against MPM manager construction dropping or mis-naming a field if Newton's config surface changes. """ - solver_cfg = MPMSolverCfg(**{field_name: value}) + solver_cfg = MPMSolverCfg(**dict(_MPM_FIELD_VALUES)) newton_cfg = _make_solver_config(solver_cfg) - assert hasattr(newton_cfg, field_name), ( - f"{field_name!r} disappeared from SolverImplicitMPM.Config — MPMSolverCfg needs to drop or rename it." - ) - assert getattr(newton_cfg, field_name) == value + for field_name, value in _MPM_FIELD_VALUES: + assert hasattr(newton_cfg, field_name), ( + f"{field_name!r} disappeared from SolverImplicitMPM.Config — MPMSolverCfg needs to drop or rename it." + ) + assert getattr(newton_cfg, field_name) == value _KAMINO_PADMM_FIELD_VALUES = [ @@ -571,16 +545,20 @@ def test_mpm_solver_cfg_forwards_every_solver_field(field_name, value): ] -@pytest.mark.parametrize("field_name, value", _KAMINO_PADMM_FIELD_VALUES) -def test_kamino_solver_cfg_forwards_padmm_fields(field_name, value): +def test_kamino_solver_cfg_forwards_padmm_fields(): """Every tunable P-ADMM cfg field round-trips into ``PADMMSolverConfig``.""" - sparse_kwargs = {"sparse_jacobian": True, "sparse_dynamics": True} if field_name == "penalty_update_method" else {} - solver_cfg = KaminoPADMMSolverCfg(**sparse_kwargs, dynamics_solver_cfg=KaminoPADMMCfg(**{field_name: value})) - newton_cfg = solver_cfg.to_solver_config() - assert hasattr(newton_cfg.padmm, field_name), ( - f"{field_name!r} disappeared from PADMMSolverConfig — KaminoPADMMCfg needs to drop or rename it." + # Adaptive penalty updates require the sparse solver path. + solver_cfg = KaminoPADMMSolverCfg( + sparse_jacobian=True, + sparse_dynamics=True, + dynamics_solver_cfg=KaminoPADMMCfg(**dict(_KAMINO_PADMM_FIELD_VALUES)), ) - assert getattr(newton_cfg.padmm, field_name) == value + newton_cfg = solver_cfg.to_solver_config() + for field_name, value in _KAMINO_PADMM_FIELD_VALUES: + assert hasattr(newton_cfg.padmm, field_name), ( + f"{field_name!r} disappeared from PADMMSolverConfig — KaminoPADMMCfg needs to drop or rename it." + ) + assert getattr(newton_cfg.padmm, field_name) == value def test_kamino_padmm_rejects_adaptive_penalties_with_dense_dynamics(): @@ -590,18 +568,18 @@ def test_kamino_padmm_rejects_adaptive_penalties_with_dense_dynamics(): solver_cfg.to_solver_config() -@pytest.mark.parametrize("field_name, value", _KAMINO_DVI_FIELD_VALUES) -def test_kamino_solver_cfg_forwards_dvi_fields(field_name, value): +def test_kamino_solver_cfg_forwards_dvi_fields(): """Every tunable DVI cfg field round-trips into ``DVISolverConfig``.""" solver_cfg = KaminoDVISolverCfg( dynamics=KaminoDynamicsCfg(preconditioning=False), - dynamics_solver_cfg=KaminoDVICfg(**{field_name: value}), + dynamics_solver_cfg=KaminoDVICfg(**dict(_KAMINO_DVI_FIELD_VALUES)), ) newton_cfg = solver_cfg.to_solver_config() - assert hasattr(newton_cfg.dvi, field_name), ( - f"{field_name!r} disappeared from DVISolverConfig — KaminoDVICfg needs to drop or rename it." - ) - assert getattr(newton_cfg.dvi, field_name) == value + for field_name, value in _KAMINO_DVI_FIELD_VALUES: + assert hasattr(newton_cfg.dvi, field_name), ( + f"{field_name!r} disappeared from DVISolverConfig — KaminoDVICfg needs to drop or rename it." + ) + assert getattr(newton_cfg.dvi, field_name) == value @pytest.mark.parametrize("field_name, value", _KAMINO_DYNAMICS_FIELD_VALUES) @@ -643,52 +621,29 @@ def test_kamino_dvi_rejects_preconditioning(): solver_cfg.to_solver_config() -def test_mpm_register_builder_attributes_is_idempotent(): - """The MPM custom-attribute hook is a no-op when attributes are already registered.""" - import newton - - builder = newton.ModelBuilder() - assert not builder.has_custom_attribute("mpm:young_modulus") - - NewtonMPMManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mpm:young_modulus") - - # Second call must be a no-op (no exceptions, attribute still present). - NewtonMPMManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mpm:young_modulus") - - -def test_mjwarp_register_builder_attributes_is_idempotent(): - """The MJWarp hook registers native MuJoCo entities exactly once.""" - import newton - - builder = newton.ModelBuilder() - assert not builder.has_custom_attribute("mujoco:actuator_gainprm") - - NewtonMJWarpManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mujoco:actuator_gainprm") - assert "mujoco:actuator" in builder.custom_frequencies - assert "mujoco:tendon" in builder.custom_frequencies - - NewtonMJWarpManager._register_builder_attributes(builder) - assert builder.has_custom_attribute("mujoco:actuator_gainprm") - - @pytest.mark.parametrize( ("manager", "active", "inactive"), [ (NewtonMJWarpManager, "mujoco:condim", ("kamino:max_solver_iterations", "mpm:young_modulus")), (NewtonKaminoManager, "kamino:max_solver_iterations", ("mujoco:condim", "mpm:young_modulus")), + (NewtonMPMManager, "mpm:young_modulus", ("mujoco:condim", "kamino:max_solver_iterations")), ], ) -def test_rigid_solver_registers_only_its_builder_attributes(manager, active, inactive): - """A rigid solver declares its own builder schema and no inactive solver schema.""" +def test_solver_registers_only_its_builder_attributes(manager, active, inactive): + """A solver declares its own builder schema once and no inactive solver schema.""" builder = ModelBuilder() manager._register_builder_attributes(builder) assert builder.has_custom_attribute(active) assert all(not builder.has_custom_attribute(name) for name in inactive) + if manager is NewtonMJWarpManager: + assert "mujoco:actuator" in builder.custom_frequencies + assert "mujoco:tendon" in builder.custom_frequencies + + # A second registration on the same builder is a no-op. + manager._register_builder_attributes(builder) + assert builder.has_custom_attribute(active) def test_clone_source_builder_has_no_solver_dependency(): @@ -802,6 +757,9 @@ def test_production_imports_scope_mujoco_joint_properties( joint.CreateBody1Rel().SetTargets([child_path]) joint.GetPrim().CreateAttribute("mjc:frictionloss", Sdf.ValueTypeNames.Double, True).Set(0.11) joint.GetPrim().CreateAttribute("mjc:damping", Sdf.ValueTypeNames.Double, True).Set(0.23) + # PhysX joint properties take precedence over the MuJoCo fallback. + joint.GetPrim().CreateAttribute("mjc:armature", Sdf.ValueTypeNames.Double, True).Set(0.12) + joint.GetPrim().CreateAttribute("physxJoint:armature", Sdf.ValueTypeNames.Float, True).Set(0.21) physics_cfg = NewtonCfg(solver_cfg=solver_cfg, load_visual_shapes=False) monkeypatch.setattr( @@ -845,6 +803,7 @@ def test_production_imports_scope_mujoco_joint_properties( assert model.joint_friction.numpy()[-1] == pytest.approx(expected_friction) assert model.joint_damping.numpy()[-1] == pytest.approx(expected_damping) + assert model.joint_armature.numpy()[-1] == pytest.approx(0.21) @pytest.mark.parametrize( @@ -854,14 +813,7 @@ def test_production_imports_scope_mujoco_joint_properties( pytest.param(NewtonFeatherstoneManager, False, id="featherstone"), ], ) -@pytest.mark.parametrize( - "author_newton_values", - [ - pytest.param(False, id="physx-over-mjc"), - pytest.param(True, id="newton-over-physx-over-mjc"), - ], -) -def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco, author_newton_values): +def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco): """Resolver precedence and MuJoCo fallback selection follow active solver needs.""" from pxr import Sdf, Usd, UsdGeom, UsdPhysics @@ -882,9 +834,8 @@ def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco, auth joint_prim.CreateAttribute("mjc:damping", Sdf.ValueTypeNames.Double, True).Set(0.23) joint_prim.CreateAttribute("mjc:armature", Sdf.ValueTypeNames.Double, True).Set(0.12) joint_prim.CreateAttribute("physxJoint:armature", Sdf.ValueTypeNames.Float, True).Set(0.21) - if author_newton_values: - joint_prim.CreateAttribute("newton:friction", Sdf.ValueTypeNames.Double, True).Set(0.31) - joint_prim.CreateAttribute("newton:armature", Sdf.ValueTypeNames.Double, True).Set(0.41) + joint_prim.CreateAttribute("newton:friction", Sdf.ValueTypeNames.Double, True).Set(0.31) + joint_prim.CreateAttribute("newton:armature", Sdf.ValueTypeNames.Double, True).Set(0.41) schema_resolvers = manager_cls._get_usd_import_schema_resolvers() builder = ModelBuilder() @@ -892,12 +843,10 @@ def test_schema_resolver_policy_and_precedence(manager_cls, imports_mujoco, auth builder.add_usd(stage, schema_resolvers=schema_resolvers) model = builder.finalize(device="cpu") - expected_friction = 0.31 if author_newton_values else (0.11 if imports_mujoco else 0.0) - expected_damping = 0.23 if imports_mujoco else 0.0 - expected_armature = 0.41 if author_newton_values else 0.21 - assert model.joint_friction.numpy()[-1] == pytest.approx(expected_friction) - assert model.joint_damping.numpy()[-1] == pytest.approx(expected_damping) - assert model.joint_armature.numpy()[-1] == pytest.approx(expected_armature) + # Newton values win over PhysX and MuJoCo; the MuJoCo fallback fills only unset fields. + assert model.joint_friction.numpy()[-1] == pytest.approx(0.31) + assert model.joint_damping.numpy()[-1] == pytest.approx(0.23 if imports_mujoco else 0.0) + assert model.joint_armature.numpy()[-1] == pytest.approx(0.41) @pytest.mark.parametrize("project_outside", [True, False]) @@ -1286,29 +1235,10 @@ def reset(self, state, world_mask=None, flags=0): # --------------------------------------------------------------------------- -# Manager class hierarchy and factory contracts +# Manager lifecycle contracts # --------------------------------------------------------------------------- -@pytest.mark.parametrize( - "manager", - [ - NewtonMJWarpManager, - NewtonXPBDManager, - NewtonVBDManager, - NewtonFeatherstoneManager, - NewtonKaminoManager, - NewtonMPMManager, - ], -) -def test_subclass_of_newton_manager(manager): - """All concrete managers inherit from :class:`NewtonManager`.""" - assert issubclass(manager, NewtonManager) - # Subclasses must override the abstract factory. - assert manager._build_solver is not NewtonManager._build_solver - assert manager._create_solver is not NewtonManager._create_solver - - def test_clear_resets_rigid_body_force_capability(monkeypatch): """Teardown clears the canonical solver capability without subclass shadowing.""" monkeypatch.setattr(NewtonManager, "_supports_rigid_body_force_input", True) @@ -1429,36 +1359,6 @@ def build_solver_with_actuator_mode(cls, model, solver_cfg): assert events == ["body", "ready", *expected_events] * 2 -def test_abstract_build_solver_raises(): - """Calling :meth:`_build_solver` on the abstract base raises.""" - with pytest.raises(NotImplementedError): - NewtonManager._build_solver(model=None, solver_cfg=NewtonSolverCfg()) - - -def test_abstract_create_solver_raises(): - """Calling :meth:`_create_solver` on the base manager raises.""" - with pytest.raises(NotImplementedError): - NewtonManager._create_solver(model=None, solver_cfg=NewtonSolverCfg()) - - -@pytest.mark.parametrize( - "manager", - [ - NewtonMJWarpManager, - NewtonXPBDManager, - NewtonVBDManager, - NewtonFeatherstoneManager, - NewtonKaminoManager, - NewtonMPMManager, - ], -) -def test_manager_name_starts_with_newton(manager): - """The ``"newton"`` prefix is required by :class:`InteractiveScene` and the - various backend factories that dispatch on ``physics_manager.__name__.lower()``. - """ - assert manager.__name__.lower().startswith("newton") - - # --------------------------------------------------------------------------- # End-to-end: build each solver via SimulationContext # --------------------------------------------------------------------------- @@ -1618,9 +1518,7 @@ def test_mjwarp_internal_contacts_with_collision_cfg_raises(): [ (8, 0, 0), # Feature disabled. (8, 2, 3), # Re-collide after substeps 2, 4, 6 (skip last). - (8, 4, 1), # Re-collide after substep 4 only. (8, 7, 1), # Re-collide after substep 7 only. - (8, 8, 0), # Gated off (>= num_substeps). ], ) def test_collision_decimation_invokes_mid_loop_collide(num_substeps, collision_decimation, expected_mid_loop_collides): @@ -1735,8 +1633,7 @@ def clear_forces(self): # --------------------------------------------------------------------------- -@pytest.mark.parametrize("num_steps", [1, 3]) -def test_reset_lands_in_state_0_after_odd_kamino_steps_without_cuda_graph(num_steps): +def test_reset_lands_in_state_0_after_odd_kamino_steps_without_cuda_graph(): """An env reset written through the data-layer binding lands in ``_state_0``. Kamino is double-buffered (``_use_single_state=False``), so each substep @@ -1759,6 +1656,7 @@ def test_reset_lands_in_state_0_after_odd_kamino_steps_without_cuda_graph(num_st the sentinel lands in ``_state_1`` instead, so the final assertion fails. """ sentinel = 1.2345 + num_steps = 1 # any odd count leaves a swapped buffer without the copy-on-last sim_cfg = SimulationCfg( dt=1.0 / 120.0, device="cuda:0", diff --git a/source/isaaclab_newton/test/physics/test_newton_solver_reset.py b/source/isaaclab_newton/test/physics/test_newton_solver_reset.py index 9694220f48db..fd4ac657832b 100644 --- a/source/isaaclab_newton/test/physics/test_newton_solver_reset.py +++ b/source/isaaclab_newton/test/physics/test_newton_solver_reset.py @@ -27,6 +27,7 @@ from isaaclab.assets import ArticulationCfg from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate from isaaclab.sim import SimulationCfg, build_simulation_context +from isaaclab.test.utils import DeviceScope, test_devices from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -59,7 +60,7 @@ def _generate_single_joint_articulations(num_articulations: int, device: str) -> return articulation -@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.DEFAULT_CUDA)) def test_env_reset_clears_selected_mjwarp_solver_internals(device): """An env reset clears the flagged world's MuJoCo warm-start history and keeps the others. diff --git a/source/isaaclab_newton/test/physics/test_vbd_core.py b/source/isaaclab_newton/test/physics/test_vbd_core.py index 3915142a275a..78d56c2342ff 100644 --- a/source/isaaclab_newton/test/physics/test_vbd_core.py +++ b/source/isaaclab_newton/test/physics/test_vbd_core.py @@ -16,18 +16,19 @@ from isaaclab.sim import SimulationContext +# The soft-contact and simulation axes are independent, so each value is covered once. @pytest.mark.parametrize( - ("soft_contact_cfg", "expected"), + ("soft_contact_cfg", "expected", "simulation"), [ - pytest.param(None, (7.0, 8.0, 9.0), id="preserve"), + pytest.param(None, (7.0, 8.0, 9.0), True, id="preserve-physics"), pytest.param( NewtonSoftContactCfg(soft_contact_ke=11.0, soft_contact_kd=12.0, soft_contact_mu=13.0), (11.0, 12.0, 13.0), - id="override", + False, + id="override-render", ), ], ) -@pytest.mark.parametrize("simulation", [False, True], ids=["render", "physics"]) def test_soft_contact_cfg_updates_finalized_model(soft_contact_cfg, expected, simulation): """Registry construction shares the builder and applies model options before native allocation.""" state_values = [] @@ -185,22 +186,25 @@ def color(self, *, balance_colors): assert events == [("color", False), "start"] -@pytest.mark.parametrize("external_rigid_solver", [False, True]) -def test_vbd_solver_force_input_capability(monkeypatch, external_rigid_solver): - """VBD accepts rigid forces only when it integrates rigid bodies.""" +def test_vbd_solver_force_input_capability(monkeypatch): + """VBD rejects rigid forces when an external solver integrates rigid bodies. + + The default (VBD integrates rigid bodies itself) is covered end to end by + ``test_initialize_solver_populates_canonical_state``. + """ physics = importlib.import_module("isaaclab_newton.physics") solver = object() monkeypatch.setattr(physics.NewtonVBDManager, "_create_solver", lambda model, cfg: solver) monkeypatch.setattr(NewtonManager, "_solver", None) monkeypatch.setattr(NewtonManager, "_use_single_state", True) monkeypatch.setattr(NewtonManager, "_needs_collision_pipeline", False) - monkeypatch.setattr(NewtonManager, "_supports_rigid_body_force_input", False) + monkeypatch.setattr(NewtonManager, "_supports_rigid_body_force_input", True) - solver_cfg = physics.VBDSolverCfg(integrate_with_external_rigid_solver=external_rigid_solver) + solver_cfg = physics.VBDSolverCfg(integrate_with_external_rigid_solver=True) physics.NewtonVBDManager._build_solver(object(), solver_cfg) assert NewtonManager._solver is solver - assert NewtonManager._supports_rigid_body_force_input is not external_rigid_solver + assert NewtonManager._supports_rigid_body_force_input is False def test_vbd_rebuilds_particle_bvh_before_physics_step(monkeypatch): diff --git a/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py b/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py index b2deb4f91220..f1501049cc62 100644 --- a/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py +++ b/source/isaaclab_newton/test/renderers/test_newton_warp_renderer_rigid_object_rendering.py @@ -23,6 +23,7 @@ from isaaclab_newton.renderers.newton_warp_renderer import NewtonWarpRenderer from isaaclab.sim import build_simulation_context +from isaaclab.test.utils import DeviceScope, test_devices _CONTRACT_DIR = Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "renderers" if str(_CONTRACT_DIR) not in sys.path: @@ -48,7 +49,7 @@ def test_kinematic_rigid_object_scale_and_pose_are_rendered() -> None: ) -@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.DEFAULT_CUDA)) def test_intrinsic_updates_preserve_shared_ray_storage(device): """Uniform updates reuse the ray field; nonuniform calibration fails without changing rays.""" data = SimpleNamespace( diff --git a/source/isaaclab_newton/test/renderers/test_segmentation.py b/source/isaaclab_newton/test/renderers/test_segmentation.py index af5368ba48b6..1b9b45b054f1 100644 --- a/source/isaaclab_newton/test/renderers/test_segmentation.py +++ b/source/isaaclab_newton/test/renderers/test_segmentation.py @@ -101,28 +101,6 @@ def test_instance_segmentation_groups_by_labelled_ancestor(): assert mapping.info["idToSemantics"][ids[0]] == {"class": "cartpole"} -def test_colorize_info_keys_are_color_tuples(): - """With colorization, info keys are ``(r, g, b, a)`` color tuples and a color palette is built.""" - stage, shape_paths = _scene() - mapper = NewtonSegmentationMapper(_model(shape_paths), stage, _cfg(), _empty_clone_plan()) - mapper.build_mapping("semantic_segmentation", colorize=True) - mapping = mapper.get_mapping("semantic_segmentation", colorize=True) - - assert mapping.shape_to_color is not None - assert random_color_from_id(BACKGROUND_ID) in mapping.info["idToLabels"] - assert random_color_from_id(UNLABELLED_ID) in mapping.info["idToLabels"] - - -def test_semantic_filter_excludes_non_matching_types(): - """A filter restricted to an absent type marks every shape UNLABELLED.""" - stage, shape_paths = _scene() - mapper = NewtonSegmentationMapper(_model(shape_paths), stage, _cfg(semantic_filter=["shape"]), _empty_clone_plan()) - mapper.build_mapping("semantic_segmentation", colorize=False) - mapping = mapper.get_mapping("semantic_segmentation", colorize=False) - - assert mapping.shape_to_id.numpy().tolist() == [UNLABELLED_ID] * len(shape_paths) - - def test_semantic_filter_comma_separated_type_clauses(): """Comma-separated ``type:label`` pairs within one semicolon group each match independently. @@ -266,7 +244,10 @@ def test_prototype_fallback_respects_semantic_filter(): def test_semantic_segmentation_mapping_overrides_color(): - """``semantic_segmentation_mapping`` forces the class color and its info key.""" + """``semantic_segmentation_mapping`` forces the class color and its info key. + + With colorization, info keys are ``(r, g, b, a)`` color tuples and a color palette is built. + """ stage, shape_paths = _scene() override = (255, 36, 66, 255) mapper = NewtonSegmentationMapper( @@ -278,6 +259,9 @@ def test_semantic_segmentation_mapping_overrides_color(): mapper.build_mapping("semantic_segmentation", colorize=True) mapping = mapper.get_mapping("semantic_segmentation", colorize=True) + assert mapping.shape_to_color is not None + assert random_color_from_id(BACKGROUND_ID) in mapping.info["idToLabels"] + assert random_color_from_id(UNLABELLED_ID) in mapping.info["idToLabels"] # The cartpole class id must be colored with the override, and keyed by it in idToLabels. assert override in mapping.info["idToLabels"] assert mapping.info["idToLabels"][override] == {"class": "cartpole"} diff --git a/source/isaaclab_newton/test/renderers/test_visual_material.py b/source/isaaclab_newton/test/renderers/test_visual_material.py index 2bd50dcdece5..e553571b405f 100644 --- a/source/isaaclab_newton/test/renderers/test_visual_material.py +++ b/source/isaaclab_newton/test/renderers/test_visual_material.py @@ -7,6 +7,7 @@ import torch import warp as wp +from isaaclab_newton.physics import NewtonManager from isaaclab_newton.renderers.newton_warp_renderer import NewtonWarpRenderer from isaaclab_newton.renderers.visual_material import ( VisualMaterialWriter, @@ -142,4 +143,4 @@ def test_shape_writer_samples_each_body_and_selected_environment_independently() def test_newton_renderer_exposes_shared_writer_factory() -> None: renderer = object.__new__(NewtonWarpRenderer) - assert renderer.visual_material_writer.__func__.__name__ == "create_visual_material_writer" + assert renderer.visual_material_writer == NewtonManager.create_visual_material_writer diff --git a/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py index 79e056feef12..10e5a386ecd0 100644 --- a/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py +++ b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py @@ -98,7 +98,32 @@ def _fisheye_distortion(apply_lens_distortion: bool) -> OpenCvFisheyeDistortionC ) -def _expected_pinhole_ground_distance(px: int, py: int) -> float: +def _invert_monotonic(forward, target: float) -> float: + """Bisect ``forward(x) == target`` on ``[0, target]`` for a monotonic map with ``forward(x) >= x``.""" + lower, upper = 0.0, target + for _ in range(64): + middle = 0.5 * (lower + upper) + if forward(middle) < target: + lower = middle + else: + upper = middle + return 0.5 * (lower + upper) + + +def _pinhole_undistorted_radius(radius_d: float) -> float: + """Invert the pinhole radial model ``r_d = r_u * (1 + k1 * r_u**2)``.""" + return _invert_monotonic(lambda r: r * (1.0 + _PINHOLE_K1 * r**2), radius_d) + + +def _fisheye_undistorted_radius(radius_d: float) -> float: + """Invert the equidistant model ``theta_d = theta * (1 + k1 theta^2 + k2 theta^4)``; return ``tan(theta)``.""" + k1, k2 = _FISHEYE_COEFFS["k1"], _FISHEYE_COEFFS["k2"] + # k3 = k4 = 0; the map is monotonic over the image's field of view. + theta = _invert_monotonic(lambda t: t * (1.0 + k1 * t**2 + k2 * t**4), radius_d) + return float(np.tan(theta)) + + +def _expected_ground_distance(px: int, py: int, undistorted_radius) -> float: """Compute the expected distorted-ray distance to the ground plane [m].""" u = px + 0.5 v = py + 0.5 @@ -107,14 +132,7 @@ def _expected_pinhole_ground_distance(px: int, py: int) -> float: radius_d = float(np.hypot(x_d, y_d)) if radius_d > 0.0: - lower, upper = 0.0, radius_d - for _ in range(64): - radius_u = 0.5 * (lower + upper) - if radius_u * (1.0 + _PINHOLE_K1 * radius_u**2) < radius_d: - lower = radius_u - else: - upper = radius_u - scale = (0.5 * (lower + upper)) / radius_d + scale = undistorted_radius(radius_d) / radius_d x_u, y_u = x_d * scale, y_d * scale else: x_u, y_u = 0.0, 0.0 @@ -129,6 +147,7 @@ def _expected_pinhole_ground_distance(px: int, py: int) -> float: x_axis /= np.linalg.norm(x_axis) y_axis = np.cross(z_axis, x_axis) ray_world = np.column_stack((x_axis, y_axis, z_axis)) @ ray_camera + assert ray_world[2] < 0.0, "the sampled pixel must look at the ground" return float(-eye[2] / ray_world[2]) @@ -165,31 +184,26 @@ def _mean_abs_distance_diff(a: np.ndarray, b: np.ndarray) -> float: def test_opencv_distortion_changes_newton_render(): - """The Newton renderer must render the distorted and zero-coefficient cameras meaningfully differently.""" + """The Newton renderer applies the OpenCV pinhole and fisheye models and honors ``apply_lens_distortion``. + + Both distorted renders are checked against the analytic ground distance of the distorted ray at sample + pixels, and differ well beyond render noise from the undistorted pinhole reference. + """ distorted = _render_distance(_pinhole_distortion(True)) reference = _render_distance(_pinhole_distortion(False)) + fisheye = _render_distance(_fisheye_distortion(True)) - assert distorted.shape == (HEIGHT, WIDTH, 1) - assert np.isfinite(distorted).mean() > 0.9 - assert np.isfinite(reference).mean() > 0.9 + for image in (distorted, reference, fisheye): + assert image.shape == (HEIGHT, WIDTH, 1) + assert np.isfinite(image).mean() > 0.9 mean_abs_diff = _mean_abs_distance_diff(distorted, reference) assert mean_abs_diff > 0.01, f"distorted vs reference distance maps differ by only {mean_abs_diff:.4f} m" for px, py in ((0, 0), (WIDTH // 2, HEIGHT // 2), (WIDTH - 1, HEIGHT - 1)): - assert distorted[py, px, 0] == pytest.approx(_expected_pinhole_ground_distance(px, py), abs=2e-3) - - -def test_opencv_fisheye_distortion_renders_through_newton(): - """The Newton renderer honors the OpenCV fisheye model: its render differs meaningfully from the pinhole. - - The same calibrated camera is rendered under the OpenCV fisheye model and under an undistorted - pinhole. The fisheye equidistant projection bends the rays, so the two distance maps must differ - well beyond render noise. - """ - fisheye = _render_distance(_fisheye_distortion(True)) - pinhole = _render_distance(_pinhole_distortion(False)) + expected = _expected_ground_distance(px, py, _pinhole_undistorted_radius) + assert distorted[py, px, 0] == pytest.approx(expected, abs=2e-3) - assert fisheye.shape == (HEIGHT, WIDTH, 1) - assert np.isfinite(fisheye).mean() > 0.9 - assert np.isfinite(pinhole).mean() > 0.9 - mean_abs_diff = _mean_abs_distance_diff(fisheye, pinhole) + mean_abs_diff = _mean_abs_distance_diff(fisheye, reference) assert mean_abs_diff > 0.05, f"fisheye vs pinhole distance maps differ by only {mean_abs_diff:.4f} m" + for px, py in ((WIDTH // 4, 3 * HEIGHT // 4), (WIDTH // 2, HEIGHT // 2), (WIDTH - 1, HEIGHT - 1)): + expected = _expected_ground_distance(px, py, _fisheye_undistorted_radius) + assert fisheye[py, px, 0] == pytest.approx(expected, abs=2e-3) diff --git a/source/isaaclab_newton/test/sensors/test_contact_sensor.py b/source/isaaclab_newton/test/sensors/test_contact_sensor.py index 5242f0eb71c3..9f160d276b97 100644 --- a/source/isaaclab_newton/test/sensors/test_contact_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_contact_sensor.py @@ -25,7 +25,6 @@ sys.path.insert(0, str(Path(__file__).resolve().parents[1])) import math -import re import pytest import torch @@ -220,13 +219,31 @@ def test_contact_lifecycle(device: str, use_mujoco_contacts: bool, shape_type: S assert no_contact_detected[env_idx], f"Env {env_idx}: Contact should stop after lift." -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("use_mujoco_contacts", COLLISION_PIPELINES) -@pytest.mark.parametrize( - "shape_type", - [ShapeType.SPHERE, ShapeType.MESH_CAPSULE], - ids=["sphere", "mesh_capsule"], -) +def _rotated_horizontal_collision_cases() -> list: + """Cover each shape and pipeline pair once while rotating through the devices. + + The last row pairs MuJoCo contacts with the mesh capsule on the second device (a GPU on the + default runtime) so the MuJoCo-Warp #1527 offset branch stays covered. + """ + devices = test_devices() + rows = [ + (ShapeType.SPHERE, COLLISION_PIPELINES[1]), + (ShapeType.SPHERE, COLLISION_PIPELINES[0]), + (ShapeType.MESH_CAPSULE, COLLISION_PIPELINES[0]), + (ShapeType.MESH_CAPSULE, COLLISION_PIPELINES[1]), + ] + cases = [] + for i, (shape_type, pipeline) in enumerate(rows): + device = devices[i % len(devices)] + cases.append( + pytest.param( + device, pipeline.values[0], shape_type, id=f"{shape_type_to_str(shape_type)}-{pipeline.id}-{device}" + ) + ) + return cases + + +@pytest.mark.parametrize("device, use_mujoco_contacts, shape_type", _rotated_horizontal_collision_cases()) def test_horizontal_collision_detects_contact(device: str, use_mujoco_contacts: bool, shape_type: ShapeType): """Test horizontal collision detection with varied velocities and separations. @@ -334,8 +351,14 @@ def test_horizontal_collision_detects_contact(device: str, use_mujoco_contacts: # =================================================================== -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("use_mujoco_contacts", COLLISION_PIPELINES) +# The pipelines select distinct contact paths; the device selects none, so it rotates across them. +@pytest.mark.parametrize( + "device, use_mujoco_contacts", + [ + pytest.param(device, pipeline.values[0], id=f"{pipeline.id}-{device}") + for device, pipeline in zip(test_devices() * len(COLLISION_PIPELINES), COLLISION_PIPELINES) + ], +) def test_resting_object_contact_force(device: str, use_mujoco_contacts: bool): """Test that resting object contact force equals weight and points upward. @@ -437,83 +460,6 @@ def test_resting_object_contact_force(device: str, use_mujoco_contacts: bool): assert not errs, "\n".join(errs) -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize("use_mujoco_contacts", COLLISION_PIPELINES) -def test_higher_drop_produces_larger_impact_force(device: str, use_mujoco_contacts: bool): - """Test that dropping from higher produces larger peak impact force. - - 8 environments with heights from 0.3m to 3.0m. - - Verifies: - - Peak impact force generally increases with height - - Overall trend: highest/lowest force ratio > 1.5 - """ - num_envs = 8 - min_height, max_height = 0.3, 3.0 - drop_heights = [min_height + (max_height - min_height) * i / (num_envs - 1) for i in range(num_envs)] - gravity_mag = 9.81 - object_radius = 0.25 - - sim_cfg = make_sim_cfg(use_mujoco_contacts=use_mujoco_contacts, device=device, gravity=(0.0, 0.0, -gravity_mag)) - - with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.object_a = create_shape_cfg( - ShapeType.SPHERE, - "{ENV_REGEX_NS}/Sphere", - pos=(0.0, 0.0, max_height + object_radius), - disable_gravity=False, - activate_contact_sensors=True, - ) - scene_cfg.contact_sensor_a = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Sphere", - update_period=0.0, - history_length=1, - ) - - scene = InteractiveScene(scene_cfg) - sim.reset() - scene.reset() - - obj: RigidObject = scene["object_a"] - contact_sensor: ContactSensor = scene["contact_sensor_a"] - - root_pose = obj.data.root_link_pose_w.torch.clone() - for env_idx in range(num_envs): - root_pose[env_idx, 2] = drop_heights[env_idx] + object_radius - obj.write_root_pose_to_sim_index(root_pose=root_pose) - - total_steps = int(((2 * max_height / gravity_mag) ** 0.5 + 0.5) / SIM_DT) - peak_forces = [0.0] * num_envs - contact_detected = [False] * num_envs - - for _ in range(total_steps): - perform_sim_step(sim, scene, SIM_DT) - force_magnitudes = torch.norm(contact_sensor.data.net_normal_forces_w.torch, dim=-1) - for env_idx in range(num_envs): - f = force_magnitudes[env_idx].max().item() - if f > 0.1: - contact_detected[env_idx] = True - peak_forces[env_idx] = max(peak_forces[env_idx], f) - - for env_idx in range(num_envs): - assert contact_detected[env_idx], f"Env {env_idx} (h={drop_heights[env_idx]:.2f}m): No contact" - - violations = [] - for i in range(num_envs - 1): - if peak_forces[i + 1] < peak_forces[i] * 0.95: - violations.append( - f"Env {i} (h={drop_heights[i]:.2f}m, F={peak_forces[i]:.2f}N) -> " - f"Env {i + 1} (h={drop_heights[i + 1]:.2f}m, F={peak_forces[i + 1]:.2f}N)" - ) - assert len(violations) <= 2, "Peak force should increase with height. Violations:\n" + "\n".join(violations) - - force_ratio = peak_forces[-1] / peak_forces[0] if peak_forces[0] > 0 else 0 - assert force_ratio > 1.5, f"Force ratio (highest/lowest) should be > 1.5. Got {force_ratio:.2f}" - - # =================================================================== # Priority 3: Filtering # =================================================================== @@ -535,22 +481,29 @@ def test_higher_drop_produces_larger_impact_force(device: str, use_mujoco_contac pytest.param(True, id="mujoco_contacts"), ], ) -def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): - """Test that filter_prim_paths_expr filters contacts and enables normal_force_matrix_w. +def test_filtered_contact_forces_and_points(device: str, use_mujoco_contacts: bool): + """Test that filter_prim_paths_expr enables per-filter forces and average contact positions. - Object A rests on ground, Object B stacked on A. - Sensor on A is filtered for B only (not ground). + Object A rests on ground, Object B stacked on A, Object C off to the side (never touching A). + Sensor on A is filtered for B and C with friction and contact point tracking enabled. Verifies: - - normal_force_matrix_w reports only filtered contact (A-B) + - normal_force_matrix_w reports only filtered contact: B-on-A ~ (0, 0, -m_B g), C column zero - net_normal_forces_w reports normal contact against all objects (ground + B) - the normal and friction outputs reconstruct Newton's total-force outputs - - filtered normal force is smaller than net normal force (ground contact excluded from matrix) + - contact_pos_w has shape (num_envs, num_sensors, num_filter_objects, 3) + - The B column reports a position at the A-B interface (top face of A, centered under B) + - The C column is NaN (no contact) + - Regression for #4970: ``scene.reset(env_ids)`` zeroes the reset env's forces and contact + positions without a physics step, and leaves the other envs untouched + - The B column returns to NaN once B is separated from A """ settle_steps = 240 num_envs = 4 - mass_b = 2.0 gravity = 9.81 + size_a = (0.5, 0.5, 0.3) + pos_a = (0.0, 0.0, 0.2) + mass_b = 2.0 expected_force_from_b = mass_b * gravity sim_cfg = make_sim_cfg(use_mujoco_contacts=use_mujoco_contacts, device=device, gravity=(0.0, 0.0, -gravity)) @@ -560,19 +513,18 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) - rigid_props_a = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=0.5, angular_damping=0.5)] + rigid_props = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=0.5, angular_damping=0.5)] scene_cfg.object_a = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/ObjectA", spawn=sim_utils.CuboidCfg( - size=(0.5, 0.5, 0.3), - rigid_props=rigid_props_a, + size=size_a, + rigid_props=rigid_props, collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), mass_props=sim_utils.MassCfg(mass=5.0), activate_contact_sensors=True, ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.2)), + init_state=RigidObjectCfg.InitialStateCfg(pos=pos_a), ) - rigid_props_b = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=2.0, angular_damping=2.0)] scene_cfg.object_b = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/ObjectB", @@ -585,13 +537,25 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.55)), ) + scene_cfg.object_c = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/ObjectC", + spawn=sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + rigid_props=rigid_props, + collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), + mass_props=sim_utils.MassCfg(mass=2.0), + activate_contact_sensors=True, + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(2.0, 0.0, 0.15)), + ) scene_cfg.contact_sensor_a = ContactSensorCfg( prim_path="{ENV_REGEX_NS}/ObjectA", update_period=0.0, history_length=0, - filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB"], + filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB", "{ENV_REGEX_NS}/ObjectC"], track_friction_forces=True, + track_contact_points=True, ) scene = InteractiveScene(scene_cfg) @@ -599,31 +563,44 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): scene.reset() contact_sensor: ContactSensor = scene["contact_sensor_a"] + object_a: RigidObject = scene["object_a"] + object_b: RigidObject = scene["object_b"] + + assert contact_sensor.filter_object_names == ["ObjectB", "ObjectC"], ( + f"unexpected filter_object_names: {contact_sensor.filter_object_names}" + ) + col_b = contact_sensor.filter_object_names.index("ObjectB") + col_c = contact_sensor.filter_object_names.index("ObjectC") # Average over the last `avg_window` ticks to reject per-step solver oscillation. avg_window = 20 + pos_samples: list[torch.Tensor] = [] matrix_samples: list[torch.Tensor] = [] net_samples: list[torch.Tensor] = [] for step in range(settle_steps): perform_sim_step(sim, scene, SIM_DT) if step >= settle_steps - avg_window: + pos_raw = contact_sensor.data.contact_pos_w matrix_raw = contact_sensor.data.normal_force_matrix_w matrix_history_raw = contact_sensor.data.normal_force_matrix_w_history net_raw = contact_sensor.data.net_normal_forces_w - if not matrix_samples: + if not pos_samples: + assert pos_raw is not None, "contact_pos_w should not be None when tracking is enabled" + assert pos_raw.torch.shape == (num_envs, 1, 2, 3), f"unexpected shape: {pos_raw.torch.shape}" assert matrix_raw is not None, "normal_force_matrix_w should not be None when filter is set" assert matrix_history_raw is not None, ( "normal_force_matrix_w_history should not be None when filter is set" ) - assert matrix_history_raw.torch.shape == (num_envs, 1, 1, 1, 3) + assert matrix_history_raw.torch.shape == (num_envs, 1, 1, 2, 3) torch.testing.assert_close(matrix_history_raw.torch[:, 0], matrix_raw.torch) assert contact_sensor.data.net_friction_forces_w is not None assert contact_sensor.data.friction_force_matrix_w is not None + pos_samples.append(pos_raw.torch.clone()) matrix_samples.append(matrix_raw.torch.clone()) net_samples.append(net_raw.torch.clone()) total_force = wp.to_torch(contact_sensor.contact_view.total_force).reshape(num_envs, 1, 3) - total_force_matrix = wp.to_torch(contact_sensor.contact_view.force_matrix).reshape(num_envs, 1, 1, 3) + total_force_matrix = wp.to_torch(contact_sensor.contact_view.force_matrix).reshape(num_envs, 1, 2, 3) torch.testing.assert_close( contact_sensor.data.net_normal_forces_w.torch + contact_sensor.data.net_friction_forces_w.torch, total_force, @@ -635,12 +612,11 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): force_matrix = torch.stack(matrix_samples).mean(dim=0) net_forces = torch.stack(net_samples).mean(dim=0) - expected_b_on_a = torch.tensor([0.0, 0.0, -expected_force_from_b], device=device) tolerance = 0.05 * expected_force_from_b errs: list[str] = [] for env_idx in range(num_envs): - b_on_a = force_matrix[env_idx, 0, 0] + b_on_a = force_matrix[env_idx, 0, col_b] net_contact = net_forces[env_idx, 0] error = torch.norm(b_on_a - expected_b_on_a).item() @@ -654,119 +630,10 @@ def test_filter_enables_force_matrix(device: str, use_mujoco_contacts: bool): f"Env {env_idx}: |B-on-A| should be < |net contact|. " f"B-on-A: {b_on_a.tolist()}, Net: {net_contact.tolist()}" ) + if torch.norm(force_matrix[env_idx, 0, col_c]).item() != 0.0: + errs.append(f"Env {env_idx}: C column should report no force. Got {force_matrix[env_idx, 0, col_c]}") assert not errs, "\n".join(errs) - -@pytest.mark.parametrize("device", test_devices()) -@pytest.mark.parametrize( - "use_mujoco_contacts", - [ - pytest.param( - False, - id="newton_contacts", - marks=pytest.mark.skip( - reason=( - "Newton normal_force_matrix_w is non-deterministic across hardware (reports 0 or inflated values)" - ), - ), - ), - pytest.param(True, id="mujoco_contacts"), - ], -) -def test_track_contact_points_reports_average_position(device: str, use_mujoco_contacts: bool): - """Test that track_contact_points reports the average contact position per filter object. - - Object A rests on ground, Object B stacked on A, Object C off to the side (never touching A). - Sensor on A is filtered for B and C with contact point tracking enabled. - - Verifies: - - contact_pos_w is available and has shape (num_envs, num_sensors, num_filter_objects, 3) - - The B column reports a position at the A-B interface (top face of A, centered under B) - - The C column is NaN (no contact) - - The B column returns to NaN once B is separated from A - """ - settle_steps = 240 - num_envs = 4 - gravity = 9.81 - size_a = (0.5, 0.5, 0.3) - pos_a = (0.0, 0.0, 0.2) - - sim_cfg = make_sim_cfg(use_mujoco_contacts=use_mujoco_contacts, device=device, gravity=(0.0, 0.0, -gravity)) - - with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) - - rigid_props = [PhysxRigidBodyCfg(disable_gravity=False, linear_damping=0.5, angular_damping=0.5)] - scene_cfg.object_a = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/ObjectA", - spawn=sim_utils.CuboidCfg( - size=size_a, - rigid_props=rigid_props, - collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), - mass_props=sim_utils.MassCfg(mass=5.0), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=pos_a), - ) - scene_cfg.object_b = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/ObjectB", - spawn=sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - rigid_props=rigid_props, - collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), - mass_props=sim_utils.MassCfg(mass=2.0), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.55)), - ) - scene_cfg.object_c = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/ObjectC", - spawn=sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - rigid_props=rigid_props, - collision_props=sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True), - mass_props=sim_utils.MassCfg(mass=2.0), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(2.0, 0.0, 0.15)), - ) - - scene_cfg.contact_sensor_a = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/ObjectA", - update_period=0.0, - history_length=1, - filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB", "{ENV_REGEX_NS}/ObjectC"], - track_contact_points=True, - ) - - scene = InteractiveScene(scene_cfg) - sim.reset() - scene.reset() - - contact_sensor: ContactSensor = scene["contact_sensor_a"] - object_a: RigidObject = scene["object_a"] - object_b: RigidObject = scene["object_b"] - - assert contact_sensor.filter_object_names == ["ObjectB", "ObjectC"], ( - f"unexpected filter_object_names: {contact_sensor.filter_object_names}" - ) - col_b = contact_sensor.filter_object_names.index("ObjectB") - col_c = contact_sensor.filter_object_names.index("ObjectC") - - # Average over the last `avg_window` ticks to reject per-step solver oscillation. - avg_window = 20 - pos_samples: list[torch.Tensor] = [] - for step in range(settle_steps): - perform_sim_step(sim, scene, SIM_DT) - if step >= settle_steps - avg_window: - pos_raw = contact_sensor.data.contact_pos_w - if not pos_samples: - assert pos_raw is not None, "contact_pos_w should not be None when tracking is enabled" - assert pos_raw.torch.shape == (num_envs, 1, 2, 3), f"unexpected shape: {pos_raw.torch.shape}" - pos_samples.append(pos_raw.torch.clone()) - stacked = torch.stack(pos_samples) assert torch.isnan(stacked[:, :, :, col_c]).all(), "C column should be NaN (no contact with ObjectC)" assert torch.isfinite(stacked[:, :, :, col_b]).all(), "B column should be finite (in contact with ObjectB)" @@ -789,6 +656,23 @@ def test_track_contact_points_reports_average_position(device: str, use_mujoco_c ) assert not errs, "\n".join(errs) + # Mimic ``ManagerBasedRLEnv._reset_idx``: reset env 0 inside a step, without a physics step. + pre_reset_forces = contact_sensor.data.net_normal_forces_w.torch.clone() + pre_reset_force_mag = torch.linalg.norm(pre_reset_forces[0], dim=-1).item() + assert pre_reset_force_mag > 1.0, f"Expected non-zero contact force before reset; got {pre_reset_force_mag!r}" + scene.reset(env_ids=torch.tensor([0], device=object_a.device)) + post_reset_forces = contact_sensor.data.net_normal_forces_w.torch + post_reset_force_mag = torch.linalg.norm(post_reset_forces[0], dim=-1).item() + assert post_reset_force_mag == 0.0, ( + "Contact sensor returned stale pre-reset data after scene.reset(): " + f"got {post_reset_force_mag}, expected 0.0 (pre-reset value was {pre_reset_force_mag})." + ) + post_reset_contact_pos = contact_sensor.data.contact_pos_w.torch[0] + assert torch.isnan(post_reset_contact_pos).all(), ( + f"contact_pos_w should reset to NaN after scene.reset(); got {post_reset_contact_pos.tolist()}" + ) + torch.testing.assert_close(post_reset_forces[1:], pre_reset_forces[1:]) + # Separate B from A and verify the B column returns to NaN (not the last contact position). lifted_pose = object_b.data.root_link_pose_w.torch.clone() lifted_pose[:, 2] += 2.0 @@ -818,7 +702,6 @@ def test_track_contact_points_reports_average_position(device: str, use_mujoco_c } -@pytest.mark.parametrize("device", test_devices()) @pytest.mark.parametrize( "use_mujoco_contacts", [ @@ -832,11 +715,12 @@ def test_track_contact_points_reports_average_position(device: str, use_mujoco_c pytest.param(True, id="mujoco_contacts"), ], ) +# The drop shape selects the collision path; the device selects none, so it rotates across the shapes. @pytest.mark.parametrize( - "drop_shape", + "device, drop_shape", [ - pytest.param(ShapeType.SPHERE, id="sphere"), - pytest.param(ShapeType.MESH_BOX, id="mesh_box"), + pytest.param(test_devices()[-1], ShapeType.SPHERE, id=f"sphere-{test_devices()[-1]}"), + pytest.param(test_devices()[0], ShapeType.MESH_BOX, id=f"mesh_box-{test_devices()[0]}"), ], ) @flaky(max_runs=3, min_passes=1) @@ -1014,7 +898,8 @@ def _make_two_box_scene_cfg(num_envs: int) -> ContactSensorTestSceneCfg: return scene_cfg -@pytest.mark.parametrize("device", test_devices()) +# Sensor metadata is host-side bookkeeping, identical on every device. +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_sensor_metadata(device: str): """Verify sensor_names and filter_object_names match the underlying sensing and counterpart configuration across body-mode, body-mode-with-filter, and shape-mode. @@ -1067,75 +952,6 @@ def test_sensor_metadata(device: str): assert shape_sensor.filter_object_names == [] -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_no_stale_data_after_scene_reset(device: str): - """Regression for #4970: ``scene.reset(env_ids)`` must not surface pre-reset contact data (Newton). - - Mirrors the PhysX equivalent (``test_contact_sensor_no_stale_data_after_reset``). Reproduces the - ``ManagerBasedRLEnv._reset_idx`` flow where reset runs inside a step without a subsequent - physics step; the contact sensor's lazy ``data`` accessor must not refetch from the Newton - contact buffer here (it still reflects the previous step). - """ - sim_cfg = make_sim_cfg(use_mujoco_contacts=False, device=device, gravity=(0.0, 0.0, -9.81)) - with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - - scene_cfg = ContactSensorTestSceneCfg(num_envs=1, env_spacing=2.0, lazy_sensor_update=False) - scene_cfg.object_a = create_shape_cfg( - ShapeType.BOX, - "{ENV_REGEX_NS}/Object", - pos=(0.0, 0.0, 1.0), - disable_gravity=False, - activate_contact_sensors=True, - ) - # Object falls onto ObjectB so that contact_pos_w has real data to clear on reset. - scene_cfg.object_b = create_shape_cfg( - ShapeType.BOX, - "{ENV_REGEX_NS}/ObjectB", - pos=(0.0, 0.0, 0.25), - disable_gravity=False, - activate_contact_sensors=True, - ) - scene_cfg.contact_sensor_a = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Object", - update_period=0.0, - history_length=1, - filter_prim_paths_expr=["{ENV_REGEX_NS}/ObjectB"], - track_contact_points=True, - ) - - scene = InteractiveScene(scene_cfg) - sim.reset() - scene.reset() - - sensor: ContactSensor = scene["contact_sensor_a"] - obj: RigidObject = scene["object_a"] - - for _ in range(200): - perform_sim_step(sim, scene, SIM_DT) - - pre_reset_force_mag = torch.linalg.norm(sensor.data.net_normal_forces_w.torch, dim=-1).item() - assert pre_reset_force_mag > 1.0, f"Expected non-zero contact force before reset; got {pre_reset_force_mag!r}" - - # Mimic ``ManagerBasedRLEnv._reset_idx``: write post-reset asset state, then scene.reset(). - env_ids = torch.tensor([0], device=obj.device) - new_root_pose = torch.tensor([[0.0, 0.0, 2.0, 1.0, 0.0, 0.0, 0.0]], device=obj.device) - new_root_vel = torch.zeros((1, 6), device=obj.device) - obj.write_root_pose_to_sim_index(root_pose=new_root_pose, env_ids=env_ids) - obj.write_root_velocity_to_sim_index(root_velocity=new_root_vel, env_ids=env_ids) - scene.reset(env_ids=env_ids) - - post_reset_force_mag = torch.linalg.norm(sensor.data.net_normal_forces_w.torch, dim=-1).item() - assert post_reset_force_mag == 0.0, ( - "Contact sensor returned stale pre-reset data after scene.reset(): " - f"got {post_reset_force_mag}, expected 0.0 (pre-reset value was {pre_reset_force_mag})." - ) - post_reset_contact_pos = sensor.data.contact_pos_w.torch - assert torch.isnan(post_reset_contact_pos).all(), ( - f"contact_pos_w should reset to NaN after scene.reset(); got {post_reset_contact_pos.tolist()}" - ) - - # =================================================================== # Selector patterns # =================================================================== @@ -1169,11 +985,13 @@ def test_alternation_resolves(): def test_segment_wildcard_does_not_cross_path_separators(): - """``[^/]*`` selects one segment, so nested links stay out.""" + """``[^/]*`` selects one segment, so nested links stay out; shape selectors carry no rule of their own.""" selected = _select(f"{_NS}/Robot/[^/]*", _LABELS) assert "/World/envs/env_0/Robot/base" in selected assert "/World/envs/env_0/Robot/Geometry/panda_link0" not in selected + assert _select(f"{_NS}/Box[^/]*", _SHAPE_LABELS) == [] + assert _select(f"{_NS}/Box[^/]*/.*", _SHAPE_LABELS) == _SHAPE_LABELS def test_expression_list_selects_the_union(): @@ -1192,29 +1010,10 @@ def test_expression_list_selects_the_union(): ] -def test_shape_expressions_match_on_the_same_terms_as_body_expressions(): - """Shape selectors carry no rule of their own.""" - assert _select(f"{_NS}/Box[^/]*", _SHAPE_LABELS) == [] - assert _select(f"{_NS}/Box[^/]*/.*", _SHAPE_LABELS) == _SHAPE_LABELS - - -@pytest.mark.parametrize("expr", [None, []]) -def test_absent_selector_compiles_to_no_pattern(expr): - """Nothing requested means unfiltered, not empty.""" - assert _compile_label_pattern(expr) is None - - -def test_invalid_expression_raises_regex_error(): - """Reject malformed selector expressions at contact sensor construction.""" - with pytest.raises(re.error): - _compile_label_pattern("foo(") - - # The clock is accumulated identically on every device; the largest age bounds the drift. @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -@pytest.mark.parametrize("clock_age", [2.5, 30.0]) @pytest.mark.parametrize("history_length", [1, 0], ids=["substep_refresh", "lazy_refresh"]) -def test_first_transition_with_aged_clock(device: str, clock_age: float, history_length: int): +def test_first_transition_with_aged_clock(device: str, history_length: int): """Regression for #7283: transitions must still be reported once the sensor clock has aged. The sensor clock is a float32 accumulator whose rounding error grows with simulated time. On a @@ -1222,6 +1021,7 @@ def test_first_transition_with_aged_clock(device: str, clock_age: float, history tolerance of :meth:`compute_first_contact` has to absorb that error. A fixed 1e-8 tolerance is ~100x too small after a few seconds and silently drops touchdowns and lift-offs. """ + clock_age = 30.0 # With history, the sensor refreshes every physics step; without it, only when data is read. decimation = 1 if history_length > 0 else 4 poll_dt = decimation * SIM_DT diff --git a/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py b/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py index 6e4c8d3be774..1d01b29c2b0b 100644 --- a/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py +++ b/source/isaaclab_newton/test/sensors/test_contact_sensor_history.py @@ -19,14 +19,27 @@ def test_force_matrix_history_rolls_newest_first_and_honors_mask(): - """Test newest-first ordering without advancing masked environments.""" + """Test newest-first ordering of every history buffer without advancing masked environments. + + Newton's total-force properties return the totals without the base-class PhysX-limitation warning. + """ data = ContactSensorData() - data.create_buffers(2, 1, 1, 3, True, False, False, "cpu") + data.create_buffers(2, 1, 1, 3, True, False, False, "cpu", track_friction_forces=True) timestamp = wp.ones((2,), dtype=wp.float32, device="cpu") timestamp_last_update = wp.zeros((2,), dtype=wp.float32, device="cpu") + # Each buffer gets a distinct scale so a mis-wired history shows up as a wrong value. + buffers = { + "net_forces_w": 1.0, + "force_matrix_w": 10.0, + "normal_force_matrix_w": 100.0, + "net_friction_forces_w": 1000.0, + "friction_force_matrix_w": 10000.0, + "net_normal_forces_w": 100000.0, + } for value, mask_values in ((1.0, [True, True]), (2.0, [True, True]), (3.0, [True, False])): - wp.to_torch(data._normal_force_matrix_w).fill_(value) + for name, scale in buffers.items(): + wp.to_torch(getattr(data, f"_{name}")).fill_(scale * value) wp.launch( update_contact_sensor_kernel, dim=(2, 1), @@ -57,10 +70,20 @@ def test_force_matrix_history_rolls_newest_first_and_honors_mask(): device="cpu", ) - history = data.normal_force_matrix_w_history.torch - for env, values in enumerate(((3.0, 2.0, 1.0), (2.0, 1.0, 0.0))): - for history_index, value in enumerate(values): - torch.testing.assert_close(history[env, history_index], torch.full_like(history[env, history_index], value)) + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + histories = {name: getattr(data, f"{name}_history").torch for name in buffers} + torch.testing.assert_close(data.net_forces_w.torch, torch.full((2, 1, 3), 3.0)) + torch.testing.assert_close(data.force_matrix_w.torch, torch.full((2, 1, 1, 3), 30.0)) + assert data.friction_forces_w is data.net_friction_forces_w + assert not [item for item in caught if issubclass(item.category, UserWarning)] + + for name, scale in buffers.items(): + history = histories[name] + for env, values in enumerate(((3.0, 2.0, 1.0), (2.0, 1.0, 0.0))): + for history_index, value in enumerate(values): + expected = torch.full_like(history[env, history_index], scale * value) + torch.testing.assert_close(history[env, history_index], expected, msg=name) def test_copy_from_newton_decomposes_normal_and_friction_forces(): @@ -108,11 +131,11 @@ def test_copy_from_newton_decomposes_normal_and_friction_forces(): device="cpu", ) - torch.testing.assert_close(wp.to_torch(data._net_forces_w), torch.tensor([[[3.0, 4.0, 0.0]]])) + torch.testing.assert_close(data.net_forces_w.torch, torch.tensor([[[3.0, 4.0, 0.0]]])) torch.testing.assert_close(data.net_normal_forces_w.torch, torch.tensor([[[3.0, 0.0, 0.0]]])) torch.testing.assert_close(data.net_friction_forces_w.torch, torch.tensor([[[0.0, 4.0, 0.0]]])) torch.testing.assert_close( - wp.to_torch(data._force_matrix_w), + data.force_matrix_w.torch, torch.tensor([[[[1.0, 2.0, 0.0], [0.0, 0.0, 3.0]]]]), ) torch.testing.assert_close( @@ -123,76 +146,3 @@ def test_copy_from_newton_decomposes_normal_and_friction_forces(): data.friction_force_matrix_w.torch, torch.tensor([[[[0.0, 2.0, 0.0], [0.0, 0.0, 1.0]]]]), ) - - -def test_net_forces_w_is_newton_total_without_warning(): - """Test Newton total-force properties return totals without a warning.""" - data = ContactSensorData() - data.create_buffers(1, 1, 1, 1, True, False, False, "cpu", track_friction_forces=True) - wp.to_torch(data._net_forces_w).fill_(7.0) - wp.to_torch(data._force_matrix_w).fill_(8.0) - wp.to_torch(data._friction_force_matrix_w).fill_(9.0) - - with warnings.catch_warnings(record=True) as caught: - warnings.simplefilter("always") - total = data.net_forces_w - matrix = data.force_matrix_w - history = data.net_forces_w_history - matrix_history = data.force_matrix_w_history - friction = data.friction_forces_w - assert total is not None - assert matrix is not None - assert history is not None - assert matrix_history is not None - assert friction is data.net_friction_forces_w - assert not [item for item in caught if issubclass(item.category, UserWarning)] - - -def test_friction_force_history_rolls_newest_first(): - """Test friction history buffers roll newest-first.""" - data = ContactSensorData() - data.create_buffers(1, 1, 1, 3, True, False, False, "cpu", track_friction_forces=True) - timestamp = wp.ones((1,), dtype=wp.float32, device="cpu") - timestamp_last_update = wp.zeros((1,), dtype=wp.float32, device="cpu") - - for value in (1.0, 2.0, 3.0): - wp.to_torch(data._net_friction_forces_w).fill_(value) - wp.to_torch(data._friction_force_matrix_w).fill_(value) - wp.launch( - update_contact_sensor_kernel, - dim=(1, 1), - inputs=[ - 3, - 1, - 0.0, - wp.array([True], dtype=wp.bool, device="cpu"), - data._net_forces_w, - data._force_matrix_w, - data._net_normal_forces_w, - data._normal_force_matrix_w, - data._net_friction_forces_w, - data._friction_force_matrix_w, - timestamp, - timestamp_last_update, - data._net_forces_w_history, - data._force_matrix_w_history, - data._net_normal_forces_w_history, - data._normal_force_matrix_w_history, - data._net_friction_forces_w_history, - data._friction_force_matrix_w_history, - None, - None, - None, - None, - ], - device="cpu", - ) - - torch.testing.assert_close( - data.net_friction_forces_w_history.torch, - torch.tensor([[[[3.0, 3.0, 3.0]], [[2.0, 2.0, 2.0]], [[1.0, 1.0, 1.0]]]]), - ) - torch.testing.assert_close( - data.friction_force_matrix_w_history.torch, - torch.tensor([[[[[3.0, 3.0, 3.0]]], [[[2.0, 2.0, 2.0]]], [[[1.0, 1.0, 1.0]]]]]), - ) diff --git a/source/isaaclab_newton/test/sensors/test_frame_transformer.py b/source/isaaclab_newton/test/sensors/test_frame_transformer.py index ed87348b8bcf..e63119b182a8 100644 --- a/source/isaaclab_newton/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_newton/test/sensors/test_frame_transformer.py @@ -55,9 +55,6 @@ class MySceneCfg(InteractiveSceneCfg): # articulation - robot robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - # sensors - frame transformer (filled inside unit test) - frame_transformer: FrameTransformerCfg = None - # block cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/cube", @@ -98,487 +95,133 @@ def sim(): # Cleanup is handled by build_simulation_context -def test_frame_transformer_feet_wrt_base(sim): - """Test feet transformations w.r.t. base source frame. - - In this test, the source frame is the robot base. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="LF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), +def _feet_frames(prefixes: list[str]) -> list[FrameTransformerCfg.FrameCfg]: + """Foot frames offset from the ANYmal shanks, named ``_FOOT_USER``.""" + offsets = { + "LF": (-1, (0.08795, 0.01305, -0.33797)), + "RF": (1, (0.08795, -0.01305, -0.33797)), + "LH": (-1, (-0.08795, 0.01305, -0.33797)), + "RH": (1, (-0.08795, -0.01305, -0.33797)), + } + frames = [] + for prefix in prefixes: + sign, xyz = offsets[prefix] + frames.append( FrameTransformerCfg.FrameCfg( - name="LH_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LH_SHANK", + name=f"{prefix}_FOOT_USER", + prim_path=f"{{ENV_REGEX_NS}}/Robot/{prefix}_SHANK", offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(-0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RH_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RH_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(-0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() - - # Acquire the index of ground truth bodies - feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"]) - - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - - # Reorder the feet indices to match the order of the target frames with _USER suffix removed - target_frame_names = [name.split("_USER")[0] for name in target_frame_names] - - # Find the indices of the feet in the order of the target frames - reordering_indices = [feet_names.index(name) for name in target_frame_names] - feet_indices = [feet_indices[i] for i in reordering_indices] - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, + pos=euler_rpy_apply(rpy=(0, 0, sign * math.pi / 2), xyz=xyz), + rot=quat_from_euler_rpy(0, 0, sign * math.pi / 2), ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_pose_w.torch - feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w.torch[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w.torch[:, feet_indices] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch - feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch - - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) - - # check if relative transforms are same - feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - for index in range(len(feet_indices)): - # ground-truth - foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] ) - # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) - - -def test_frame_transformer_feet_wrt_thigh(sim): - """Test feet transformation w.r.t. thigh source frame.""" - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/LF_THIGH", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="LF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), - ), - FrameTransformerCfg.FrameCfg( - name="RF_FOOT_USER", - prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, math.pi / 2), - ), - ), - ], - ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() + ) + return frames - # Acquire the index of ground truth bodies - source_frame_index = scene.articulations["robot"].find_bodies("LF_THIGH")[0][0] - feet_indices, feet_names = scene.articulations["robot"].find_bodies(["LF_FOOT", "RF_FOOT"]) - # Check names are parsed the same order - user_feet_names = [f"{name}_USER" for name in feet_names] - assert scene.sensors["frame_transformer"].data.target_frame_names == user_feet_names - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, - ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset buffers - scene.reset() +def _assert_relative_poses(source_pos_w, source_quat_w, target_pos_w, target_quat_w, pos_source, quat_source): + """Check the source-relative target poses against the world poses, target by target.""" + for index in range(target_pos_w.shape[1]): + target_pos_b, target_quat_b = math_utils.subtract_frame_transforms( + source_pos_w, source_quat_w, target_pos_w[:, index], target_quat_w[:, index] + ) + torch.testing.assert_close(pos_source[:, index], target_pos_b) + torch.testing.assert_close(quat_source[:, index], target_quat_b) - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - # check absolute frame transforms in world frame - # -- ground-truth - source_pose_w_gt = scene.articulations["robot"].data.body_state_w.torch[:, source_frame_index, :7] - feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w.torch[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w.torch[:, feet_indices] - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - feet_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch - feet_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch - # check if they are same - torch.testing.assert_close(source_pose_w_gt[:, :3], source_pos_w_tf) - torch.testing.assert_close(source_pose_w_gt[:, 3:], source_quat_w_tf) - torch.testing.assert_close(feet_pos_w_gt, feet_pos_w_tf) - torch.testing.assert_close(feet_quat_w_gt, feet_quat_w_tf) - - # check if relative transforms are same - feet_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - feet_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - for index in range(len(feet_indices)): - # ground-truth - foot_pos_b, foot_quat_b = math_utils.subtract_frame_transforms( - source_pose_w_gt[:, :3], source_pose_w_gt[:, 3:], feet_pos_w_tf[:, index], feet_quat_w_tf[:, index] - ) - # check if they are same - torch.testing.assert_close(feet_pos_source_tf[:, index], foot_pos_b) - torch.testing.assert_close(feet_quat_source_tf[:, index], foot_quat_b) +def test_frame_transformer_sources_and_targets(sim): + """Frame transformers with different sources and targets track ground truth across scene resets. + One scene hosts five sensors, each checked against asset ground truth every step: -def test_frame_transformer_robot_body_to_external_cube(sim): - """Test transformation from robot body to a cube in the scene.""" - # Spawn things into stage + * ``ft_base``: offset foot frames on the shanks w.r.t. the robot base (root source). + * ``ft_thigh``: foot frames w.r.t. a non-root source; target names follow ``find_bodies`` order. + * ``ft_cube``: a separate rigid object as target of a robot body. + * ``ft_offsets``: +-0.1 m offset frames on the cube w.r.t. the cube itself. + * ``ft_all``: every robot body through a ``[^/]*`` wildcard, named after the bodies. + """ scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( + scene_cfg.ft_base = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", target_frames=_feet_frames(["LF", "RF", "LH", "RH"]) + ) + scene_cfg.ft_thigh = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/LF_THIGH", target_frames=_feet_frames(["LF", "RF"]) + ) + scene_cfg.ft_cube = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - name="CUBE_USER", - prim_path="{ENV_REGEX_NS}/cube", - ), - ], + target_frames=[FrameTransformerCfg.FrameCfg(name="CUBE_USER", prim_path="{ENV_REGEX_NS}/cube")], ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() - - # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, - ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) - # reset buffers - scene.reset() - - # set joint targets - robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_pose_w.torch - cube_pos_w_gt = scene.rigid_objects["cube"].data.root_pos_w.torch - cube_quat_w_gt = scene.rigid_objects["cube"].data.root_quat_w.torch - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - cube_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch.squeeze() - cube_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch.squeeze() - - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(cube_pos_w_gt, cube_pos_w_tf) - torch.testing.assert_close(cube_quat_w_gt, cube_quat_w_tf) - - # check if relative transforms are same - cube_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - cube_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - # ground-truth - cube_pos_b, cube_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], cube_pos_w_tf, cube_quat_w_tf - ) - # check if they are same - torch.testing.assert_close(cube_pos_source_tf[:, 0], cube_pos_b) - torch.testing.assert_close(cube_quat_source_tf[:, 0], cube_quat_b) - - -def test_frame_transformer_offset_frames(sim): - """Test body transformation w.r.t. base source frame. - - In this test, the source frame is the cube frame. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( + scene_cfg.ft_offsets = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/cube", target_frames=[ - FrameTransformerCfg.FrameCfg( - name="CUBE_CENTER", - prim_path="{ENV_REGEX_NS}/cube", - ), + FrameTransformerCfg.FrameCfg(name="CUBE_CENTER", prim_path="{ENV_REGEX_NS}/cube"), FrameTransformerCfg.FrameCfg( name="CUBE_TOP", prim_path="{ENV_REGEX_NS}/cube", - offset=OffsetCfg( - pos=(0.0, 0.0, 0.1), - rot=(0.0, 0.0, 0.0, 1.0), - ), + offset=OffsetCfg(pos=(0.0, 0.0, 0.1), rot=(0.0, 0.0, 0.0, 1.0)), ), FrameTransformerCfg.FrameCfg( name="CUBE_BOTTOM", prim_path="{ENV_REGEX_NS}/cube", - offset=OffsetCfg( - pos=(0.0, 0.0, -0.1), - rot=(0.0, 0.0, 0.0, 1.0), - ), + offset=OffsetCfg(pos=(0.0, 0.0, -0.1), rot=(0.0, 0.0, 0.0, 1.0)), ), ], ) - scene = InteractiveScene(scene_cfg) - - # Play the simulator - sim.reset() - - # Define simulation stepping - sim_dt = sim.get_physics_dt() - # Simulate physics - for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset - if count % 25 == 0: - # reset root state - root_state = torch.cat( - ( - scene["cube"].data.default_root_pose.torch, - scene["cube"].data.default_root_vel.torch, - ), - dim=-1, - ).clone() - root_state[:, :3] += scene.env_origins - # -- set root state - # -- cube - scene["cube"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - # reset buffers - scene.reset() - - # write data to sim - scene.write_data_to_sim() - # perform step - sim.step() - # read data from sim - scene.update(sim_dt) - - # check absolute frame transforms in world frame - # -- ground-truth - cube_pos_w_gt = scene["cube"].data.root_pos_w.torch - cube_quat_w_gt = scene["cube"].data.root_quat_w.torch - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - target_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch.squeeze() - target_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch.squeeze() - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - - cube_center_idx = target_frame_names.index("CUBE_CENTER") - cube_bottom_idx = target_frame_names.index("CUBE_BOTTOM") - cube_top_idx = target_frame_names.index("CUBE_TOP") - - # check if they are same - torch.testing.assert_close(cube_pos_w_gt, source_pos_w_tf) - torch.testing.assert_close(cube_quat_w_gt, source_quat_w_tf) - torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) - torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) - - # test offsets are applied correctly - # -- cube top - cube_pos_top = target_pos_w_tf[:, cube_top_idx] - cube_quat_top = target_quat_w_tf[:, cube_top_idx] - torch.testing.assert_close( - cube_pos_top, cube_pos_w_gt + torch.tensor([0.0, 0.0, 0.1], device=cube_pos_w_gt.device) - ) - torch.testing.assert_close(cube_quat_top, cube_quat_w_gt) - - # -- cube bottom - cube_pos_bottom = target_pos_w_tf[:, cube_bottom_idx] - cube_quat_bottom = target_quat_w_tf[:, cube_bottom_idx] - torch.testing.assert_close( - cube_pos_bottom, cube_pos_w_gt + torch.tensor([0.0, 0.0, -0.1], device=cube_pos_w_gt.device) - ) - torch.testing.assert_close(cube_quat_bottom, cube_quat_w_gt) - - -def test_frame_transformer_all_bodies(sim): - """Test transformation of all bodies w.r.t. base source frame. - - In this test, the source frame is the robot base. - - The target_frames are all bodies in the robot, implemented using .* pattern. - """ - # Spawn things into stage - scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) - scene_cfg.frame_transformer = FrameTransformerCfg( + scene_cfg.ft_all = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/[^/]*", - ), - ], + target_frames=[FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/[^/]*")], ) scene = InteractiveScene(scene_cfg) # Play the simulator sim.reset() - target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - articulation_body_names = scene.articulations["robot"].data.body_names - - reordering_indices = [target_frame_names.index(name) for name in articulation_body_names] + robot = scene.articulations["robot"] + cube = scene["cube"] + + # -- ft_base: reorder the feet indices to match the target frames with the _USER suffix removed + base_feet_indices, base_feet_names = robot.find_bodies(["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"]) + base_frame_names = [name.split("_USER")[0] for name in scene.sensors["ft_base"].data.target_frame_names] + base_feet_indices = [base_feet_indices[base_feet_names.index(name)] for name in base_frame_names] + # -- ft_thigh: names are parsed in the same order as the bodies + thigh_index = robot.find_bodies("LF_THIGH")[0][0] + thigh_feet_indices, thigh_feet_names = robot.find_bodies(["LF_FOOT", "RF_FOOT"]) + assert scene.sensors["ft_thigh"].data.target_frame_names == [f"{name}_USER" for name in thigh_feet_names] + # -- ft_all: wildcard frames are named after the bodies + all_frame_names = scene.sensors["ft_all"].data.target_frame_names + articulation_body_names = robot.data.body_names + all_reordering_indices = [all_frame_names.index(name) for name in articulation_body_names] # default joint targets - default_actions = scene.articulations["robot"].data.default_joint_pos.torch.clone() + default_actions = robot.data.default_joint_pos.torch.clone() # Define simulation stepping sim_dt = sim.get_physics_dt() # Simulate physics for count in range(50): - # reset every 25 steps so the sensor is checked across a scene reset + # reset every 25 steps so the sensors are checked across a scene reset if count % 25 == 0: - # reset root state + # -- robot root_state = torch.cat( - ( - scene.articulations["robot"].data.default_root_pose.torch, - scene.articulations["robot"].data.default_root_vel.torch, - ), - dim=-1, + (robot.data.default_root_pose.torch, robot.data.default_root_vel.torch), dim=-1 ).clone() root_state[:, :3] += scene.env_origins - joint_pos = scene.articulations["robot"].data.default_joint_pos.torch - joint_vel = scene.articulations["robot"].data.default_joint_vel.torch - # -- set root state - # -- robot - scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) - scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) - scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) + robot.write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + robot.write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch) + robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch) + # -- cube + cube_state = torch.cat( + (cube.data.default_root_pose.torch, cube.data.default_root_vel.torch), dim=-1 + ).clone() + cube_state[:, :3] += scene.env_origins + cube.write_root_pose_to_sim_index(root_pose=cube_state[:, :7]) + cube.write_root_velocity_to_sim_index(root_velocity=cube_state[:, 7:]) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) + robot.set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -586,39 +229,94 @@ def test_frame_transformer_all_bodies(sim): # read data from sim scene.update(sim_dt) - # check absolute frame transforms in world frame # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_pose_w.torch - bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w.torch - bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w.torch - - # -- frame transformer - source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w.torch - source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w.torch - bodies_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w.torch - bodies_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w.torch - - # check if they are same - torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf) - torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf) - torch.testing.assert_close(bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices]) - torch.testing.assert_close(bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices]) - - bodies_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source.torch - bodies_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source.torch - - # Go through each body and check if relative transforms are same - for index in range(len(articulation_body_names)): - body_pos_b, body_quat_b = math_utils.subtract_frame_transforms( - root_pose_w[:, :3], root_pose_w[:, 3:], bodies_pos_w_tf[:, index], bodies_quat_w_tf[:, index] - ) + root_pose_w = robot.data.root_pose_w.torch + body_pos_w = robot.data.body_pos_w.torch + body_quat_w = robot.data.body_quat_w.torch + cube_pos_w_gt = cube.data.root_pos_w.torch + cube_quat_w_gt = cube.data.root_quat_w.torch + + # -- ft_base: feet w.r.t. the robot base + data = scene.sensors["ft_base"].data + torch.testing.assert_close(root_pose_w[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(root_pose_w[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(body_pos_w[:, base_feet_indices], data.target_pos_w.torch) + torch.testing.assert_close(body_quat_w[:, base_feet_indices], data.target_quat_w.torch) + _assert_relative_poses( + root_pose_w[:, :3], + root_pose_w[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) - torch.testing.assert_close(bodies_pos_source_tf[:, index], body_pos_b) - torch.testing.assert_close(bodies_quat_source_tf[:, index], body_quat_b) + # -- ft_thigh: feet w.r.t. a thigh + data = scene.sensors["ft_thigh"].data + source_pose_w_gt = robot.data.body_state_w.torch[:, thigh_index, :7] + torch.testing.assert_close(source_pose_w_gt[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(source_pose_w_gt[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(body_pos_w[:, thigh_feet_indices], data.target_pos_w.torch) + torch.testing.assert_close(body_quat_w[:, thigh_feet_indices], data.target_quat_w.torch) + _assert_relative_poses( + source_pose_w_gt[:, :3], + source_pose_w_gt[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) + + # -- ft_cube: the cube w.r.t. the robot base + data = scene.sensors["ft_cube"].data + torch.testing.assert_close(root_pose_w[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(root_pose_w[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(cube_pos_w_gt, data.target_pos_w.torch.squeeze()) + torch.testing.assert_close(cube_quat_w_gt, data.target_quat_w.torch.squeeze()) + _assert_relative_poses( + root_pose_w[:, :3], + root_pose_w[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) + + # -- ft_offsets: offset frames w.r.t. the cube + data = scene.sensors["ft_offsets"].data + target_pos_w_tf = data.target_pos_w.torch + target_quat_w_tf = data.target_quat_w.torch + cube_center_idx = data.target_frame_names.index("CUBE_CENTER") + cube_bottom_idx = data.target_frame_names.index("CUBE_BOTTOM") + cube_top_idx = data.target_frame_names.index("CUBE_TOP") + torch.testing.assert_close(cube_pos_w_gt, data.source_pos_w.torch) + torch.testing.assert_close(cube_quat_w_gt, data.source_quat_w.torch) + torch.testing.assert_close(cube_pos_w_gt, target_pos_w_tf[:, cube_center_idx]) + torch.testing.assert_close(cube_quat_w_gt, target_quat_w_tf[:, cube_center_idx]) + offset = torch.tensor([0.0, 0.0, 0.1], device=cube_pos_w_gt.device) + torch.testing.assert_close(target_pos_w_tf[:, cube_top_idx], cube_pos_w_gt + offset) + torch.testing.assert_close(target_quat_w_tf[:, cube_top_idx], cube_quat_w_gt) + torch.testing.assert_close(target_pos_w_tf[:, cube_bottom_idx], cube_pos_w_gt - offset) + torch.testing.assert_close(target_quat_w_tf[:, cube_bottom_idx], cube_quat_w_gt) + + # -- ft_all: every body w.r.t. the robot base + data = scene.sensors["ft_all"].data + torch.testing.assert_close(root_pose_w[:, :3], data.source_pos_w.torch) + torch.testing.assert_close(root_pose_w[:, 3:], data.source_quat_w.torch) + torch.testing.assert_close(body_pos_w, data.target_pos_w.torch[:, all_reordering_indices]) + torch.testing.assert_close(body_quat_w, data.target_quat_w.torch[:, all_reordering_indices]) + _assert_relative_poses( + root_pose_w[:, :3], + root_pose_w[:, 3:], + data.target_pos_w.torch, + data.target_quat_w.torch, + data.target_pos_source.torch, + data.target_quat_source.torch, + ) -@pytest.mark.parametrize("source_robot", ["Robot", "Robot_1"]) -@pytest.mark.parametrize("path_prefix", ["{ENV_REGEX_NS}", "/World"]) +# Each source robot and each path prefix is covered once; the axes select independent branches. +@pytest.mark.parametrize(("source_robot", "path_prefix"), [("Robot", "{ENV_REGEX_NS}"), ("Robot_1", "/World")]) def test_frame_transformer_duplicate_body_names(sim, source_robot, path_prefix): """Test tracking bodies with same leaf name at different hierarchy levels. diff --git a/source/isaaclab_newton/test/sensors/test_imu.py b/source/isaaclab_newton/test/sensors/test_imu.py index 91f1b3a611dd..1f584c973429 100644 --- a/source/isaaclab_newton/test/sensors/test_imu.py +++ b/source/isaaclab_newton/test/sensors/test_imu.py @@ -12,11 +12,10 @@ import pytest import torch -import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg import isaaclab.sim as sim_utils -from isaaclab.assets import RigidObject, RigidObjectCfg +from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors.imu import Imu, ImuCfg from isaaclab.sim import SimulationCfg @@ -65,8 +64,11 @@ def sim(): yield sim -def test_initialization_and_data_shapes(sim): - """The Newton IMU sensor initializes and exposes correctly shaped buffers after one step.""" +def test_at_rest_measures_gravity_and_zero_angular_velocity(sim): + """A settled IMU measures gravity (~9.81 m/s^2 upward) and near-zero angular velocity. + + While the cube still falls, the accelerometer reads near zero (proper, not coordinate, acceleration). + """ scene_cfg = ImuTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() @@ -74,25 +76,20 @@ def test_initialization_and_data_shapes(sim): imu: Imu = scene["imu"] assert imu.num_instances == 2 - sim.step() - scene.update(sim.get_physics_dt()) - - assert imu.data.ang_vel_b.torch.shape == (2, 3) - assert imu.data.lin_acc_b.torch.shape == (2, 3) + # The cube falls from z=1.0 and lands after ~86 steps; 10 steps are still in freefall. + for _ in range(10): + sim.step() + scene.update(sim.get_physics_dt()) - -def test_at_rest_measures_gravity_and_zero_angular_velocity(sim): - """A settled IMU measures gravity (~9.81 m/s^2 upward) and near-zero angular velocity.""" - scene_cfg = ImuTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() + # In freefall, gravity and inertial acceleration cancel. + acc_magnitude = torch.norm(imu.data.lin_acc_b.torch, dim=-1) + torch.testing.assert_close(acc_magnitude, torch.zeros_like(acc_magnitude), atol=0.5, rtol=0.0) # Step enough for the cube to settle on the ground - for _ in range(500): + for _ in range(490): sim.step() scene.update(sim.get_physics_dt()) - imu: Imu = scene["imu"] lin_acc = imu.data.lin_acc_b.torch ang_vel = imu.data.ang_vel_b.torch @@ -113,112 +110,41 @@ def test_at_rest_measures_gravity_and_zero_angular_velocity(sim): torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel), atol=0.1, rtol=0.0) -def test_reset(sim): - """Test that reset zeroes out IMU data.""" - scene_cfg = ImuTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - # Step enough for the cube to settle on the ground so the accelerometer reads gravity. - # The cube falls from z=1.0 (bottom at z=0.9) and reaches the ground in ~86 steps - # at 200 Hz; 200 steps gives time to settle after impact. - for _ in range(200): - sim.step() - scene.update(sim.get_physics_dt()) - - imu: Imu = scene["imu"] - - lin_acc = imu.data.lin_acc_b.torch - assert torch.any(lin_acc != 0), "Expected non-zero data before reset" - - imu.reset() - - # Access internal buffers directly: accessing imu.data triggers lazy re-evaluation - # which re-fills from the Newton sensor, so we check the raw buffers instead. - ang_vel_after = wp.to_torch(imu._data._ang_vel_b) - lin_acc_after = wp.to_torch(imu._data._lin_acc_b) - - torch.testing.assert_close(ang_vel_after, torch.zeros_like(ang_vel_after)) - torch.testing.assert_close(lin_acc_after, torch.zeros_like(lin_acc_after)) - - -@configclass -class FreefallSceneCfg(InteractiveSceneCfg): - """Scene with a rigid cube and IMU but no ground plane (freefall).""" - - env_spacing = 2.0 - cube = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), - mass_props=sim_utils.MassCfg(mass=1.0), - collision_props=sim_utils.UsdPhysicsCollisionCfg(), - physics_material=sim_utils.RigidBodyMaterialCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 0.0)), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)), - ) - - imu = ImuCfg( - prim_path="{ENV_REGEX_NS}/Cube", - ) - - -def test_freefall_acceleration(sim): - """Test that a freefalling IMU measures near-zero acceleration.""" - scene_cfg = FreefallSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - # Step a few times while the cube is in freefall (no ground contact) - for _ in range(10): - sim.step() - scene.update(sim.get_physics_dt()) - - imu: Imu = scene["imu"] - lin_acc = imu.data.lin_acc_b.torch - - # In freefall, accelerometer should read near zero (gravity and inertial acceleration cancel) - acc_magnitude = torch.norm(lin_acc, dim=-1) - torch.testing.assert_close( - acc_magnitude, - torch.zeros_like(acc_magnitude), - atol=0.5, - rtol=0.0, - ) - - def test_no_stale_data_after_scene_reset(sim): - """Regression for #4970: ``scene.reset(env_ids)`` must not surface pre-reset IMU values (Newton). + """Regression for #4970: resets must not surface pre-reset IMU values (Newton). - Mirrors the PhysX equivalent. Reproduces the ``ManagerBasedRLEnv._reset_idx`` flow where - reset runs inside a step without a subsequent physics step; the IMU sensor's lazy ``data`` - accessor must not refetch from the Newton rigid-body view here (the velocity buffer reflects - the previous step and would produce a spurious finite-difference acceleration). + Reproduces the ``ManagerBasedRLEnv._reset_idx`` flow, where reset runs inside a step without a + subsequent physics step: Newton's accelerometer still holds the pre-reset reading, so the public + ``data`` accessor must return the zeroed buffers instead of refetching it. The cube rests on the + ground first so the stale reading (gravity, ~9.81 m/s^2) is distinguishable from a reset one. """ - scene_cfg = ImuTestSceneCfg(num_envs=1) + scene_cfg = ImuTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() scene.reset() imu: Imu = scene["imu"] - cube: RigidObject = scene["cube"] - # Let the cube fall so the rigid-body view accumulates a non-zero velocity. - for _ in range(30): + # The cube falls from z=1.0 (bottom at z=0.9) and lands in ~86 steps at 200 Hz; 200 steps let it settle. + for _ in range(200): scene.write_data_to_sim() sim.step(render=False) scene.update(dt=sim.get_physics_dt()) - # Reset the scene without writing fresh velocity/transform. The Newton velocity - # buffer therefore still holds the pre-reset (falling) value. - env_ids = torch.tensor([0], device=cube.device) - scene.reset(env_ids=env_ids) - - # The public ``data`` accessor must not refetch a stale physics buffer; ``reset()`` zeroes - # ``_lin_acc_b`` / ``_ang_vel_b`` and those must be what comes out here. - post_reset_lin_acc = imu.data.lin_acc_b.torch - post_reset_ang_vel = imu.data.ang_vel_b.torch - torch.testing.assert_close(post_reset_lin_acc, torch.zeros_like(post_reset_lin_acc)) - torch.testing.assert_close(post_reset_ang_vel, torch.zeros_like(post_reset_ang_vel)) + pre_reset_lin_acc = imu.data.lin_acc_b.torch.clone() + assert (pre_reset_lin_acc[:, 2] > 5.0).all(), f"Expected a settled gravity reading, got {pre_reset_lin_acc}" + + # Partial reset: env 0 reads zeros, env 1 keeps its measurement. + scene.reset(env_ids=torch.tensor([0], device=imu.device)) + lin_acc = imu.data.lin_acc_b.torch + ang_vel = imu.data.ang_vel_b.torch + torch.testing.assert_close(lin_acc[0], torch.zeros_like(lin_acc[0])) + torch.testing.assert_close(ang_vel[0], torch.zeros_like(ang_vel[0])) + torch.testing.assert_close(lin_acc[1], pre_reset_lin_acc[1]) + + # Full reset zeroes every environment. + imu.reset() + lin_acc = imu.data.lin_acc_b.torch + ang_vel = imu.data.ang_vel_b.torch + torch.testing.assert_close(lin_acc, torch.zeros_like(lin_acc)) + torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel)) diff --git a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py index dd54dbb9ea98..95b1f40de1da 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -92,16 +92,6 @@ class _SingleJointSceneCfg(InteractiveSceneCfg): wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") -@configclass -class _CartpoleSceneCfg(InteractiveSceneCfg): - """Scene with a cartpole (2-joint) articulation and the joint-wrench sensor.""" - - env_spacing = 4.0 - terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") - robot = _make_cartpole_articulation_cfg() - wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") - - @configclass class _CartpoleDampedSceneCfg(InteractiveSceneCfg): """Cartpole with pole damping for steady-state physics validation tests.""" @@ -152,46 +142,10 @@ def test_data_before_init_is_none(): # --------------------------------------------------------------------------- -# Initialization and shapes +# Initialization # --------------------------------------------------------------------------- -def test_initialization_and_shapes(sim): - """Sensor initializes on sim reset and exposes correctly-shaped buffers.""" - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) - sim.reset() - - robot: Articulation = scene["robot"] - sensor: JointWrenchSensor = scene["wrench"] - sim.step() - scene.update(sim.get_physics_dt()) - - # revolute_articulation has one joint whose child is "Arm". - num_envs = 2 - num_joints = 1 - assert sensor.data.force.torch.shape == (num_envs, num_joints, 3) - assert sensor.data.torque.torch.shape == (num_envs, num_joints, 3) - assert sensor.body_names == ["Arm"] - assert sensor._root_view is robot.root_view # noqa: SLF001 - - -def test_multi_body_articulation(sim): - """Cartpole (2 joints) exposes a wrench for each joint labelled by its child body.""" - scene = InteractiveScene(_CartpoleSceneCfg(num_envs=2)) - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - sim.step() - scene.update(sim.get_physics_dt()) - - num_envs = 2 - num_joints = 2 - assert sensor.data.force.torch.shape == (num_envs, num_joints, 3) - assert sensor.data.torque.torch.shape == (num_envs, num_joints, 3) - assert len(sensor.body_names) == 2 - assert "rail" not in [n.lower() for n in sensor.body_names] - - def test_nested_articulation_root_resolution(sim): """Sensor covers a nested articulation root from the configured asset prefix.""" scene = InteractiveScene(_NestedRootAntSceneCfg(num_envs=1)) @@ -210,163 +164,40 @@ def test_nested_articulation_root_resolution(sim): # --------------------------------------------------------------------------- # Physical correctness +# +# The joint-frame convention (orientation and anchor) is owned by the shared ``test_joint_wrench_frame`` +# contract. The tests below check load paths with frame-independent quantities from public asset data. # --------------------------------------------------------------------------- -def _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env: int, - joint: int, - gravity: torch.Tensor, - ext_force_b: torch.Tensor | None = None, - ext_torque_b: torch.Tensor | None = None, - descendant_body_names: list[str] | None = None, -): - """Compute the analytical joint-frame wrench for a single joint. - - Uses the same geometric data (body_com, joint_X_c, body_q) and frame - transformations as the kernel, but computes the wrench analytically from - known loads rather than reading body_parent_f. Computes the moment of - forces about the joint anchor and rotates the result into the child-side - joint frame. - - For terminal links, the wrench is due to the child body alone. For - interior joints, pass all bodies in the subtree below the joint via - ``descendant_body_names`` so the helper sums their gravitational - contributions. +def test_wrench_with_external_force_and_torque(sim): + """External loads applied through the wrench composer reach the reported joint wrench. - Args: - sensor: The JointWrenchSensor instance (used to read Newton model bindings). - robot: The Articulation asset (used for body mass lookup). - env: Environment index. - joint: Joint index within the sensor. - gravity: Gravity vector in world frame, shape (3,). - ext_force_b: External force on the child body in body frame [N], shape (3,). - ext_torque_b: External torque on the child body in body frame [N·m], shape (3,). - descendant_body_names: Bodies whose gravitational load acts through this - joint. Defaults to the child body only (correct for terminal links). - For an interior joint, pass all bodies in the subtree below the joint. - - Returns: - A tuple of (force, torque) tensors, each shape (3,), in the child-side - joint frame. + The arm first settles under gravity alone, then under an additional body-frame force and torque. Force + magnitudes are frame-independent, and so is ``dF . dtau``: with ``dF = -f`` and + ``dtau = -(tau + r x f)`` for any anchor offset ``r``, it equals ``f . tau``. """ - body_idx = wp.to_torch(sensor._joint_child)[joint].item() - - # Link transform in world (of the child body — defines the joint frame). - link_xform = wp.to_torch(sensor._sim_bind_body_q)[env, body_idx] # (7,) = pos(3) + quat(4) - link_pos = link_xform[:3] - link_quat = link_xform[3:] # wp.quatf = (x, y, z, w) - - # Joint anchor and orientation in world = link_xform * joint_X_c. - joint_X_c = wp.to_torch(sensor._sim_bind_joint_X_c)[env, joint] # (7,) - jxc_pos = joint_X_c[:3] - jxc_quat = joint_X_c[3:] - anchor_world = link_pos + math_utils.quat_apply(link_quat.unsqueeze(0), jxc_pos.unsqueeze(0)).squeeze(0) - joint_quat_world = math_utils.quat_mul(link_quat.unsqueeze(0), jxc_quat.unsqueeze(0)).squeeze(0) - - # Bodies whose weight contributes to the wrench at this joint. - if descendant_body_names is None: - descendant_body_names = [sensor.body_names[joint]] - - link_names = list(sensor._root_view.link_names) - - total_force_w = torch.zeros(3, device=gravity.device) - total_torque_w = torch.zeros(3, device=gravity.device) - - for body_name in descendant_body_names: - b_idx = link_names.index(body_name) - b_xform = wp.to_torch(sensor._sim_bind_body_q)[env, b_idx] - b_pos = b_xform[:3] - b_quat = b_xform[3:] - b_com_local = wp.to_torch(sensor._sim_bind_body_com)[env, b_idx] - b_com_world = b_pos + math_utils.quat_apply(b_quat.unsqueeze(0), b_com_local.unsqueeze(0)).squeeze(0) - - art_b_idx = robot.body_names.index(body_name) - mass = robot.data.body_mass.torch[env, art_b_idx].item() - weight_w = mass * gravity - - total_force_w = total_force_w + weight_w - r = b_com_world - anchor_world - total_torque_w = total_torque_w + torch.cross(r, weight_w, dim=-1) - - # External force/torque on the child body only (if provided). Actuator - # torque is intentionally omitted; see tolerance comment in calling tests. - if ext_force_b is not None: - ext_force_w = math_utils.quat_apply(link_quat.unsqueeze(0), ext_force_b.unsqueeze(0)).squeeze(0) - total_force_w = total_force_w + ext_force_w - # Moment of the external force about the joint anchor (applied at child COM). - child_com_local = wp.to_torch(sensor._sim_bind_body_com)[env, body_idx] - child_com_world = link_pos + math_utils.quat_apply( - link_quat.unsqueeze(0), child_com_local.unsqueeze(0) - ).squeeze(0) - total_torque_w = total_torque_w + torch.cross(child_com_world - anchor_world, ext_force_w, dim=-1) - if ext_torque_b is not None: - total_torque_w = total_torque_w + math_utils.quat_apply( - link_quat.unsqueeze(0), ext_torque_b.unsqueeze(0) - ).squeeze(0) - - # Reaction wrench = negation of total wrench (joint supports against all loads). - reaction_force_w = -total_force_w - reaction_torque_w = -total_torque_w - - # Rotate into joint frame. - expected_force = math_utils.quat_apply_inverse( - joint_quat_world.unsqueeze(0), reaction_force_w.unsqueeze(0) - ).squeeze(0) - expected_torque = math_utils.quat_apply_inverse( - joint_quat_world.unsqueeze(0), reaction_torque_w.unsqueeze(0) - ).squeeze(0) - - return expected_force, expected_torque - - -def test_force_and_torque_components_at_rest(sim): - """Component-level validation of force and torque against analytical expectations (gravity only).""" scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) sim.reset() sensor: JointWrenchSensor = scene["wrench"] robot: Articulation = scene["robot"] + arm_idx = robot.body_names.index("Arm") + gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + weight_w = robot.data.body_mass.torch[0, arm_idx] * gravity + for _ in range(400): sim.step() scene.update(sim.get_physics_dt()) + force_gravity = sensor.data.force.torch[0, 0].clone() + torque_gravity = sensor.data.torque.torch[0, 0].clone() + torch.testing.assert_close(force_gravity.norm(), weight_w.norm(), atol=1e-2, rtol=1e-3) - gravity = torch.tensor(sim.cfg.gravity, device=sim.device) - expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env=0, - joint=0, - gravity=gravity, - ) - - force = sensor.data.force.torch[0, 0] - torque = sensor.data.torque.torch[0, 0] - - torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) - torch.testing.assert_close(torque, expected_torque, atol=1e-2, rtol=1e-3) - - -def test_wrench_with_external_force_and_torque(sim): - """Full analytical wrench validation with external force and torque applied. - - Mirrors the PhysX ``test_body_incoming_joint_wrench_b_single_joint`` pattern: - apply a known wrench, settle, compute the expected reaction wrench analytically, - and compare component-by-component. - """ - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - robot: Articulation = scene["robot"] - arm_idx = robot.body_names.index("Arm") - - # Apply 10 N in body-Y and 10 N·m in body-Z on the arm (matches PhysX test). + # Force on every axis makes the dot-product check sensitive to each torque component. ext_force_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) - ext_force_b[:, arm_idx, 1] = 10.0 + ext_force_b[:, arm_idx, :] = torch.tensor([5.0, 10.0, 10.0], device=sim.device) ext_torque_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) + ext_torque_b[:, arm_idx, 1] = 5.0 ext_torque_b[:, arm_idx, 2] = 10.0 for _ in range(800): @@ -375,24 +206,17 @@ def test_wrench_with_external_force_and_torque(sim): sim.step() scene.update(sim.get_physics_dt()) - gravity = torch.tensor(sim.cfg.gravity, device=sim.device) - expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( - sensor, - robot, - env=0, - joint=0, - gravity=gravity, - ext_force_b=ext_force_b[0, arm_idx], - ext_torque_b=ext_torque_b[0, arm_idx], - ) - force = sensor.data.force.torch[0, 0] torque = sensor.data.torque.torch[0, 0] - - # The PD actuator contributes a small torque (~0.1 N·m) to body_parent_f that is - # not modelled in the analytical helper. Force is unaffected (actuator is pure torque). - torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) - torch.testing.assert_close(torque, expected_torque, atol=0.15, rtol=1e-2) + arm_quat_w = robot.data.body_link_quat_w.torch[0, arm_idx] + ext_force_w = math_utils.quat_apply(arm_quat_w.unsqueeze(0), ext_force_b[0, arm_idx].unsqueeze(0)).squeeze(0) + torch.testing.assert_close(force.norm(), (weight_w + ext_force_w).norm(), atol=1e-2, rtol=1e-3) + torch.testing.assert_close((force - force_gravity).norm(), ext_force_b[0, arm_idx].norm(), atol=1e-2, rtol=1e-3) + # The PD actuator adds a small torque (~0.1 N·m) along the joint axis that is not modelled here. + expected_dot = torch.dot(ext_force_b[0, arm_idx], ext_torque_b[0, arm_idx]) + torch.testing.assert_close( + torch.dot(force - force_gravity, torque - torque_gravity), expected_dot, atol=1.5, rtol=0.0 + ) @pytest.mark.parametrize("fixed_pole", [False, True]) @@ -421,13 +245,15 @@ def test_interior_joint_wrench_at_rest(sim, tmp_path, fixed_pole): sim.step() scene.update(sim.get_physics_dt()) + assert sensor.data.force.torch.shape == sensor.data.torque.torch.shape == (1, 2, 3) + # Each joint carries the weight of its subtree, whatever frame the force is expressed in. gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + masses = robot.data.body_mass.torch[0] for joint, descendants in enumerate((["cart", "pole"], ["pole"])): - expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( - sensor, robot, env=0, joint=joint, gravity=gravity, descendant_body_names=descendants + subtree_mass = sum(masses[robot.body_names.index(name)] for name in descendants) + torch.testing.assert_close( + sensor.data.force.torch[0, joint].norm(), (subtree_mass * gravity).norm(), atol=1e-2, rtol=1e-3 ) - torch.testing.assert_close(sensor.data.force.torch[0, joint], expected_force, atol=1e-2, rtol=1e-3) - torch.testing.assert_close(sensor.data.torque.torch[0, joint], expected_torque, atol=1e-2, rtol=1e-3) # --------------------------------------------------------------------------- @@ -435,43 +261,15 @@ def test_interior_joint_wrench_at_rest(sim, tmp_path, fixed_pole): # --------------------------------------------------------------------------- -def test_reset_zeros_selected_then_all_envs(sim): - """Partial reset zeros only the selected envs; a full reset clears every force / torque buffer.""" - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=4)) - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - for _ in range(100): - sim.step() - scene.update(sim.get_physics_dt()) - - force_before = sensor.data.force.torch.clone() - assert torch.all(torch.any(force_before != 0, dim=(1, 2))), "Expected non-zero data in every env before reset" - - sensor.reset(env_ids=[0, 2]) - - # Access raw buffers to skip lazy re-population from the Newton view on the next data read. - force_after = wp.to_torch(sensor._data._force) - torch.testing.assert_close(force_after[0], torch.zeros_like(force_after[0])) - torch.testing.assert_close(force_after[2], torch.zeros_like(force_after[2])) - torch.testing.assert_close(force_after[1], force_before[1]) - torch.testing.assert_close(force_after[3], force_before[3]) - - sensor.reset() - - force_after = wp.to_torch(sensor._data._force) - torque_after = wp.to_torch(sensor._data._torque) - torch.testing.assert_close(force_after, torch.zeros_like(force_after)) - torch.testing.assert_close(torque_after, torch.zeros_like(torque_after)) - - def test_no_stale_data_after_scene_reset(sim): """Regression for #4970: ``scene.reset(env_ids)`` must not surface pre-reset wrenches (Newton). Mirrors the PhysX equivalent. The joint-wrench sensor's lazy ``data`` accessor must not refetch from the Newton articulation view here (the wrench buffer reflects the previous step). + A partial reset leaves the other envs untouched and a full sensor reset zeroes every env. """ - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) + num_envs = 4 + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=num_envs)) sim.reset() sensor: JointWrenchSensor = scene["wrench"] @@ -479,11 +277,23 @@ def test_no_stale_data_after_scene_reset(sim): sim.step() scene.update(sim.get_physics_dt()) + # revolute_articulation has one joint whose child is "Arm". + assert sensor.body_names == ["Arm"] + assert sensor.data.force.torch.shape == sensor.data.torque.torch.shape == (num_envs, 1, 3) pre_reset_force = sensor.data.force.torch.clone() - pre_reset_torque = sensor.data.torque.torch.clone() - assert torch.any(pre_reset_force != 0) or torch.any(pre_reset_torque != 0), "Expected non-zero wrench before reset" + assert torch.all(torch.any(pre_reset_force != 0, dim=(1, 2))), "Expected non-zero wrench in every env" + + scene.reset(env_ids=torch.tensor([0, 2], device=sensor.device)) - scene.reset(env_ids=torch.tensor([0], device=sensor.device)) + post_reset_force = sensor.data.force.torch + post_reset_torque = sensor.data.torque.torch + for env in (0, 2): + torch.testing.assert_close(post_reset_force[env], torch.zeros_like(post_reset_force[env])) + torch.testing.assert_close(post_reset_torque[env], torch.zeros_like(post_reset_torque[env])) + for env in (1, 3): + torch.testing.assert_close(post_reset_force[env], pre_reset_force[env]) + + sensor.reset() post_reset_force = sensor.data.force.torch post_reset_torque = sensor.data.torque.torch diff --git a/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py b/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py index 9cc18abc5912..a28adf49236d 100644 --- a/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py @@ -124,11 +124,19 @@ def _step_and_read(sim, scene) -> NewtonRaycastSensor: return scene["raycast"] -@pytest.mark.parametrize("global_world_only", [False, True]) -def test_rays_hit_ground_plane(sim, global_world_only): - """All rays of a downward grid pattern hit the global-world ground at the sensor height.""" - scene_cfg = RaycastTestSceneCfg(num_envs=2) - scene_cfg.raycast.global_world_only = global_world_only +# Graph mode and ``global_world_only`` select independent branches, so each value is covered once. +@pytest.mark.parametrize( + ("sim", "generic_cfg"), + [pytest.param(True, False, id="cuda_graph-newton_cfg"), pytest.param(False, True, id="eager-generic_cfg")], + indirect=["sim"], +) +def test_rays_hit_ground_plane(sim, generic_cfg): + """All rays of a downward grid pattern hit the global-world ground at the sensor height. + + The generic row uses the backend-dispatching :class:`RayCasterCfg` with ``global_world_only=True``, + which must select the Newton BVH implementation. + """ + scene_cfg = GenericRaycastTestSceneCfg(num_envs=2) if generic_cfg else RaycastTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) expected_bvh_flags = ShapeFlags.VISIBLE | ShapeFlags.COLLIDE_SHAPES assert NewtonManager._sensor_bvh_shape_flags == expected_bvh_flags @@ -144,31 +152,16 @@ def test_rays_hit_ground_plane(sim, global_world_only): torch.testing.assert_close(distances, torch.full_like(distances, RAY_START_HEIGHT), atol=1e-3, rtol=0) expected_normal = torch.tensor([0.0, 0.0, 1.0], device=normals.device).expand_as(normals) torch.testing.assert_close(normals, expected_normal, atol=1e-3, rtol=0) + if generic_cfg: + assert isinstance(sensor, NewtonRaycastSensor) + # Camera and multi-mesh factories retain their explicit legacy implementations. + assert RayCasterCamera.resolve_class() is LegacyRayCasterCamera + assert MultiMeshRayCaster.resolve_class() is LegacyMultiMeshRayCaster + assert MultiMeshRayCasterCamera.resolve_class() is LegacyMultiMeshRayCasterCamera -def test_generic_ray_caster_uses_newton_scene_bvh(sim): - """The backend-dispatching ray caster selects the Newton BVH implementation.""" - scene = InteractiveScene(GenericRaycastTestSceneCfg(num_envs=1)) - sim.reset() - sensor = _step_and_read(sim, scene) - - assert isinstance(sensor, NewtonRaycastSensor) - assert hasattr(sensor.data, "ray_distances") - torch.testing.assert_close( - sensor.data.ray_distances.torch, - torch.full_like(sensor.data.ray_distances.torch, RAY_START_HEIGHT), - atol=1e-3, - rtol=0, - ) - - -def test_remaining_warp_mesh_factories_select_legacy_newton_adapters(sim): - """Camera and multi-mesh factories retain their explicit legacy implementations.""" - assert RayCasterCamera.resolve_class() is LegacyRayCasterCamera - assert MultiMeshRayCaster.resolve_class() is LegacyMultiMeshRayCaster - assert MultiMeshRayCasterCamera.resolve_class() is LegacyMultiMeshRayCasterCamera - - +# The legacy adapter does not use the Newton manager graph. +@pytest.mark.parametrize("sim", [pytest.param(False, id="eager")], indirect=True) def test_legacy_multi_mesh_tracks_ad_hoc_regex_target(sim): """Tracked target registration remains valid when discovery returns concrete owner paths.""" obstacle_cfg = sim_utils.CuboidCfg( @@ -202,7 +195,10 @@ def test_legacy_multi_mesh_tracks_ad_hoc_regex_target(sim): def test_bvh_refit_tracks_moving_geometry(sim): - """Sliding a box under the sensor changes the hits, proving the BVH refits live.""" + """Sliding a box under the sensor changes the hits, proving the BVH refits live. + + After a carrier pose write, the first sensor read and the pose getter both resolve pending FK. + """ scene = InteractiveScene(RaycastTestSceneCfg(num_envs=1)) sim.reset() sensor = _step_and_read(sim, scene) @@ -224,13 +220,7 @@ def test_bvh_refit_tracks_moving_geometry(sim): distances = sensor.data.ray_distances.torch torch.testing.assert_close(distances, torch.full_like(distances, RAY_START_HEIGHT - 1.0), atol=1e-3, rtol=0) - -def test_sensor_reads_refresh_fk_after_carrier_pose_write(sim): - """The first sensor read and the pose getter both resolve pending FK after a carrier pose write.""" - scene = InteractiveScene(RaycastTestSceneCfg(num_envs=1)) - sim.reset() - sensor = _step_and_read(sim, scene) - initial_distances = sensor.data.ray_distances.torch.clone() + initial_distances = distances.clone() initial_positions = sensor.get_world_poses()[0].torch.clone() # Lift the carrier: the first data read after the write must see the refreshed body_q. diff --git a/source/isaaclab_newton/test/sensors/test_pva.py b/source/isaaclab_newton/test/sensors/test_pva.py index 805f0871724f..b008ca3ad3d4 100644 --- a/source/isaaclab_newton/test/sensors/test_pva.py +++ b/source/isaaclab_newton/test/sensors/test_pva.py @@ -12,7 +12,6 @@ import pytest import torch -import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg import isaaclab.sim as sim_utils @@ -65,135 +64,31 @@ def sim(): yield sim -def test_initialization_and_data_shapes(sim): - """The Newton PVA sensor initializes and exposes correctly shaped, populated buffers after one step.""" - scene_cfg = PvaTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - pva: Pva = scene["pva"] - assert pva.num_instances == 2 - - sim.step() - scene.update(sim.get_physics_dt()) - - assert pva.data.pos_w.torch.shape == (2, 3) - assert pva.data.quat_w.torch.shape == (2, 4) - assert pva.data.pose_w.torch.shape == (2, 7) - assert pva.data.lin_vel_b.torch.shape == (2, 3) - assert pva.data.ang_vel_b.torch.shape == (2, 3) - assert pva.data.lin_acc_b.torch.shape == (2, 3) - assert pva.data.ang_acc_b.torch.shape == (2, 3) - assert pva.data.projected_gravity_b.torch.shape == (2, 3) - # The cube starts above the ground plane, so the world position is populated rather than zero. - assert torch.all(pva.data.pos_w.torch[:, 2] > 0.0), ( - f"Expected positive z position, got {pva.data.pos_w.torch[:, 2]}" - ) - - def test_at_rest_reports_gravity_and_zero_velocity(sim): - """A settled PVA sensor reports unit gravity along body -Z and near-zero velocity.""" - scene_cfg = PvaTestSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - - # Cube falls from z=1.0 (bottom at z=0.9), reaches ground in ~86 steps at 200 Hz. - for _ in range(200): - sim.step() - scene.update(sim.get_physics_dt()) + """A settled PVA sensor reports unit gravity along body -Z and near-zero velocity. - pva: Pva = scene["pva"] - proj_grav = pva.data.projected_gravity_b.torch - lin_vel = pva.data.lin_vel_b.torch - ang_vel = pva.data.ang_vel_b.torch - - expected = torch.tensor([[0.0, 0.0, -1.0]], dtype=proj_grav.dtype, device=proj_grav.device).repeat(2, 1) - torch.testing.assert_close(proj_grav, expected, atol=0.05, rtol=0.0) - torch.testing.assert_close(lin_vel, torch.zeros_like(lin_vel), atol=0.05, rtol=0.0) - torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel), atol=0.05, rtol=0.0) - - -def test_reset(sim): - """Test that reset zeroes out PVA data.""" + While the cube still falls, PVA reports coordinate acceleration (from body_qdd), not proper + acceleration: (0, 0, -9.81) in the body frame of the upright cube, with a growing downward speed. + """ scene_cfg = PvaTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() - for _ in range(10): - sim.step() - scene.update(sim.get_physics_dt()) - pva: Pva = scene["pva"] + assert pva.num_instances == 2 - pos = pva.data.pos_w.torch - assert torch.any(pos != 0), "Expected non-zero data before reset" - - pva.reset() - - # Access internal buffers directly to avoid lazy re-evaluation via pva.data - # (the data property triggers _update_buffers_impl which would overwrite reset values). - pos = wp.to_torch(pva._data._pos_w) - lin_vel = wp.to_torch(pva._data._lin_vel_b) - ang_vel = wp.to_torch(pva._data._ang_vel_b) - lin_acc = wp.to_torch(pva._data._lin_acc_b) - ang_acc = wp.to_torch(pva._data._ang_acc_b) - quat = wp.to_torch(pva._data._quat_w) - - torch.testing.assert_close(pos, torch.zeros_like(pos)) - torch.testing.assert_close(lin_vel, torch.zeros_like(lin_vel)) - torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel)) - torch.testing.assert_close(lin_acc, torch.zeros_like(lin_acc)) - torch.testing.assert_close(ang_acc, torch.zeros_like(ang_acc)) - expected_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=quat.dtype, device=quat.device).repeat(2, 1) - torch.testing.assert_close(quat, expected_quat) - - -@configclass -class FreefallSceneCfg(InteractiveSceneCfg): - """Scene with a rigid cube and PVA but no ground plane (freefall).""" - - env_spacing = 2.0 - cube = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), - mass_props=sim_utils.MassCfg(mass=1.0), - collision_props=sim_utils.UsdPhysicsCollisionCfg(), - physics_material=sim_utils.RigidBodyMaterialCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.0, 0.0)), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)), - ) - - pva = PvaCfg( - prim_path="{ENV_REGEX_NS}/Cube", - ) - - -def test_freefall_reports_gravity_acceleration_and_growing_speed(sim): - """A freefalling body reports coordinate acceleration equal to gravity and a growing downward speed. - - PVA reports coordinate acceleration (from body_qdd), not proper acceleration. - In freefall, coordinate acceleration equals gravitational acceleration (~9.81 m/s^2 - downward). For an upright body, this is (0, 0, -9.81) in the body frame. - """ - scene_cfg = FreefallSceneCfg(num_envs=2) - scene = InteractiveScene(scene_cfg) - sim.reset() - + # Cube falls from z=1.0 (bottom at z=0.9), reaches ground in ~86 steps at 200 Hz. for _ in range(10): sim.step() scene.update(sim.get_physics_dt()) - pva: Pva = scene["pva"] + assert pva.data.quat_w.torch.shape == (2, 4) + torch.testing.assert_close(pva.data.pose_w.torch[:, :3], pva.data.pos_w.torch) + torch.testing.assert_close(pva.data.pose_w.torch[:, 3:], pva.data.quat_w.torch) lin_acc = pva.data.lin_acc_b.torch ang_acc = pva.data.ang_acc_b.torch - - # Coordinate acceleration in freefall should be ~(0, 0, -9.81) in body frame. expected_acc = torch.tensor([[0.0, 0.0, -9.81]], dtype=lin_acc.dtype, device=lin_acc.device).repeat(2, 1) torch.testing.assert_close(lin_acc, expected_acc, atol=0.5, rtol=0.0) - # Angular acceleration should be near zero (no torques in freefall). torch.testing.assert_close(ang_acc, torch.zeros_like(ang_acc), atol=0.05, rtol=0.0) @@ -201,11 +96,23 @@ def test_freefall_reports_gravity_acceleration_and_growing_speed(sim): for _ in range(40): sim.step() scene.update(sim.get_physics_dt()) - speed = torch.norm(pva.data.lin_vel_b.torch, dim=-1) assert torch.all(speed > 0.1), f"Expected non-zero velocity in freefall, got {speed}" assert torch.all(speed > early_speed), f"Expected speed to grow in freefall, got {early_speed} -> {speed}" + for _ in range(150): + sim.step() + scene.update(sim.get_physics_dt()) + + proj_grav = pva.data.projected_gravity_b.torch + lin_vel = pva.data.lin_vel_b.torch + ang_vel = pva.data.ang_vel_b.torch + + expected = torch.tensor([[0.0, 0.0, -1.0]], dtype=proj_grav.dtype, device=proj_grav.device).repeat(2, 1) + torch.testing.assert_close(proj_grav, expected, atol=0.05, rtol=0.0) + torch.testing.assert_close(lin_vel, torch.zeros_like(lin_vel), atol=0.05, rtol=0.0) + torch.testing.assert_close(ang_vel, torch.zeros_like(ang_vel), atol=0.05, rtol=0.0) + @configclass class OffsetRotatedSceneCfg(InteractiveSceneCfg): @@ -213,7 +120,7 @@ class OffsetRotatedSceneCfg(InteractiveSceneCfg): The cube is rotated 90 degrees about the X axis and the sensor has a +Z offset of 0.5 m in the body frame. This exercises the lever-arm - velocity/acceleration corrections and body-frame gravity projection. + position offset and body-frame gravity projection. """ env_spacing = 2.0 @@ -276,9 +183,10 @@ def test_no_stale_data_after_scene_reset(sim): Mirrors the PhysX equivalent. The PVA sensor's lazy ``data`` accessor must not refetch from the Newton rigid-body view here (the velocity buffer reflects the previous step and would - produce spurious finite-difference accelerations). + produce spurious finite-difference accelerations). A partial reset leaves other envs untouched + and a full sensor reset zeroes every output. """ - scene_cfg = PvaTestSceneCfg(num_envs=1) + scene_cfg = PvaTestSceneCfg(num_envs=2) scene = InteractiveScene(scene_cfg) sim.reset() scene.reset() @@ -292,8 +200,9 @@ def test_no_stale_data_after_scene_reset(sim): sim.step(render=False) scene.update(dt=sim.get_physics_dt()) - pre_reset_vel_mag = torch.linalg.norm(pva.data.lin_vel_b.torch, dim=-1).item() - assert pre_reset_vel_mag > 0.05, f"Expected non-zero velocity before reset; got {pre_reset_vel_mag!r}" + pre_reset_vel = pva.data.lin_vel_b.torch.clone() + pre_reset_vel_mag = torch.linalg.norm(pre_reset_vel, dim=-1) + assert (pre_reset_vel_mag > 0.05).all(), f"Expected non-zero velocity before reset; got {pre_reset_vel_mag!r}" # Reset the scene without writing fresh velocity/transform. The Newton velocity buffer # therefore still holds the pre-reset (falling) value. @@ -302,5 +211,15 @@ def test_no_stale_data_after_scene_reset(sim): post_reset_vel = pva.data.lin_vel_b.torch post_reset_acc = pva.data.lin_acc_b.torch - torch.testing.assert_close(post_reset_vel, torch.zeros_like(post_reset_vel)) - torch.testing.assert_close(post_reset_acc, torch.zeros_like(post_reset_acc)) + torch.testing.assert_close(post_reset_vel[0], torch.zeros_like(post_reset_vel[0])) + torch.testing.assert_close(post_reset_acc[0], torch.zeros_like(post_reset_acc[0])) + torch.testing.assert_close(post_reset_vel[1], pre_reset_vel[1]) + + # Full reset zeroes every environment. + pva.reset() + for name in ("pos_w", "lin_vel_b", "ang_vel_b", "lin_acc_b", "ang_acc_b"): + value = getattr(pva.data, name).torch + torch.testing.assert_close(value, torch.zeros_like(value), msg=name) + quat = pva.data.quat_w.torch + expected_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=quat.dtype, device=quat.device).repeat(2, 1) + torch.testing.assert_close(quat, expected_quat) diff --git a/source/isaaclab_newton/test/sensors/test_site_injection.py b/source/isaaclab_newton/test/sensors/test_site_injection.py index 686086f6a035..7ef68b1ed8ed 100644 --- a/source/isaaclab_newton/test/sensors/test_site_injection.py +++ b/source/isaaclab_newton/test/sensors/test_site_injection.py @@ -18,40 +18,12 @@ class TestTransformToVecQuat: - """Tests for the zero-copy view split utility.""" - - def test_1d_pos_quat_split(self): - """1D array: position is first 3 floats, quaternion is last 4.""" - t = wp.zeros(3, dtype=wp.transformf, device="cpu") - pos, quat = transform_to_vec_quat(t) - assert pos.shape == (3,) - assert quat.shape == (3,) - assert pos.dtype == wp.vec3f - assert quat.dtype == wp.quatf - - def test_2d_pos_quat_split(self): - """2D array: shapes are (N, M) with vec3f and quatf dtypes.""" - t = wp.zeros((2, 4), dtype=wp.transformf, device="cpu") - pos, quat = transform_to_vec_quat(t) - assert pos.shape == (2, 4) - assert quat.shape == (2, 4) - assert pos.dtype == wp.vec3f - assert quat.dtype == wp.quatf - - def test_zero_copy_1d(self): - """Writes through pos/quat views are reflected in the original transform array.""" - t = wp.zeros(1, dtype=wp.transformf, device="cpu") - pos, quat = transform_to_vec_quat(t) - # Write known values through the views - pos.numpy()[0] = (1.0, 2.0, 3.0) - quat.numpy()[0] = (0.0, 0.0, 0.0, 1.0) - floats = t.view(wp.float32).numpy() - assert list(floats[0]) == pytest.approx([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]) + """Error paths of the zero-copy view split utility; values are covered by the frame-transformer tests.""" def test_invalid_ndim_raises(self): - """Passing a 0D or 4D array raises an error.""" - with pytest.raises((ValueError, IndexError)): - transform_to_vec_quat(wp.zeros((), dtype=wp.transformf, device="cpu")) + """Passing a 4D array raises the documented ValueError rather than a Warp view error.""" + with pytest.raises(ValueError, match="ndim=4"): + transform_to_vec_quat(wp.zeros((1, 1, 1, 1), dtype=wp.transformf, device="cpu")) def test_wrong_dtype_raises(self): """Passing wrong dtype raises TypeError.""" @@ -93,36 +65,9 @@ def test_global_site_entry_is_int_none_tuple(self): global_idx, per_world = entry assert isinstance(global_idx, int) assert per_world is None - - def test_global_site_pending_cleared(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {(None, False, tuple(xform)): ("ft_0", xform)} - NewtonManager._cl_inject_sites_fallback() - assert len(NewtonManager._cl_pending_sites) == 0 -class TestFallbackLocalSingleBody: - """Single-body local site must produce a (None, [[idx]]) entry — one world.""" - - def setup_method(self): - NewtonManager.clear() - NewtonManager._builder = MockBuilder(["Robot/base", "Robot/hand"]) - - def test_single_body_entry_shape(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {("Robot/base", False, tuple(xform)): ("ft_0", xform)} - NewtonManager._cl_inject_sites_fallback() - - entry = NewtonManager._cl_site_index_map["ft_0"] - global_idx, per_world = entry - assert global_idx is None - assert isinstance(per_world, list) - assert len(per_world) == 1 # one world - assert len(per_world[0]) == 1 # one match - assert isinstance(per_world[0][0], int) - - class TestFallbackLocalWildcard: """Wildcard local site matching N bodies must produce (None, [[idx0..idxN-1]]) — one world.""" @@ -204,11 +149,6 @@ def _make_site_map( class TestSourceValidation: - def test_valid_source_one_per_env(self): - site_map = _make_site_map([[10], [20]], []) - indices, _ = FrameTransformer._validate_site_map("source", "/Robot/base", [], [], site_map, num_envs=2) - assert indices == [10, 20] - def test_source_wrong_env_count_raises(self): # site map has 1 world entry but num_envs=2 site_map = _make_site_map([[10]], []) @@ -227,20 +167,6 @@ def test_source_two_in_env_raises(self): class TestTargetValidation: - def test_valid_single_target_per_env(self): - site_map = _make_site_map([[10], [20]], [[[30], [40]]]) - _, tgt = FrameTransformer._validate_site_map( - "source", "/Robot/base", ["target_0"], ["/Robot/hand"], site_map, num_envs=2 - ) - assert tgt[0] == [[30], [40]] - - def test_valid_wildcard_two_bodies_per_env(self): - site_map = _make_site_map([[10], [20]], [[[30, 31], [40, 41]]]) - _, tgt = FrameTransformer._validate_site_map( - "source", "/Robot/base", ["target_0"], ["/Robot/foot.*"], site_map, num_envs=2 - ) - assert tgt[0] == [[30, 31], [40, 41]] - def test_target_zero_bodies_raises(self): site_map = _make_site_map([[10], [20]], [[[], []]]) with pytest.raises(ValueError, match="matched no bodies"): @@ -282,63 +208,3 @@ def test_zero_targets_shapes_refs(self): assert refs == [0, 0] assert names == [] assert tgt_per_tgt == [] - - -class TestSingleTarget: - def test_one_env_one_target(self): - """1 env, 1 target: [src, tgt] shapes, [world_orig, src] refs.""" - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10], - target_per_world=[[[20]]], - target_frame_body_names=["hand"], - shape_labels={}, - world_origin_idx=0, - num_envs=1, - ) - assert shapes == [10, 20] - assert refs == [0, 10] - assert names == ["hand"] - - def test_two_envs_two_targets(self): - """2 envs, 2 targets: stride-2 interleaved layout.""" - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10, 11], - target_per_world=[[[20], [21]], [[30], [31]]], - target_frame_body_names=["arm", "hand"], - shape_labels={}, - world_origin_idx=0, - num_envs=2, - ) - assert shapes == [10, 20, 30, 11, 21, 31] - assert refs == [0, 10, 10, 0, 11, 11] - assert names == ["arm", "hand"] - - -class TestWildcardExpansion: - def test_wildcard_two_bodies_per_env(self): - """Wildcard: 2 bodies per env expand to 2 target entries with names derived from shape_labels.""" - shape_labels = {20: "FL_foot/label_0", 21: "FL_foot/label_0", 22: "FR_foot/label_0", 23: "FR_foot/label_0"} - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10, 11], - target_per_world=[[[20, 22], [21, 23]]], - target_frame_body_names=["foot"], - shape_labels=shape_labels, - world_origin_idx=0, - num_envs=2, - ) - assert shapes == [10, 20, 22, 11, 21, 23] - assert refs == [0, 10, 10, 0, 11, 11] - assert tgt_per_tgt == [[20, 21], [22, 23]] - assert names == ["FL_foot", "FR_foot"] - - def test_wildcard_single_body_uses_config_name(self): - """Single body match: config name is used regardless of shape_labels.""" - names, tgt_per_tgt, shapes, refs = _call( - source_indices=[10, 11], - target_per_world=[[[20], [21]]], - target_frame_body_names=["foot"], - shape_labels={}, - world_origin_idx=0, - num_envs=2, - ) - assert names == ["foot"] diff --git a/source/isaaclab_newton/test/sim/test_cable_usd_import.py b/source/isaaclab_newton/test/sim/test_cable_usd_import.py index 1ddf6b04d040..79d36c9145c0 100644 --- a/source/isaaclab_newton/test/sim/test_cable_usd_import.py +++ b/source/isaaclab_newton/test/sim/test_cable_usd_import.py @@ -32,17 +32,11 @@ def _import_cable_joint_stiffness(**material_kwargs) -> list[float]: return [builder.joint_target_ke[dof0 + offset] for offset in range(4)] -def test_newton_cable_shear_and_twist_fall_back_when_unset(): - """Test that unset shear/twist reuse the stretch/bend stiffness.""" - stiffness = _import_cable_joint_stiffness() - - assert stiffness[_SHEAR] == pytest.approx(stiffness[_STRETCH]) - assert stiffness[_TWIST] == pytest.approx(stiffness[_BEND]) - - def test_newton_cable_authored_shear_and_twist_override_fallbacks(): - """Test that authored moduli decouple shear from stretch and twist from bend.""" + """Test that unset shear/twist reuse stretch/bend and authored moduli decouple them.""" fallback = _import_cable_joint_stiffness() + assert fallback[_SHEAR] == pytest.approx(fallback[_STRETCH]) + assert fallback[_TWIST] == pytest.approx(fallback[_BEND]) stiffness = _import_cable_joint_stiffness(shear_stiffness=9.0e9, twist_stiffness=7.0e7) # Stretch and bend are untouched; only the newly authored degrees of freedom move. diff --git a/source/isaaclab_newton/test/sim/test_mpm_visualization.py b/source/isaaclab_newton/test/sim/test_mpm_visualization.py index d201c6eae789..3b7e283e0988 100644 --- a/source/isaaclab_newton/test/sim/test_mpm_visualization.py +++ b/source/isaaclab_newton/test/sim/test_mpm_visualization.py @@ -44,7 +44,10 @@ def _create_visualization(monkeypatch, visual_material=None): def test_each_environment_renders_its_own_particle_slice(monkeypatch): - """Every environment gets a points prim carrying its own positions, plus the shared widths and color.""" + """Every environment gets a world-frame points prim with its own positions, plus the shared widths and color. + + Points are authored in the world frame, so the prims must reset the environment's xform stack. + """ stage, prim_paths = _create_visualization(monkeypatch) assert prim_paths == _PRIM_PATHS @@ -53,14 +56,7 @@ def test_each_environment_renders_its_own_particle_slice(monkeypatch): np.testing.assert_array_equal(np.asarray(points.GetPointsAttr().Get()), _POSITIONS[env_idx]) np.testing.assert_array_equal(np.asarray(points.GetWidthsAttr().Get()), _WIDTHS) assert points.GetDisplayColorAttr().Get() == [Gf.Vec3f(*_COLOR)] - - -def test_particle_clouds_ignore_the_inherited_environment_transform(monkeypatch): - """Points are authored in the world frame, so the prims must reset the environment's xform stack.""" - stage, prim_paths = _create_visualization(monkeypatch) - - for prim_path in prim_paths: - assert UsdGeom.Points(stage.GetPrimAtPath(prim_path)).GetResetXformStack() + assert points.GetResetXformStack() def test_prim_path_count_must_match_environment_count(monkeypatch): diff --git a/source/isaaclab_newton/test/sim/test_newton_schemas.py b/source/isaaclab_newton/test/sim/test_newton_schemas.py index 6619792aae68..9e07c1caa3bb 100644 --- a/source/isaaclab_newton/test/sim/test_newton_schemas.py +++ b/source/isaaclab_newton/test/sim/test_newton_schemas.py @@ -19,11 +19,9 @@ MujocoJointDrivePropertiesCfg, MujocoRigidBodyPropertiesCfg, NewtonArticulationRootPropertiesCfg, - NewtonCollisionPropertiesCfg, NewtonJointDrivePropertiesCfg, NewtonMaterialPropertiesCfg, NewtonMeshCollisionPropertiesCfg, - NewtonRigidBodyPropertiesCfg, NewtonSDFCollisionPropertiesCfg, ) @@ -64,15 +62,6 @@ def _has_authored_api_schema(prim, schema_name: str) -> bool: # --------------------------------------------------------------------------- -@pytest.mark.isaacsim_ci -def test_newton_rigid_body_inherits_field_routing(setup_sim): - """Inherited disable_gravity must use the PhysX namespace consumed by Newton.""" - prim = sim_utils.create_prim("/World/newton_body", prim_type="Cube") - schemas.define_rigid_body_properties("/World/newton_body", NewtonRigidBodyPropertiesCfg(disable_gravity=True)) - assert prim.GetAttribute("physxRigidBody:disableGravity").Get() is True - assert not prim.GetAttribute("physics:disableGravity").IsValid() - - @pytest.mark.isaacsim_ci def test_mujoco_gravcomp_authored_only_when_set(setup_sim): """gravcomp=0.5 writes mjc:gravcomp=0.5; gravcomp=None leaves the attribute unauthored.""" @@ -128,32 +117,6 @@ def test_joint_drive_max_velocity_routes_to_physx_namespace(setup_sim, cfg_type) assert joint.GetPrim().GetAttribute("physxJoint:maxJointVelocity").Get() == pytest.approx(math.degrees(5.0)) -# --------------------------------------------------------------------------- -# Newton collision -# --------------------------------------------------------------------------- - - -@pytest.mark.isaacsim_ci -def test_newton_collision_schema_applied_only_when_set(setup_sim): - """contact_margin=0.01 writes newton:contactMargin and applies NewtonCollisionAPI; all-None applies nothing.""" - stage = sim_utils.get_current_stage() - sim_utils.create_prim("/World/col_newton", prim_type="Cube", translation=(2.0, 0.0, 0.5)) - schemas.define_collision_properties( - "/World/col_newton", NewtonCollisionPropertiesCfg(contact_margin=0.01, contact_offset=0.02, rest_offset=0.01) - ) - prim = stage.GetPrimAtPath("/World/col_newton") - assert prim.GetAttribute("physxCollision:contactOffset").Get() == pytest.approx(0.02) - assert prim.GetAttribute("physxCollision:restOffset").Get() == pytest.approx(0.01) - assert not prim.GetAttribute("physics:contactOffset").IsValid() - assert not prim.GetAttribute("physics:restOffset").IsValid() - assert prim.GetAttribute("newton:contactMargin").Get() == pytest.approx(0.01) - assert "NewtonCollisionAPI" in prim.GetAppliedSchemas() - - sim_utils.create_prim("/World/col_newton2", prim_type="Cube", translation=(3.0, 0.0, 0.5)) - schemas.define_collision_properties("/World/col_newton2", NewtonCollisionPropertiesCfg()) - assert "NewtonCollisionAPI" not in stage.GetPrimAtPath("/World/col_newton2").GetAppliedSchemas() - - # --------------------------------------------------------------------------- # Newton material # --------------------------------------------------------------------------- @@ -161,42 +124,28 @@ def test_newton_collision_schema_applied_only_when_set(setup_sim): @pytest.mark.isaacsim_ci def test_newton_material_schema_applied_only_when_set(setup_sim): - """Newton friction fields write newton:* attributes and apply NewtonMaterialAPI; all-None applies nothing.""" - mat_cfg = NewtonMaterialPropertiesCfg(torsional_friction=0.3, rolling_friction=0.001) + """Newton material fields write newton:* attributes and apply NewtonMaterialAPI; all-None applies nothing.""" + mat_cfg = NewtonMaterialPropertiesCfg( + torsional_friction=0.3, + rolling_friction=0.001, + contact_stiffness=1.0e4, + contact_damping=250.0, + contact_friction_gain=40.0, + contact_adhesion=0.02, + ) prim = spawn_rigid_body_material("/World/newton_mat", mat_cfg) assert prim.GetAttribute("newton:torsionalFriction").Get() == pytest.approx(0.3) assert prim.GetAttribute("newton:rollingFriction").Get() == pytest.approx(0.001) + assert prim.GetAttribute("newton:contactStiffness").Get() == pytest.approx(1.0e4) + assert prim.GetAttribute("newton:contactDamping").Get() == pytest.approx(250.0) + assert prim.GetAttribute("newton:contactFrictionGain").Get() == pytest.approx(40.0) + assert prim.GetAttribute("newton:contactAdhesion").Get() == pytest.approx(0.02) assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() prim = spawn_rigid_body_material("/World/newton_mat2", NewtonMaterialPropertiesCfg()) assert "NewtonMaterialAPI" not in prim.GetAppliedSchemas() -@pytest.mark.isaacsim_ci -def test_newton_material_fragment_composes_with_usd_physics_fragment(setup_sim): - """NewtonMaterialCfg is a rigid-body material fragment (backend symmetry with the PhysX - fragment): it must compose in a fragment list with UsdPhysicsRigidBodyMaterialCfg and author - both the ``newton:*`` and solver-common ``physics:*`` namespaces on the same material prim.""" - from isaaclab_newton.sim.spawners.materials import NewtonMaterialCfg - - from isaaclab.sim.spawners.materials.physics_materials import spawn_rigid_body_material_from_fragments - from isaaclab.sim.spawners.materials.physics_materials_cfg import UsdPhysicsRigidBodyMaterialCfg - - prim = spawn_rigid_body_material_from_fragments( - "/World/newton_mat_frag", - [ - UsdPhysicsRigidBodyMaterialCfg(static_friction=0.6, dynamic_friction=0.5), - NewtonMaterialCfg(torsional_friction=0.3, rolling_friction=0.001), - ], - ) - assert bool(UsdPhysics.MaterialAPI(prim)) - assert prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(0.6) - assert prim.GetAttribute("physics:dynamicFriction").Get() == pytest.approx(0.5) - assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() - assert prim.GetAttribute("newton:torsionalFriction").Get() == pytest.approx(0.3) - assert prim.GetAttribute("newton:rollingFriction").Get() == pytest.approx(0.001) - - @pytest.mark.isaacsim_ci def test_newton_material_fragment_authors_all_six_newton_attrs(setup_sim): """Regression test: Newton's USD material schema resolver (``SchemaResolverNewton``) reads six @@ -204,23 +153,29 @@ def test_newton_material_fragment_authors_all_six_newton_attrs(setup_sim): (``contactStiffness``/``contactDamping``/``contactFrictionGain``/``contactAdhesion``) that replace the deprecated per-shape ``ke``/``kd``/``kf``/``ka`` parameters. All six must round-trip through :class:`~isaaclab_newton.sim.spawners.materials.NewtonMaterialCfg`, even though the - generated ``NewtonMaterialAPI`` schema currently only declares the two friction attributes.""" + generated ``NewtonMaterialAPI`` schema currently only declares the two friction attributes. + + The fragment also composes with :class:`UsdPhysicsRigidBodyMaterialCfg` on the same prim.""" from isaaclab_newton.sim.spawners.materials import NewtonMaterialCfg from newton._src.usd.schema_resolver import PrimType from newton._src.usd.schemas import SchemaResolverNewton from isaaclab.sim.spawners.materials import spawn_rigid_body_material_from_fragments + from isaaclab.sim.spawners.materials.physics_materials_cfg import UsdPhysicsRigidBodyMaterialCfg prim = spawn_rigid_body_material_from_fragments( "/World/newton_mat_contact", - NewtonMaterialCfg( - torsional_friction=0.3, - rolling_friction=0.001, - contact_stiffness=2500.0, - contact_damping=100.0, - contact_friction_gain=1000.0, - contact_adhesion=0.01, - ), + [ + UsdPhysicsRigidBodyMaterialCfg(static_friction=0.6, dynamic_friction=0.5), + NewtonMaterialCfg( + torsional_friction=0.3, + rolling_friction=0.001, + contact_stiffness=2500.0, + contact_damping=100.0, + contact_friction_gain=1000.0, + contact_adhesion=0.01, + ), + ], ) expected = { "mu_torsional": 0.3, @@ -231,6 +186,9 @@ def test_newton_material_fragment_authors_all_six_newton_attrs(setup_sim): "ka": 0.01, } + assert bool(UsdPhysics.MaterialAPI(prim)) + assert prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(0.6) + assert prim.GetAttribute("physics:dynamicFriction").Get() == pytest.approx(0.5) assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() resolver = SchemaResolverNewton() assert set(resolver.mapping[PrimType.MATERIAL]) == set(expected) @@ -264,32 +222,6 @@ def test_newton_articulation_root_schema_applied_only_when_set(setup_sim): assert "NewtonArticulationRootAPI" not in stage.GetPrimAtPath("/World/nart2").GetAppliedSchemas() -# --------------------------------------------------------------------------- -# Newton mesh collision (max_hull_vertices, NewtonMeshCollisionAPI) -# --------------------------------------------------------------------------- - - -@pytest.mark.isaacsim_ci -def test_newton_mesh_collision_schema_applied_only_when_set(setup_sim): - """max_hull_vertices=64 writes newton:maxHullVertices and applies NewtonMeshCollisionAPI; None applies nothing.""" - stage = sim_utils.get_current_stage() - sim_utils.create_prim("/World/mesh_col", prim_type="Cube", translation=(4.0, 0.0, 0.5)) - schemas.define_mesh_collision_properties( - "/World/mesh_col", - NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull", max_hull_vertices=64), - ) - prim = stage.GetPrimAtPath("/World/mesh_col") - assert prim.GetAttribute("newton:maxHullVertices").Get() == 64 - assert "NewtonMeshCollisionAPI" in prim.GetAppliedSchemas() - - sim_utils.create_prim("/World/mesh_col2", prim_type="Cube", translation=(5.0, 0.0, 0.5)) - schemas.define_mesh_collision_properties( - "/World/mesh_col2", - NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull"), - ) - assert "NewtonMeshCollisionAPI" not in stage.GetPrimAtPath("/World/mesh_col2").GetAppliedSchemas() - - # --------------------------------------------------------------------------- # Newton SDF collision # --------------------------------------------------------------------------- @@ -351,24 +283,6 @@ def test_newton_sdf_collision_schema_not_applied_without_sdf_fields(setup_sim): assert not _has_authored_api_schema(prim, "NewtonSDFCollisionAPI") -# --------------------------------------------------------------------------- -# Class hierarchy contract: Mujoco IS-A Newton -# --------------------------------------------------------------------------- - - -def test_mujoco_isinstance_newton(): - """MujocoXxxCfg instances must be isinstance of their Newton parent. - - The auto-enable spawner logic and any future polymorphic dispatch on - ``isinstance(cfg, NewtonRigidBodyPropertiesCfg)`` depends on this contract. - """ - mjc_rigid = MujocoRigidBodyPropertiesCfg(gravcomp=0.5) - assert isinstance(mjc_rigid, NewtonRigidBodyPropertiesCfg) - - mjc_joint = MujocoJointDrivePropertiesCfg(actuatorgravcomp=True) - assert isinstance(mjc_joint, NewtonJointDrivePropertiesCfg) - - # --------------------------------------------------------------------------- # Multi-namespace mixed write — verify per-declaring-class MRO routing keeps # fields owned by different classes in different namespaces on the same prim. @@ -404,15 +318,10 @@ def test_newton_mesh_collision_mixed_namespace_write(setup_sim): assert "NewtonCollisionAPI" in applied assert "NewtonMeshCollisionAPI" in applied - -@pytest.mark.isaacsim_ci -def test_newton_legacy_cfg_authors_contact_attrs(setup_sim): - """The legacy Newton material cfg authors all newton:* attributes the fragment authors.""" - mat_cfg = NewtonMaterialPropertiesCfg( - contact_stiffness=1.0e4, contact_damping=250.0, contact_friction_gain=40.0, contact_adhesion=0.02 + # Without max_hull_vertices the mesh collision schema stays unapplied. + sim_utils.create_prim("/World/mesh_col2", prim_type="Cube", translation=(5.0, 0.0, 0.5)) + schemas.define_mesh_collision_properties( + "/World/mesh_col2", + NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull"), ) - prim = spawn_rigid_body_material("/World/newton_mat_contact", mat_cfg) - assert prim.GetAttribute("newton:contactStiffness").Get() == pytest.approx(1.0e4) - assert prim.GetAttribute("newton:contactDamping").Get() == pytest.approx(250.0) - assert prim.GetAttribute("newton:contactFrictionGain").Get() == pytest.approx(40.0) - assert prim.GetAttribute("newton:contactAdhesion").Get() == pytest.approx(0.02) + assert "NewtonMeshCollisionAPI" not in stage.GetPrimAtPath("/World/mesh_col2").GetAppliedSchemas() diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index 831a154ed7bd..0182c0734597 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -13,7 +13,7 @@ import sys from pathlib import Path -from isaaclab.test.utils import test_devices +from isaaclab.test.utils import DeviceScope, test_devices sys.path.insert(0, str(Path(__file__).resolve().parents[1])) sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sim")) @@ -109,26 +109,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: # ================================================================== -@pytest.mark.parametrize("device", test_devices()) -def test_reject_body_and_shape_paths(device): - """FrameView rejects prim paths that resolve to a Newton physics body or collision shape.""" - ctx = _sim_context(device, num_envs=2) - sim = ctx.__enter__() - sim._app_control_on_stop_handle = None - InteractiveScene(_SceneCfg(num_envs=2, env_spacing=2.0)) - sim.reset() - - with pytest.raises(ValueError, match="physics body"): - FrameView("/World/envs/env_[^/]+/Cube", device=device) - - shape_labels = list(NewtonManager.get_model().shape_label) - assert shape_labels, "scene must contribute at least one collision shape" - with pytest.raises(ValueError, match="collision shape"): - FrameView(shape_labels[0], device=device) - ctx.__exit__(None, None, None) - - -@pytest.mark.parametrize("device", test_devices()) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_non_colliding_shapes_after_finalize(device): """Non-colliding site and visual shapes remain valid after finalization.""" ctx = _sim_context(device, num_envs=1) @@ -161,13 +142,13 @@ def test_non_colliding_shapes_after_finalize(device): ctx.__exit__(None, None, None) -@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) -def test_body_local_frame_resolves_before_and_after_reset(device): - """A body-local site resolves through the ClonePlan before reset and from Newton body labels after it. +@pytest.mark.parametrize("device", test_devices()) +def test_body_local_frame_resolves_from_body_labels_after_reset(device): + """A body-local site created after reset resolves from the finalized Newton body labels. - Only the prototype env authors the child prim on the stage; the view created before ``sim.reset`` - expands it through the ClonePlan, while the view created afterwards resolves the same frame directly - from the finalized Newton body labels. Both must agree with the parent body poses. + Only the prototype env authors the child prim on the stage, so the view must expand it through the + Newton body labels rather than the stage. The ClonePlan path before reset is covered by the shared + contract tests. """ num_envs = 3 ctx = _sim_context(device, num_envs=num_envs) @@ -180,22 +161,22 @@ def test_body_local_frame_resolves_before_and_after_reset(device): assert not stage.GetPrimAtPath("/World/envs/env_1/Cube").IsValid() sim_utils.create_prim("/World/envs/env_0/Cube/CameraMount", translation=CHILD_OFFSET) - clone_plan_view = FrameView("/World/envs/env_[^/]+/Cube/CameraMount", device=device) sim.reset() label_view = FrameView("/World/envs/env_[^/]+/Cube/CameraMount", device=device) - assert clone_plan_view.count == num_envs assert label_view.count == num_envs assert not stage.GetPrimAtPath("/World/envs/env_1/Cube/CameraMount").IsValid() expected = _get_body_positions(num_envs, device) + torch.tensor(CHILD_OFFSET, device=device) - torch.testing.assert_close(clone_plan_view.get_world_poses()[0].torch, expected, atol=1e-5, rtol=0) torch.testing.assert_close(label_view.get_world_poses()[0].torch, expected, atol=1e-5, rtol=0) ctx.__exit__(None, None, None) -@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) def test_close_before_reset_cancels_deferred_initialization(device): - """A view closed before the Newton model exists must not initialize on ``PHYSICS_READY``.""" + """A view closed before the Newton model exists must not initialize on ``PHYSICS_READY``. + + After reset, new views over Newton bodies or collision shapes are rejected. + """ num_envs = 3 ctx = _sim_context(device, num_envs=num_envs) sim = ctx.__enter__() @@ -210,6 +191,14 @@ def test_close_before_reset_cancels_deferred_initialization(device): sim.reset() assert view.count == 0, "a closed view still initialized from the physics-ready callback" + + # FrameView rejects prim paths that resolve to a Newton physics body or collision shape. + with pytest.raises(ValueError, match="physics body"): + FrameView("/World/envs/env_[^/]+/Cube", device=device) + shape_labels = list(NewtonManager.get_model().shape_label) + assert shape_labels, "scene must contribute at least one collision shape" + with pytest.raises(ValueError, match="collision shape"): + FrameView(shape_labels[0], device=device) ctx.__exit__(None, None, None) diff --git a/tools/_file_scheduler.py b/tools/_file_scheduler.py new file mode 100644 index 000000000000..06111b5e3e83 --- /dev/null +++ b/tools/_file_scheduler.py @@ -0,0 +1,165 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Run test files concurrently within a fixed budget of slots. + +``tools/conftest.py`` runs every test file in its own pytest process. Most of a process's life is spent +starting up -- booting Kit, importing, collecting -- rather than testing, so running several files at once +overlaps that fixed cost instead of paying it file after file. Splitting one file across ``pytest-xdist`` +workers does the opposite for most files, since every worker pays the startup again; it only pays off for +the few long files whose tests dwarf their startup, which is why a file may ask for several slots. + +The scheduler knows nothing about pytest. It is given :class:`TestFileJob` entries and a ``run`` callable, +and guarantees: + +* at most ``max_slots`` slots are in use, a job holding :attr:`TestFileJob.slots` of them; +* jobs start in the order they are given, so a job that does not fit yet is not overtaken (a wide job + would otherwise wait forever behind a stream of narrow ones); +* rendering jobs never overlap one another, though other jobs may overtake a rendering job waiting its turn. + The first renderer in a fresh container compiles shaders into a cache the rest reuse, and several starting + at once each compile the same shaders; renderers also share one log file, so overlapping ones would + garble each other's diagnostics. + +With ``max_slots <= 1`` jobs run one after the other on the calling thread, exactly as a plain loop would. +""" + +from __future__ import annotations + +import queue +import threading +from collections.abc import Callable, Iterable +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass +from typing import Generic, TypeVar + +R = TypeVar("R") + + +@dataclass(frozen=True) +class TestFileJob: + """One test file to run.""" + + __test__ = False # not a pytest test class, despite the name + + path: str + """Test file path; also the key of its result.""" + + slots: int = 1 + """Slots the job holds while it runs, e.g. one per ``pytest-xdist`` worker; more than the budget holds them all.""" + + renders: bool = False + """Whether the job starts a renderer, and so never runs alongside another job that does.""" + + +@dataclass(frozen=True) +class JobContext: + """What the scheduler tells a job it starts.""" + + renderer_cold: bool + """Whether this job renders and no rendering job had finished starting before it, so it compiles shaders.""" + + mark_started: Callable[[], None] + """Call once the job's process has finished starting; a rendering job's start warms the shader cache.""" + + +class _Scheduler(Generic[R]): + def __init__(self, run: Callable[[TestFileJob, JobContext], R], max_slots: int): + self._run = run + self._max_slots = max_slots + self._renderer_ready = threading.Event() + self._finished: queue.Queue[tuple[TestFileJob, R | BaseException]] = queue.Queue() + + def _context(self, job: TestFileJob) -> JobContext: + def mark_started(): + if job.renders: + self._renderer_ready.set() + + return JobContext(renderer_cold=job.renders and not self._renderer_ready.is_set(), mark_started=mark_started) + + def run_serially(self, jobs: Iterable[TestFileJob]) -> dict[str, R]: + results = {} + for job in jobs: + results[job.path] = self._run(job, self._context(job)) + return results + + def run_concurrently(self, jobs: Iterable[TestFileJob]) -> dict[str, R]: + source = iter(jobs) + pending: list[TestFileJob] = [] + running: list[TestFileJob] = [] + results: dict[str, R] = {} + order: list[str] = [] + exhausted = False + + def free_slots() -> int: + return self._max_slots - sum(self._cost(job) for job in running) + + def next_startable() -> TestFileJob | None: + nonlocal exhausted + index = 0 + while True: + # Pull only as far as the first startable job: a shared work queue hands a claimed file to no one else. + if index == len(pending): + if exhausted or free_slots() == 0: + return None + try: + pending.append(next(source)) + except StopIteration: + exhausted = True + return None + job = pending[index] + if job.renders and any(other.renders for other in running): + index += 1 + continue + return job if self._cost(job) <= free_slots() else None + + with ThreadPoolExecutor(max_workers=self._max_slots, thread_name_prefix="test-file") as pool: + while True: + while (job := next_startable()) is not None: + pending.remove(job) + running.append(job) + order.append(job.path) + pool.submit(self._run_and_report, job, self._context(job)) + if not running: + break + job, outcome = self._finished.get() + running.remove(job) + if isinstance(outcome, BaseException): + raise outcome + results[job.path] = outcome + return {path: results[path] for path in order} + + def _run_and_report(self, job: TestFileJob, context: JobContext) -> None: + try: + outcome = self._run(job, context) + except BaseException as error: # re-raised on the scheduling thread + outcome = error + self._finished.put((job, outcome)) + + def _cost(self, job: TestFileJob) -> int: + return max(1, min(job.slots, self._max_slots)) + + + +def run_test_files( + jobs: Iterable[TestFileJob], run: Callable[[TestFileJob, JobContext], R], max_slots: int = 1 +) -> dict[str, R]: + """Run ``run`` for every job, at most ``max_slots`` slots at a time. + + Args: + jobs: Jobs in the order they should start. Consumed lazily, so it may be a generator that claims + files from a queue shared with other runners. + run: Runs one job and returns its result. Called on a worker thread when ``max_slots > 1``. + max_slots: Slots available; a job wider than this is given all of them. + + Returns: + Each job's result keyed by :attr:`TestFileJob.path`, in the order the jobs started. + + Raises: + BaseException: Whatever ``run`` raised, once the jobs already running have finished. + """ + scheduler = _Scheduler(run, max_slots) + if max_slots <= 1: + return scheduler.run_serially(jobs) + return scheduler.run_concurrently(jobs) diff --git a/tools/conftest.py b/tools/conftest.py index a6120bb4619d..ef87419bc877 100644 --- a/tools/conftest.py +++ b/tools/conftest.py @@ -10,7 +10,9 @@ import signal import subprocess import sys +import threading import time +from collections.abc import Callable from dataclasses import dataclass import pytest @@ -26,6 +28,7 @@ import test_settings as test_settings # isort: skip from crash_journal import JOURNAL_ENV_VAR, create_crash_report # isort: skip from _device_split import DEVICE_SPLIT_PASSES, is_device_split_file # isort: skip +from _file_scheduler import JobContext, TestFileJob, run_test_files # isort: skip logging.basicConfig(level=logging.INFO, format="%(message)s") logger = logging.getLogger(__name__) @@ -49,12 +52,22 @@ def pytest_ignore_collect(collection_path, config): AppLauncher prints ``[ISAACLAB] AppLauncher initialization complete`` to ``sys.__stderr__`` (never suppressed) when Kit finishes initializing, and pytest -prints ``collected N items`` to stdout after collection. If neither appears +prints ``collected N items`` to stdout after collection (``N workers [M items]`` +under ``pytest-xdist``, once every worker has collected). If none appears within this deadline the process is treated as hung. Kit startup can exceed 60 s on cold CI workers, so this catches real startup hangs without killing legitimate slow launches. """ +TEST_JOBS_ENV_VAR = "TEST_JOBS" +"""Environment variable naming how many test-file slots a run may use at once; unset or ``1`` runs files one by one. + +Each file still runs in its own pytest process. A file listed in :data:`test_settings.PYTEST_WORKERS` holds +several slots and splits its tests across that many ``pytest-xdist`` workers, one in +:data:`test_settings.EXCLUSIVE_TESTS` holds them all, and any other file holds one. See +``tools/_file_scheduler.py`` for the scheduling rules. +""" + STARTUP_HANG_RETRIES = 2 """Number of times to retry a test that hangs during startup before giving up.""" @@ -163,7 +176,7 @@ def resolve_exit_code(num_failing: int, num_timeout: int, num_crashed: int, num_ return 0 -def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1): +def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1, echo=True): """Read whatever is readable on the child's pipes, echoing it as it arrives. Args: @@ -171,11 +184,13 @@ def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1): stdout_fd: Read end of the child's stdout, already non-blocking. stderr_fd: Read end of the child's stderr, already non-blocking. timeout: Seconds to wait for either pipe to become readable. + echo: Whether to echo what was read to this process's own streams. Returns: - Tuple of ``(stdout_bytes, stderr_bytes)`` read in this pass. Both are - echoed to this process's own streams before being returned, so output - reaches the job log while the test is still running. + Tuple of ``(stdout_bytes, stderr_bytes)`` read in this pass. When + ``echo`` is set, both are echoed to this process's own streams before + being returned, so output reaches the job log while the test is still + running. """ stdout_chunk = b"" stderr_chunk = b"" @@ -188,20 +203,82 @@ def _drain_ready_output(process, stdout_fd, stderr_fd, timeout=0.1): chunk = process.stdout.read(1024) if chunk: stdout_chunk += chunk - sys.stdout.buffer.write(chunk) - sys.stdout.buffer.flush() + if echo: + sys.stdout.buffer.write(chunk) + sys.stdout.buffer.flush() elif fd == stderr_fd: chunk = process.stderr.read(1024) if chunk: stderr_chunk += chunk - sys.stderr.buffer.write(chunk) - sys.stderr.buffer.flush() + if echo: + sys.stderr.buffer.write(chunk) + sys.stderr.buffer.flush() except OSError: time.sleep(timeout) return stdout_chunk, stderr_chunk -def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): +@dataclass(frozen=True) +class _CaptureOptions: + """How a test process is watched, beyond its command and limits.""" + + echo: bool = True + """Stream the process's output to the job log as it arrives. Off when files run concurrently, whose output + would interleave; each pass's output is then printed whole once it ends.""" + + workers: int = 1 + """``pytest-xdist`` workers the process splits into; a hang dump asks each of them for a stack as well.""" + + on_started: Callable[[], None] | None = None + """Called once the process has finished starting up, i.e. reached pytest collection.""" + + +def _test_jobs() -> int: + """Return the number of test-file slots :data:`TEST_JOBS_ENV_VAR` allows, at least 1.""" + try: + return max(1, int(os.environ.get(TEST_JOBS_ENV_VAR, "") or 1)) + except ValueError: + logger.warning(f"Ignoring non-integer {TEST_JOBS_ENV_VAR}={os.environ[TEST_JOBS_ENV_VAR]!r}") + return 1 + + +_OUTPUT_LOCK = threading.Lock() +"""Serializes printing whole passes' output, so concurrently finishing files do not interleave in the job log.""" + + +def _print_captured_output(label, options, stdout_data, stderr_data): + """Print one process's captured output in a single block, unless it was already echoed as it arrived.""" + if options.echo: + return + with _OUTPUT_LOCK: + sys.stdout.write(f"\n{'=' * 30} output of {label} {'=' * 30}\n") + sys.stdout.flush() + sys.stdout.buffer.write(stdout_data) + sys.stdout.flush() + sys.stderr.buffer.write(stderr_data) + sys.stderr.flush() + sys.stdout.write(f"{'=' * 30} end of {label} {'=' * 30}\n") + sys.stdout.flush() + + +def _child_pids(pid: int) -> list[int]: + """Return the direct children of ``pid``, or an empty list where ``/proc`` is unavailable.""" + children = [] + for entry in os.listdir("/proc") if os.path.isdir("/proc") else []: + if not entry.isdigit(): + continue + try: + with open(f"/proc/{entry}/stat") as handle: + # The command name is parenthesized and may contain spaces; the parent PID follows it. + parent = int(handle.read().rsplit(")", 1)[1].split()[1]) + except (OSError, IndexError, ValueError): + continue + if parent == pid: + children.append(int(entry)) + return sorted(children) + + +def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env, options): """Ask a hung process for a stack of every thread, and collect what it writes. Sends :data:`hang_dump.DUMP_SIGNAL`, which ``tools/hang_dump.py`` registers with ``faulthandler`` in the @@ -210,13 +287,16 @@ def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): The signal goes to the test process itself rather than its group. The handler is registered there, and a standalone script the test launched as a grandchild has no handler -- ``SIGUSR1`` would simply kill it, - losing it from the process tree the caller has already recorded. + losing it from the process tree the caller has already recorded. Under ``pytest-xdist`` the tests run in + the worker processes, the controller's direct children, so each worker is asked in turn as well; one at + a time, because they all append to the same dump file. Args: process: The hung child. stdout_fd: Read end of the child's stdout, already non-blocking. stderr_fd: Read end of the child's stderr, already non-blocking. env: Environment the child was started with, read for the dump file it was told to write. + options: How the child is watched: its ``pytest-xdist`` worker count and whether to echo its output. Returns: Tuple of ``(dump_section, stdout_bytes, stderr_bytes)``. *dump_section* is a report section, or @@ -232,23 +312,28 @@ def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): if hang_dump.DUMP_SIGNAL is None or not dump_file: return "", stdout_data, stderr_data + targets = [process.pid] + if options.workers > 1: + targets += _child_pids(process.pid) + for _ in range(HANG_DUMP_PASSES): - # Only this pass's share of the file is the dump it asked for. - start = hang_dump.size(dump_file) - try: - os.kill(process.pid, hang_dump.DUMP_SIGNAL) - except OSError: - break + for pid in targets: + # Only this request's share of the file is the dump it asked for. + start = hang_dump.size(dump_file) + try: + os.kill(pid, hang_dump.DUMP_SIGNAL) + except OSError: + continue - # Keep draining while the handler runs, so a full pipe cannot be what stops it answering. - deadline = time.time() + HANG_DUMP_GRACE - while time.time() < deadline: - stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd) - stdout_data += stdout_chunk - stderr_data += stderr_chunk + # Keep draining while the handler runs, so a full pipe cannot be what stops it answering. + deadline = time.time() + HANG_DUMP_GRACE + while time.time() < deadline: + stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd, echo=options.echo) + stdout_data += stdout_chunk + stderr_data += stderr_chunk - if dumped := hang_dump.read_since(dump_file, start): - dumps.append(dumped) + if dumped := hang_dump.read_since(dump_file, start): + dumps.append(dumped if len(targets) == 1 else f"(pid {pid})\n{dumped}") # exit early if the process died if process.poll() is not None: break @@ -260,7 +345,7 @@ def _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env): return f"=== HANG STACK DUMP (all threads) ===\n{body}", stdout_data, stderr_data -def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, report_file=""): +def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, report_file="", options=None): """Run a command with timeout and capture all output while streaming in real-time. Args: @@ -273,6 +358,7 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo report_file: Path to the JUnit XML report file. When set, the process is given only :data:`SHUTDOWN_GRACE_PERIOD` seconds to exit after the file appears on disk. + options: How the process is watched. Defaults to :class:`_CaptureOptions`. Returns: Tuple of ``(returncode, stdout_bytes, stderr_bytes, kill_reason, @@ -281,6 +367,7 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo did not reach pytest collection in time, or ``"shutdown_hang"`` when the test completed but the process hung during shutdown. """ + options = options or _CaptureOptions() stdout_data = b"" stderr_data = b"" process = None @@ -312,15 +399,21 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo pass start_time = time.time() - startup_done = startup_deadline <= 0 + started = False shutdown_deadline = 0.0 while process.poll() is None: elapsed = time.time() - start_time - if not startup_done: - if b"AppLauncher initialization complete" in stderr_data or b"collected " in stdout_data: - startup_done = True + if not started and ( + b"AppLauncher initialization complete" in stderr_data + or b"collected " in stdout_data + or b" workers [" in stdout_data + ): + started = True + if options.on_started is not None: + options.on_started() + startup_done = started or startup_deadline <= 0 if report_file and not shutdown_deadline and os.path.exists(report_file): shutdown_deadline = time.time() + SHUTDOWN_GRACE_PERIOD @@ -339,7 +432,9 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo # Ask the process where it is stuck before killing it -- SIGKILL below cannot be caught, # so this is the only chance to get a stack out of it. - hang_stacks, dump_stdout, dump_stderr = _dump_hung_process_stacks(process, stdout_fd, stderr_fd, env) + hang_stacks, dump_stdout, dump_stderr = _dump_hung_process_stacks( + process, stdout_fd, stderr_fd, env, options + ) stdout_data += dump_stdout stderr_data += dump_stderr if hang_stacks: @@ -360,7 +455,7 @@ def capture_test_output_with_timeout(cmd, timeout, env, startup_deadline=0, repo wall_time = time.time() - start_time return -1, stdout_data, stderr_data, kill_reason, wall_time, pre_kill_diag - stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd) + stdout_chunk, stderr_chunk = _drain_ready_output(process, stdout_fd, stderr_fd, echo=options.echo) stdout_data += stdout_chunk stderr_data += stderr_chunk @@ -739,6 +834,7 @@ def _retry_failed_test_in_fresh_process( kill_reason, wall_time, pre_kill_diag, + options, ): """Retry selected failed test files in a fresh subprocess. @@ -772,8 +868,9 @@ def _retry_failed_test_in_fresh_process( retry_wall_time, pre_kill_diag, ) = capture_test_output_with_timeout( - cmd, timeout, env, startup_deadline=startup_deadline, report_file=report_file + cmd, timeout, env, startup_deadline=startup_deadline, report_file=report_file, options=options ) + _print_captured_output(file_name, options, stdout_data, stderr_data) wall_time += retry_wall_time if not os.path.exists(report_file): # The attempt died before pytest wrote its report; the caller rebuilds the result from @@ -827,6 +924,7 @@ class _PassContext: inject_shard_select: Whether to load the multi-GPU ``mgpu_shard_select`` plugin in the pytest subprocess; true only on a non-default-GPU shard. pytest_targets: Test file or node IDs passed to the pytest subprocess. + capture: How each pytest subprocess is watched, including its ``pytest-xdist`` worker count. """ test_file: str @@ -838,6 +936,7 @@ class _PassContext: env: dict inject_shard_select: bool pytest_targets: list[str] + capture: _CaptureOptions _RESULT_PRIORITY = { @@ -992,6 +1091,8 @@ def _run_one_pass( cmd += ["-p", "mgpu_shard_select"] if ctx.ci_marker: cmd += ["-m", ctx.ci_marker] + if ctx.capture.workers > 1: + cmd += ["-n", str(ctx.capture.workers)] if k_expr is not None: cmd += ["-k", k_expr] cmd += ctx.pytest_targets @@ -1010,8 +1111,14 @@ def _run_one_pass( os.remove(stale_file) returncode, stdout_data, stderr_data, kill_reason, wall_time, pre_kill_diag = capture_test_output_with_timeout( - cmd, ctx.timeout, pass_env, startup_deadline=ctx.startup_deadline, report_file=report_file + cmd, + ctx.timeout, + pass_env, + startup_deadline=ctx.startup_deadline, + report_file=report_file, + options=ctx.capture, ) + _print_captured_output(pass_file_label, ctx.capture, stdout_data, stderr_data) total_wall_time += wall_time has_report = os.path.exists(report_file) @@ -1205,6 +1312,7 @@ def _run_one_pass( kill_reason=kill_reason, wall_time=wall_time, pre_kill_diag=pre_kill_diag, + options=ctx.capture, ) if not os.path.exists(report_file): @@ -1286,117 +1394,180 @@ def _run_one_pass( ) +def _starts_renderer(test_content: str) -> bool: + """Return whether a test file's source starts an RTX renderer: Kit's, by enabling cameras, or OVRTX.""" + return "enable_cameras=True" in test_content or "ovrtx" in test_content.lower() + + +def _test_file_job(test_file: str, max_slots: int) -> TestFileJob: + """Describe a test file to the scheduler: how many slots it holds and whether it starts a renderer.""" + try: + with open(test_file) as fh: + test_content = fh.read() + except OSError: + test_content = "" + file_name = os.path.basename(test_file) + slots = max_slots if file_name in test_settings.EXCLUSIVE_TESTS else _pytest_workers(file_name) + return TestFileJob(path=test_file, slots=slots, renders=_starts_renderer(test_content)) + + +def _pytest_workers(file_name: str) -> int: + """Return how many ``pytest-xdist`` workers a test file is split across, 1 when it is not split.""" + return test_settings.PYTEST_WORKERS.get(file_name, 1) + + +@dataclass +class _FileResult: + """Outcome of every pass of one test file.""" + + reports: list[JUnitXml] + status: dict + failed: bool + + def run_individual_tests(test_files, workspace_root, ci_marker, test_node_ids_by_file=None): - """Run each test file separately, ensuring one finishes before starting the next. + """Run each test file in its own pytest process, up to :data:`TEST_JOBS_ENV_VAR` slots at a time. When ``ISAACLAB_TEST_QUEUE`` names a shared work-queue file, files are claimed from it (work-stealing across sibling shard containers) instead of iterating ``test_files``; each file still runs once, on this container's pinned GPU. """ - failed_tests = [] - test_status = {} - xml_reports = [] - cold_cache_applied = False test_node_ids_by_file = test_node_ids_by_file or {} global_k_expr = os.environ.get("TEST_K_EXPR", "").strip() or None if global_k_expr is not None: logger.info(f"Applying global pytest -k expression to every test file: '{global_k_expr}'") + max_slots = _test_jobs() queue_path = os.environ.get("ISAACLAB_TEST_QUEUE", "") - file_source = _queued_files(queue_path) if queue_path else test_files - - for test_file in file_source: - logger.info(f"\n\n🚀 Running {test_file} independently...\n") - file_name = os.path.basename(test_file) - env = os.environ.copy() - env["PYTHONFAULTHANDLER"] = "1" - - # Multi-GPU lane only: make the device-selection plugin importable in this - # per-file subprocess (injected via ``-p`` in _run_one_pass, not as a - # repo-root conftest). Detect a shard by the runtime device mask excluding cpu - # (position 0) and cuda:0 (position 1) -- the same ISAACLAB_TEST_DEVICES the - # plugin and test_devices() read. The plugin re-checks this; the cheap - # prefix test here leaves single-GPU CI's command (mask unset or "11...") - # unchanged. - _mask = os.environ.get("ISAACLAB_TEST_DEVICES", "") - _inject_shard_select = _mask[:2] == "00" - if _inject_shard_select: - _plugin_dir = os.path.join(workspace_root, ".github", "actions", "multi-gpu") - env["PYTHONPATH"] = _plugin_dir + os.pathsep + env.get("PYTHONPATH", "") - - timeout = test_settings.PER_TEST_TIMEOUTS.get(file_name, test_settings.DEFAULT_TIMEOUT) - - # Read the test file once for cold-cache and device-split detection. - try: - with open(test_file) as fh: - test_content = fh.read() - except OSError: - test_content = "" - - # The first camera-enabled test in a fresh container compiles shaders - # (~600 s). Give it extra time so that doesn't look like a test timeout. - is_cold_cache_test = not cold_cache_applied and "enable_cameras=True" in test_content - if is_cold_cache_test: - timeout += COLD_CACHE_BUFFER - cold_cache_applied = True - logger.info(f"⏱️ Adding {COLD_CACHE_BUFFER}s cold-cache buffer (timeout now {timeout}s)") - - startup_deadline = _resolve_startup_deadline(file_name, timeout, is_cold_cache_test) - - pytest_targets = test_node_ids_by_file.get(os.path.normpath(test_file), [str(test_file)]) - - ctx = _PassContext( - test_file=test_file, - file_name=file_name, + if queue_path: + jobs = (_test_file_job(test_file, max_slots) for test_file in _queued_files(queue_path)) + else: + # Wide files first: split ones are the long poles, and wide ones can only start once enough slots are free. + jobs = sorted((_test_file_job(test_file, max_slots) for test_file in test_files), key=lambda job: -job.slots) + if max_slots > 1: + logger.info(f"Running test files {max_slots} slots at a time; each file's output is printed when it ends") + + def run(job: TestFileJob, context: JobContext) -> _FileResult: + result = _run_test_file( + job, + context, workspace_root=workspace_root, ci_marker=ci_marker, - timeout=timeout, - startup_deadline=startup_deadline, - env=env, - inject_shard_select=_inject_shard_select, - pytest_targets=pytest_targets, + pytest_targets=test_node_ids_by_file.get(os.path.normpath(job.path), [str(job.path)]), + global_k_expr=global_k_expr, + workers=min(_pytest_workers(os.path.basename(job.path)), max_slots), + echo=max_slots == 1, ) - - # On a multi-GPU shard, test_devices() already resolves to this shard's single - # GPU and mgpu_shard_select drops every other variant, so the device_split - # CPU/GPU two-pass (which exists to dodge the process-global device lock when - # CPU and GPU share one container) is unnecessary here — the CPU pass would - # collect zero tests yet still pay full Kit-startup cost. Run once on a shard. - if _inject_shard_select: - passes = [("", None)] - elif is_device_split_file(test_file, source=test_content): - logger.info(f"⚙️ device_split detected — invoking {file_name} once per device (CPU then GPU)") - passes = DEVICE_SPLIT_PASSES - else: - passes = [("", None)] - - merged_status: dict | None = None - for suffix, k_expr in passes: - if global_k_expr is not None: - k_expr = f"({k_expr}) and ({global_k_expr})" if k_expr else global_k_expr - report, status, was_failure = _run_one_pass(ctx, k_expr=k_expr, suffix=suffix) - if report is not None: - xml_reports.append(report) - if was_failure and test_file not in failed_tests: - failed_tests.append(test_file) - merged_status = _merge_pass_status(merged_status, status) - - assert merged_status is not None # the pass list is never empty - test_status[test_file] = merged_status - # When running under the directory-based work queue (option 2), move the # claim entry from inflight// to done// so the post-run # reconciler can distinguish "ran to completion" from "claimed but # crashed mid-test". A claim that stays in inflight at job-end is a # silent drop signal. if queue_path: - _mark_queued_file_done(queue_path, test_file) + _mark_queued_file_done(queue_path, job.path) + return result + + results = run_test_files(jobs, run, max_slots=max_slots) logger.info("~~~~~~~~~~~~ Finished running all tests") + failed_tests = [test_file for test_file, result in results.items() if result.failed] + test_status = {test_file: result.status for test_file, result in results.items()} + xml_reports = [report for result in results.values() for report in result.reports] return failed_tests, test_status, xml_reports +def _run_test_file( + job: TestFileJob, + context: JobContext, + *, + workspace_root: str, + ci_marker: str | None, + pytest_targets: list[str], + global_k_expr: str | None, + workers: int, + echo: bool, +) -> _FileResult: + """Run every pass of one test file and merge their results.""" + test_file = job.path + logger.info(f"\n\n🚀 Running {test_file} independently...\n") + file_name = os.path.basename(test_file) + env = os.environ.copy() + env["PYTHONFAULTHANDLER"] = "1" + + # Multi-GPU lane only: make the device-selection plugin importable in this + # per-file subprocess (injected via ``-p`` in _run_one_pass, not as a + # repo-root conftest). Detect a shard by the runtime device mask excluding cpu + # (position 0) and cuda:0 (position 1) -- the same ISAACLAB_TEST_DEVICES the + # plugin and test_devices() read. The plugin re-checks this; the cheap + # prefix test here leaves single-GPU CI's command (mask unset or "11...") + # unchanged. + _mask = os.environ.get("ISAACLAB_TEST_DEVICES", "") + _inject_shard_select = _mask[:2] == "00" + if _inject_shard_select: + _plugin_dir = os.path.join(workspace_root, ".github", "actions", "multi-gpu") + env["PYTHONPATH"] = _plugin_dir + os.pathsep + env.get("PYTHONPATH", "") + + timeout = test_settings.PER_TEST_TIMEOUTS.get(file_name, test_settings.DEFAULT_TIMEOUT) + + # Read the test file once for device-split detection. + try: + with open(test_file) as fh: + test_content = fh.read() + except OSError: + test_content = "" + + # The first renderer in a fresh container compiles shaders (~600 s). + # Give it extra time so that doesn't look like a test timeout. + is_cold_cache_test = context.renderer_cold + if is_cold_cache_test: + timeout += COLD_CACHE_BUFFER + logger.info(f"⏱️ Adding {COLD_CACHE_BUFFER}s cold-cache buffer (timeout now {timeout}s)") + + startup_deadline = _resolve_startup_deadline(file_name, timeout, is_cold_cache_test) + + ctx = _PassContext( + test_file=test_file, + file_name=file_name, + workspace_root=workspace_root, + ci_marker=ci_marker, + timeout=timeout, + startup_deadline=startup_deadline, + env=env, + inject_shard_select=_inject_shard_select, + pytest_targets=pytest_targets, + capture=_CaptureOptions(echo=echo, workers=workers, on_started=context.mark_started), + ) + + # On a multi-GPU shard, test_devices() already resolves to this shard's single + # GPU and mgpu_shard_select drops every other variant, so the device_split + # CPU/GPU two-pass (which exists to dodge the process-global device lock when + # CPU and GPU share one container) is unnecessary here — the CPU pass would + # collect zero tests yet still pay full Kit-startup cost. Run once on a shard. + if _inject_shard_select: + passes = [("", None)] + elif is_device_split_file(test_file, source=test_content): + logger.info(f"⚙️ device_split detected — invoking {file_name} once per device (CPU then GPU)") + passes = DEVICE_SPLIT_PASSES + else: + passes = [("", None)] + + reports = [] + failed = False + merged_status: dict | None = None + for suffix, k_expr in passes: + if global_k_expr is not None: + k_expr = f"({k_expr}) and ({global_k_expr})" if k_expr else global_k_expr + report, status, was_failure = _run_one_pass(ctx, k_expr=k_expr, suffix=suffix) + if report is not None: + reports.append(report) + failed = failed or was_failure + merged_status = _merge_pass_status(merged_status, status) + + assert merged_status is not None # the pass list is never empty + return _FileResult(reports=reports, status=merged_status, failed=failed) + + def _resolve_startup_deadline(file_name: str, timeout: int, is_cold_cache_test: bool) -> int: """Resolve the startup deadline for one independently launched test file.""" base_deadline = test_settings.PER_TEST_STARTUP_TIMEOUTS.get(file_name, STARTUP_DEADLINE) diff --git a/tools/hang_dump.py b/tools/hang_dump.py index ad519be57c59..9fef9b8b4b95 100644 --- a/tools/hang_dump.py +++ b/tools/hang_dump.py @@ -107,8 +107,10 @@ def register(): path = dump_path() if not path or not is_supported(): return False + # pytest-xdist workers share the controller's dump file, which the controller already truncated. + mode = "a" if os.environ.get("PYTEST_XDIST_WORKER") else "w" try: - _dump_file = open(path, "w") # noqa: SIM115 (held open for the process lifetime, see above) + _dump_file = open(path, mode) # noqa: SIM115 (held open for the process lifetime, see above) except OSError: return False faulthandler.register(DUMP_SIGNAL, file=_dump_file, all_threads=True, chain=False) diff --git a/tools/test_crash_journal.py b/tools/test_crash_journal.py index 86eb6531c1c8..a07e6f471b8d 100644 --- a/tools/test_crash_journal.py +++ b/tools/test_crash_journal.py @@ -429,6 +429,42 @@ def test_drop(): assert read_journal(str(journal_file)).collected == [f"{_FILE}::test_keep"] +def test_an_xdist_run_journals_each_event_once(tmp_path): + """Regression test for ``pytest-xdist`` runs journaling every event twice. + + The workers and the controller both fire the per-test hooks, and every worker fires the + collection hook. Duplicated starts turn a worker crash that xdist recovered from into an + unmatched start, so a later session crash would be blamed on a test that already reported. + """ + pytest.importorskip("xdist") + _write_test_module( + tmp_path, + """ + def test_a(): + pass + + def test_b(): + assert 1 == 2 + + def test_c(): + pass + """, + ) + journal_file = tmp_path / "journal.jsonl" + junit_file = tmp_path / "report.xml" + _run_pytest(tmp_path, journal_file, junit_file, "-p", "xdist.plugin", "-n", "2") + + records = [json.loads(line) for line in journal_file.read_text(encoding="utf-8").splitlines()] + node_ids = [f"{_FILE}::test_{name}" for name in "abc"] + assert [record["event"] for record in records].count("collected") == 1 + for event in ("start", "result", "finish"): + assert sorted(record["node_id"] for record in records if record["event"] == event) == node_ids + + journal = read_journal(str(journal_file)) + assert journal.collected == node_ids + assert journal.culprit is None + + # -- artificial crashes in a real pytest run -------------------------------------------------- diff --git a/tools/test_file_scheduler.py b/tools/test_file_scheduler.py new file mode 100644 index 000000000000..7d1791f9a6dd --- /dev/null +++ b/tools/test_file_scheduler.py @@ -0,0 +1,231 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Unit tests for ``tools/_file_scheduler.py``.""" + +from __future__ import annotations + +import threading + +import pytest +from _file_scheduler import JobContext, TestFileJob, run_test_files + +_TIMEOUT = 10.0 +"""Seconds any wait in these tests may take before it counts as a deadlock.""" + + +class _Harness: + """A ``run`` callable whose jobs start and finish only when the test says so. + + Records which jobs are in flight, so a test can assert on concurrency at each step instead of relying + on sleeps. + """ + + def __init__(self): + self._lock = threading.Condition() + self.running: dict[str, TestFileJob] = {} + self.started: list[str] = [] + self.contexts: dict[str, JobContext] = {} + self.peak_slots = 0 + self._release: dict[str, threading.Event] = {} + + def __call__(self, job: TestFileJob, context: JobContext) -> str: + release = threading.Event() + with self._lock: + self.running[job.path] = job + self.started.append(job.path) + self.contexts[job.path] = context + self._release[job.path] = release + self.peak_slots = max(self.peak_slots, sum(j.slots for j in self.running.values())) + self._lock.notify_all() + assert release.wait(_TIMEOUT), f"{job.path} was never released" + with self._lock: + del self.running[job.path] + self._lock.notify_all() + return f"result:{job.path}" + + def wait_running(self, *paths: str) -> None: + """Wait until exactly ``paths`` are in flight.""" + with self._lock: + assert self._lock.wait_for(lambda: set(self.running) == set(paths), _TIMEOUT), ( + f"expected {sorted(paths)} running, have {sorted(self.running)}" + ) + + def finish(self, path: str) -> None: + with self._lock: + self._release[path].set() + assert self._lock.wait_for(lambda: path not in self.running, _TIMEOUT) + + +def _run_in_background(jobs, harness, max_slots): + outcome = {} + + def target(): + try: + outcome["results"] = run_test_files(jobs, harness, max_slots=max_slots) + except BaseException as error: + outcome["error"] = error + + thread = threading.Thread(target=target, daemon=True) + thread.start() + return thread, outcome + + +def test_one_slot_runs_jobs_in_order_on_the_calling_thread(): + seen = [] + + def run(job, context): + seen.append((job.path, threading.current_thread() is threading.main_thread())) + return job.path.upper() + + results = run_test_files([TestFileJob("a"), TestFileJob("b", slots=4)], run, max_slots=1) + + assert seen == [("a", True), ("b", True)] + assert results == {"a": "A", "b": "B"} + + +def test_jobs_fill_the_slots_and_the_next_starts_when_one_finishes(): + harness = _Harness() + jobs = [TestFileJob(name) for name in "abcd"] + thread, outcome = _run_in_background(jobs, harness, max_slots=3) + + harness.wait_running("a", "b", "c") + harness.finish("b") + harness.wait_running("a", "c", "d") + for name in "acd": + harness.finish(name) + thread.join(_TIMEOUT) + + assert harness.peak_slots == 3 + assert outcome["results"] == {name: f"result:{name}" for name in "abcd"} + + +def test_a_wide_job_holds_its_slots_and_is_not_overtaken(): + harness = _Harness() + jobs = [TestFileJob("narrow"), TestFileJob("wide", slots=3), TestFileJob("after")] + thread, outcome = _run_in_background(jobs, harness, max_slots=3) + + # "wide" needs all three slots; "after" fits beside "narrow" but must not jump the queue. + harness.wait_running("narrow") + with harness._lock: + assert not harness._lock.wait_for(lambda: "wide" in harness.started, timeout=0.2) + harness.finish("narrow") + harness.wait_running("wide") + harness.finish("wide") + harness.wait_running("after") + harness.finish("after") + thread.join(_TIMEOUT) + + assert harness.started == ["narrow", "wide", "after"] + assert harness.peak_slots == 3 + + +def test_a_job_wider_than_the_budget_gets_every_slot(): + harness = _Harness() + thread, outcome = _run_in_background([TestFileJob("huge", slots=8), TestFileJob("next")], harness, max_slots=2) + + harness.wait_running("huge") + harness.finish("huge") + harness.wait_running("next") + harness.finish("next") + thread.join(_TIMEOUT) + + assert "error" not in outcome + + +def test_rendering_jobs_never_overlap_and_others_overtake_them(): + harness = _Harness() + jobs = [ + TestFileJob("render-1", renders=True), + TestFileJob("render-2", renders=True), + TestFileJob("plain"), + TestFileJob("sentinel"), + ] + thread, outcome = _run_in_background(jobs, harness, max_slots=2) + + harness.wait_running("render-1", "plain") + assert harness.contexts["render-1"].renderer_cold + assert not harness.contexts["plain"].renderer_cold + + # Starting up is not enough: the slot "plain" frees goes past the second renderer to the job behind it. + harness.contexts["render-1"].mark_started() + harness.finish("plain") + harness.wait_running("render-1", "sentinel") + harness.finish("render-1") + harness.wait_running("render-2", "sentinel") + assert not harness.contexts["render-2"].renderer_cold + + harness.finish("render-2") + harness.finish("sentinel") + thread.join(_TIMEOUT) + assert "error" not in outcome + + +def test_a_renderer_that_ends_without_starting_releases_the_next_which_is_still_cold(): + harness = _Harness() + jobs = [TestFileJob("render-1", renders=True), TestFileJob("render-2", renders=True)] + thread, outcome = _run_in_background(jobs, harness, max_slots=2) + + harness.wait_running("render-1") + harness.finish("render-1") # e.g. killed by a startup hang, never reaching collection + harness.wait_running("render-2") + assert harness.contexts["render-2"].renderer_cold + harness.finish("render-2") + thread.join(_TIMEOUT) + + assert "error" not in outcome + + +def test_only_the_first_renderer_is_cold_when_run_serially(): + contexts = {} + + def run(job, context): + contexts[job.path] = context + context.mark_started() + + run_test_files([TestFileJob("render-1", renders=True), TestFileJob("render-2", renders=True)], run, max_slots=1) + + assert contexts["render-1"].renderer_cold + assert not contexts["render-2"].renderer_cold + + +def test_jobs_are_pulled_only_when_they_can_start(): + harness = _Harness() + pulled = [] + + def claim(): + for name in "abc": + pulled.append(name) + yield TestFileJob(name) + + thread, outcome = _run_in_background(claim(), harness, max_slots=2) + + harness.wait_running("a", "b") + assert pulled == ["a", "b"] + harness.finish("a") + harness.wait_running("b", "c") + harness.finish("b") + harness.finish("c") + thread.join(_TIMEOUT) + + assert pulled == ["a", "b", "c"] + + +def test_an_error_in_one_job_is_raised_after_the_others_finish(): + finished = threading.Event() + other_started = threading.Event() + + def run(job, context): + if job.path == "bad": + assert other_started.wait(_TIMEOUT) + raise RuntimeError("boom") + other_started.set() + finished.wait(0.2) + finished.set() + return job.path + + with pytest.raises(RuntimeError, match="boom"): + run_test_files([TestFileJob("bad"), TestFileJob("good")], run, max_slots=2) + assert finished.is_set() diff --git a/tools/test_settings.py b/tools/test_settings.py index bcba63b6b65e..ca63b8d0aa3b 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -80,6 +80,25 @@ } """Per-test startup timeouts for cold external asset downloads.""" +PYTEST_WORKERS = { + # 20 independent export round trips, ~18 min serially: the RL job's long pole. + "test_leapp_export_flow.py": 4, +} +"""Test files split across ``pytest-xdist`` workers, and how many. + +Every worker starts its own process -- and its own Kit app and simulation, for files that launch one -- so +splitting only pays off for files whose tests take far longer than that startup. List a file here when it +is the long pole of its CI job. Each worker holds one of the job's ``TEST_JOBS`` slots; a file never gets +more workers than the job has slots. +""" + +EXCLUSIVE_TESTS = [ + # Both assert wall-clock limits, which other files running at the same time would eat into. + "test_kit_startup_performance.py", + "test_robot_load_performance.py", +] +"""Test files that run with no other test file alongside them, when a job runs several files at once.""" + CUROBO_PLANNER_TESTS = [ "test_curobo_planner_franka.py", "test_curobo_planner_cube_stack.py", diff --git a/uv.lock b/uv.lock index 7cc1458e74b0..3f98b1aa491c 100644 --- a/uv.lock +++ b/uv.lock @@ -1105,6 +1105,15 @@ epath = [ { name = "zipp" }, ] +[[package]] +name = "execnet" +version = "2.1.2" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/bf/89/780e11f9588d9e7128a3f87788354c7946a9cbb1401ad38a48c4db9a4f07/execnet-2.1.2.tar.gz", hash = "sha256:63d83bfdd9a23e35b9c6a3261412324f964c2ec8dcd8d3c6916ee9373e0befcd", size = 166622, upload-time = "2025-11-12T09:56:37.75Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/ab/84/02fc1827e8cdded4aa65baef11296a9bbe595c474f0d6d758af082d849fd/execnet-2.1.2-py3-none-any.whl", hash = "sha256:67fba928dd5a544b783f6056f449e5e3931a5c378b128bc18501f7ea79e296ec", size = 40708, upload-time = "2025-11-12T09:56:36.333Z" }, +] + [[package]] name = "executing" version = "2.2.1" @@ -1818,6 +1827,7 @@ dev = [ { name = "myst-parser" }, { name = "pytest" }, { name = "pytest-mock" }, + { name = "pytest-xdist" }, { name = "sphinx" }, { name = "sphinx-book-theme" }, { name = "sphinx-copybutton" }, @@ -1903,6 +1913,7 @@ test = [ { name = "junitparser" }, { name = "pytest" }, { name = "pytest-mock" }, + { name = "pytest-xdist" }, ] tetrahedralization = [ { name = "pytetwild", extra = ["all"] }, @@ -2007,6 +2018,7 @@ requires-dist = [ { name = "pyopengl-accelerate", specifier = ">=3.1.0" }, { name = "pytest", marker = "extra == 'test'" }, { name = "pytest-mock", marker = "extra == 'test'" }, + { name = "pytest-xdist", marker = "extra == 'test'" }, { name = "pytetwild", extras = ["all"], marker = "extra == 'tetrahedralization'", specifier = ">=0.3.0,<0.4" }, { name = "ray", extras = ["default"], marker = "extra == 'rlinf'", specifier = ">=2.47.0" }, { name = "requests", specifier = ">=2.25.0" }, @@ -4405,6 +4417,19 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/5a/cc/06253936f4a7fa2e0f48dfe6d851d9c56df896a9ab09ac019d70b760619c/pytest_mock-3.15.1-py3-none-any.whl", hash = "sha256:0a25e2eb88fe5168d535041d09a4529a188176ae608a6d249ee65abc0949630d", size = 10095, upload-time = "2025-09-16T16:37:25.734Z" }, ] +[[package]] +name = "pytest-xdist" +version = "3.8.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "execnet" }, + { name = "pytest" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/78/b4/439b179d1ff526791eb921115fca8e44e596a13efeda518b9d845a619450/pytest_xdist-3.8.0.tar.gz", hash = "sha256:7e578125ec9bc6050861aa93f2d59f1d8d085595d6551c2c90b6f4fad8d3a9f1", size = 88069, upload-time = "2025-07-01T13:30:59.346Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/ca/31/d4e37e9e550c2b92a9cbc2e4d0b7420a27224968580b5a447f420847c975/pytest_xdist-3.8.0-py3-none-any.whl", hash = "sha256:202ca578cfeb7370784a8c33d6d05bc6e13b4f25b5053c30a152269fd10f0b88", size = 46396, upload-time = "2025-07-01T13:30:56.632Z" }, +] + [[package]] name = "pytetwild" version = "0.3.0"