From 16cc1a5128b76304480970fc9e151771f9190e69 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Tue, 7 Jul 2026 17:19:02 +0200 Subject: [PATCH 1/7] Add IMU and PVA performance benchmark Provide a shared standalone benchmark for the PhysX IMU and PVA update paths so baseline, cached-eager, and recorded-launch latency can be compared under the same workload. --- scripts/benchmarks/benchmark_imu_pva.py | 140 ++++++++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 scripts/benchmarks/benchmark_imu_pva.py diff --git a/scripts/benchmarks/benchmark_imu_pva.py b/scripts/benchmarks/benchmark_imu_pva.py new file mode 100644 index 000000000000..eca69ea688c2 --- /dev/null +++ b/scripts/benchmarks/benchmark_imu_pva.py @@ -0,0 +1,140 @@ +# 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 + +"""Benchmark the PhysX IMU and PVA sensor update paths. + +Usage: + ./isaaclab.sh -p scripts/benchmarks/benchmark_imu_pva.py --sensor imu --num_envs 4096 + ./isaaclab.sh -p scripts/benchmarks/benchmark_imu_pva.py --sensor pva --num_envs 4096 +""" + +from __future__ import annotations + +import argparse + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Benchmark a PhysX IMU or PVA sensor update path.") +parser.add_argument("--sensor", choices=("imu", "pva"), required=True, help="Sensor update path to benchmark.") +parser.add_argument("--num_envs", type=int, default=4096, help="Number of environments and sensor instances.") +parser.add_argument("--num_steps", type=int, default=500, help="Number of timed updates.") +parser.add_argument("--warmup_steps", type=int, default=50, help="Number of untimed warm-up updates.") +parser.add_argument("--label", type=str, default="current", help="Label printed with the benchmark results.") +parser.add_argument( + "--disable_recorded_launch", + action="store_true", + help="Use cached PhysX views with ordinary eager Warp launches.", +) +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Everything below follows application launch.""" + +import statistics +import time + +import torch +import warp as wp + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import ImuCfg, PvaCfg +from isaaclab.utils.configclass import configclass + + +@configclass +class ImuPvaBenchmarkSceneCfg(InteractiveSceneCfg): + """One kinematic rigid body and one selected sensor per environment.""" + + body = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Body", + spawn=sim_utils.CuboidCfg( + size=(0.1, 0.1, 0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True, disable_gravity=True), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)), + ) + imu: ImuCfg | None = None + pva: PvaCfg | None = None + + +def _percentile(samples: list[float], percentile: float) -> float: + """Return a linearly interpolated percentile for scalar samples.""" + ordered = sorted(samples) + position = (len(ordered) - 1) * percentile + lower = int(position) + upper = min(lower + 1, len(ordered) - 1) + fraction = position - lower + return ordered[lower] + fraction * (ordered[upper] - ordered[lower]) + + +def main() -> None: + """Run the selected sensor benchmark and print latency statistics.""" + sim_dt = 1.0 / 120.0 + sim_cfg = sim_utils.SimulationCfg(dt=sim_dt, device=args_cli.device, gravity=(0.0, 0.0, -9.81)) + sim = sim_utils.SimulationContext(sim_cfg) + + scene_cfg = ImuPvaBenchmarkSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=1.0, + lazy_sensor_update=True, + ) + if args_cli.sensor == "imu": + scene_cfg.imu = ImuCfg(prim_path="{ENV_REGEX_NS}/Body") + else: + scene_cfg.pva = PvaCfg(prim_path="{ENV_REGEX_NS}/Body") + + scene = InteractiveScene(scene_cfg) + sim.reset() + scene.reset() + sensor = scene[args_cli.sensor] + + if args_cli.disable_recorded_launch: + sensor._use_recorded_launch = False + for _ in range(args_cli.warmup_steps): + sim.step(render=False) + sensor.update(sim_dt, force_recompute=True) + wp.synchronize_device(sim.device) + + synchronized_ms: list[float] = [] + submission_ms: list[float] = [] + for _ in range(args_cli.num_steps): + sim.step(render=False) + wp.synchronize_device(sim.device) + start = time.perf_counter() + sensor.update(sim_dt, force_recompute=True) + submitted = time.perf_counter() + wp.synchronize_device(sim.device) + finished = time.perf_counter() + synchronized_ms.append((finished - start) * 1000.0) + submission_ms.append((submitted - start) * 1000.0) + + lin_acc_b = sensor.data.lin_acc_b.torch + ang_vel_b = sensor.data.ang_vel_b.torch + finite_instances = int((torch.isfinite(lin_acc_b).all(dim=-1) & torch.isfinite(ang_vel_b).all(dim=-1)).sum()) + + print("-" * 80) + print(f"{args_cli.sensor.upper()} update benchmark (PhysX)") + print(f" label : {args_cli.label}") + print(f" device : {sim.device}") + print(f" num_envs : {args_cli.num_envs}") + print(f" num_steps : {args_cli.num_steps}") + print(f" synchronized mean : {statistics.mean(synchronized_ms):.3f} ms") + print(f" synchronized p50 : {_percentile(synchronized_ms, 0.50):.3f} ms") + print(f" synchronized p95 : {_percentile(synchronized_ms, 0.95):.3f} ms") + print(f" submission mean : {statistics.mean(submission_ms):.3f} ms") + print(f" submission p50 : {_percentile(submission_ms, 0.50):.3f} ms") + print(f" submission p95 : {_percentile(submission_ms, 0.95):.3f} ms") + print("-" * 80) + print(f" finite sensor outputs : {finite_instances} / {args_cli.num_envs}") + + +if __name__ == "__main__": + main() + simulation_app.close() From a66f8b06d47dae7efb49ffaec955535798805d58 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Tue, 7 Jul 2026 17:28:20 +0200 Subject: [PATCH 2/7] Record PhysX IMU and PVA update launches Reuse stable typed PhysX buffers and recorded Warp commands to reduce per-update Python overhead. Refresh changing masks and timesteps on the recorded commands so runtime behavior remains unchanged. --- .../physx-imu-pva-record-launch.rst | 4 + .../isaaclab_physx/sensors/imu/imu.py | 73 +++++- .../isaaclab_physx/sensors/pva/pva.py | 75 +++++- .../sensors/test_imu_pva_recorded_launch.py | 222 ++++++++++++++++++ 4 files changed, 356 insertions(+), 18 deletions(-) create mode 100644 source/isaaclab_physx/changelog.d/physx-imu-pva-record-launch.rst create mode 100644 source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py diff --git a/source/isaaclab_physx/changelog.d/physx-imu-pva-record-launch.rst b/source/isaaclab_physx/changelog.d/physx-imu-pva-record-launch.rst new file mode 100644 index 000000000000..80c3e0a84c60 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/physx-imu-pva-record-launch.rst @@ -0,0 +1,4 @@ +Fixed +^^^^^ + +* Fixed excessive PhysX IMU and PVA update overhead by reusing typed physics buffers and recorded kernel launches. diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py index ce7724fbdf6f..e6d0244d86a1 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py @@ -5,6 +5,7 @@ from __future__ import annotations +import logging from collections.abc import Sequence from typing import TYPE_CHECKING @@ -22,6 +23,8 @@ if TYPE_CHECKING: from isaaclab.sensors.imu import ImuCfg +logger = logging.getLogger(__name__) + class Imu(BaseImu): """The PhysX Inertial Measurement Unit (IMU) sensor. @@ -62,6 +65,13 @@ def __init__(self, cfg: ImuCfg): super().__init__(cfg) self._data = ImuData() self._rigid_parent_expr: str | None = None + self._raw_transforms: wp.array | None = None + self._raw_velocities: wp.array | None = None + self._raw_coms: wp.array | None = None + self._update_cmd: wp.Launch | None = None + self._update_env_mask: wp.array | None = None + self._update_inv_dt: float | None = None + self._use_recorded_launch: bool = False def __str__(self) -> str: """Returns: A string containing information about the instance.""" @@ -148,32 +158,68 @@ def _initialize_impl(self): self._offset_pos_b = wp.from_torch(composed_p.contiguous(), dtype=wp.vec3f) self._offset_quat_b = wp.from_torch(composed_q.contiguous(), dtype=wp.quatf) + self._use_recorded_launch = wp.get_device(self._device).is_cuda + def _update_buffers_impl(self, env_mask: wp.array | None = None): """Fills the buffers of the sensor data.""" env_mask = self._resolve_indices_and_mask(None, env_mask) - transforms = self._view.get_transforms().view(wp.transformf) - velocities = self._view.get_velocities().view(wp.spatial_vectorf) - wp.copy(self._coms_buffer, self._view.get_coms().view(wp.transformf)) - - wp.launch( + transforms = self._view.get_transforms() + velocities = self._view.get_velocities() + coms = self._view.get_coms() + if self._raw_transforms is None: + self._raw_transforms = transforms.view(wp.transformf) + self._raw_velocities = velocities.view(wp.spatial_vectorf) + self._raw_coms = coms.view(wp.transformf) + wp.copy(self._coms_buffer, self._raw_coms) + + inv_dt = 1.0 / self._dt + if self._use_recorded_launch: + if self._update_cmd is None: + try: + self._update_cmd = self._launch_update(env_mask, inv_dt, record_cmd=True) + self._update_env_mask = env_mask + self._update_inv_dt = inv_dt + except Exception as exc: + self._use_recorded_launch = False + logger.warning( + f"Failed to record the update of the IMU at '{self.cfg.prim_path}'." + f" Falling back to eager kernel launches. Reason: {exc}" + ) + if self._update_cmd is not None: + if env_mask is not self._update_env_mask: + self._update_cmd.set_param_by_name("env_mask", env_mask) + self._update_env_mask = env_mask + if inv_dt != self._update_inv_dt: + self._update_cmd.set_param_by_name("inv_dt", inv_dt) + self._update_inv_dt = inv_dt + self._update_cmd.launch() + return + + self._launch_update(env_mask, inv_dt) + + def _launch_update(self, env_mask: wp.array, inv_dt: float, record_cmd: bool = False) -> wp.Launch | None: + """Launch or record the kernel that updates the IMU data.""" + + return wp.launch( imu_update_kernel, dim=self._num_envs, inputs=[ env_mask, - transforms, - velocities, + self._raw_transforms, + self._raw_velocities, self._coms_buffer, self._offset_pos_b, self._offset_quat_b, self._gravity_bias_w, - 1.0 / self._dt, + inv_dt, self._timestamp, self._prev_lin_vel_w, self._data._ang_vel_b, self._data._lin_acc_b, ], device=self._device, + record_cmd=record_cmd, ) def _initialize_buffers_impl(self): @@ -188,3 +234,14 @@ def _initialize_buffers_impl(self): self._offset_quat_b = wp.from_torch(offset_quat_torch.contiguous(), dtype=wp.quatf) self._coms_buffer = wp.zeros(self._view.count, dtype=wp.transformf, device=self._device) + + def _invalidate_initialize_callback(self, event): + """Invalidate the sensor and release cached PhysX and launch state.""" + super()._invalidate_initialize_callback(event) + self._view = None + self._raw_transforms = None + self._raw_velocities = None + self._raw_coms = None + self._update_cmd = None + self._update_env_mask = None + self._update_inv_dt = None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py index 00b5a65883ae..6088f99ef985 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py @@ -5,6 +5,7 @@ from __future__ import annotations +import logging from collections.abc import Sequence from typing import TYPE_CHECKING @@ -26,6 +27,8 @@ if TYPE_CHECKING: from isaaclab.sensors.pva import PvaCfg +logger = logging.getLogger(__name__) + class Pva(BasePva): """The PhysX Pose Velocity Acceleration (PVA) sensor. @@ -75,6 +78,13 @@ def __init__(self, cfg: PvaCfg): # Internal: expression used to build the rigid body view (may be different from cfg.prim_path) self._rigid_parent_expr: str | None = None + self._raw_transforms: wp.array | None = None + self._raw_velocities: wp.array | None = None + self._raw_coms: wp.array | None = None + self._update_cmd: wp.Launch | None = None + self._update_env_mask: wp.array | None = None + self._update_inv_dt: float | None = None + self._use_recorded_launch: bool = False def __str__(self) -> str: """Returns: A string containing information about the instance.""" @@ -181,28 +191,61 @@ def _initialize_impl(self): self._offset_pos_b = wp.from_torch(composed_p.contiguous(), dtype=wp.vec3f) self._offset_quat_b = wp.from_torch(composed_q.contiguous(), dtype=wp.quatf) + self._use_recorded_launch = wp.get_device(self._device).is_cuda + def _update_buffers_impl(self, env_mask: wp.array | None = None): """Fills the buffers of the sensor data.""" env_mask = self._resolve_indices_and_mask(None, env_mask) - # Fetch view data as warp typed arrays - transforms = self._view.get_transforms().view(wp.transformf) - velocities = self._view.get_velocities().view(wp.spatial_vectorf) - # get_coms() returns a CPU warp array; copy to pre-allocated GPU buffer - wp.copy(self._coms_buffer, self._view.get_coms().view(wp.transformf)) - - wp.launch( + transforms = self._view.get_transforms() + velocities = self._view.get_velocities() + coms = self._view.get_coms() + if self._raw_transforms is None: + self._raw_transforms = transforms.view(wp.transformf) + self._raw_velocities = velocities.view(wp.spatial_vectorf) + self._raw_coms = coms.view(wp.transformf) + wp.copy(self._coms_buffer, self._raw_coms) + + inv_dt = 1.0 / self._dt + if self._use_recorded_launch: + if self._update_cmd is None: + try: + self._update_cmd = self._launch_update(env_mask, inv_dt, record_cmd=True) + self._update_env_mask = env_mask + self._update_inv_dt = inv_dt + except Exception as exc: + self._use_recorded_launch = False + logger.warning( + f"Failed to record the update of the PVA sensor at '{self.cfg.prim_path}'." + f" Falling back to eager kernel launches. Reason: {exc}" + ) + if self._update_cmd is not None: + if env_mask is not self._update_env_mask: + self._update_cmd.set_param_by_name("env_mask", env_mask) + self._update_env_mask = env_mask + if inv_dt != self._update_inv_dt: + self._update_cmd.set_param_by_name("inv_dt", inv_dt) + self._update_inv_dt = inv_dt + self._update_cmd.launch() + return + + self._launch_update(env_mask, inv_dt) + + def _launch_update(self, env_mask: wp.array, inv_dt: float, record_cmd: bool = False) -> wp.Launch | None: + """Launch or record the kernel that updates the PVA data.""" + + return wp.launch( pva_update_kernel, dim=self._num_envs, inputs=[ env_mask, - transforms, - velocities, + self._raw_transforms, + self._raw_velocities, self._coms_buffer, self._offset_pos_b, self._offset_quat_b, self.GRAVITY_VEC_W, - 1.0 / self._dt, + inv_dt, self._timestamp, self._prev_lin_vel_w, self._prev_ang_vel_w, @@ -215,6 +258,7 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): self._data._projected_gravity_b, ], device=self._device, + record_cmd=record_cmd, ) def _initialize_buffers_impl(self): @@ -236,6 +280,17 @@ def _initialize_buffers_impl(self): # Pre-allocate GPU buffer for COMs (get_coms() returns CPU array) self._coms_buffer = wp.zeros(self._view.count, dtype=wp.transformf, device=self._device) + def _invalidate_initialize_callback(self, event): + """Invalidate the sensor and release cached PhysX and launch state.""" + super()._invalidate_initialize_callback(event) + self._view = None + self._raw_transforms = None + self._raw_velocities = None + self._raw_coms = None + self._update_cmd = None + self._update_env_mask = None + self._update_inv_dt = None + def _set_debug_vis_impl(self, debug_vis: bool): # set visibility of markers # note: parent only deals with callbacks. not their visibility diff --git a/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py b/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py new file mode 100644 index 000000000000..01e3a64b2e9d --- /dev/null +++ b/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py @@ -0,0 +1,222 @@ +# 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 + +"""Tests for cached PhysX views and recorded IMU/PVA updates.""" + +from isaaclab.app import AppLauncher + +simulation_app = AppLauncher(headless=True).app + +from types import SimpleNamespace + +import pytest +import torch +import warp as wp +from isaaclab_physx.sensors.imu import imu as imu_module +from isaaclab_physx.sensors.imu.imu import Imu +from isaaclab_physx.sensors.imu.imu_data import ImuData +from isaaclab_physx.sensors.pva import pva as pva_module +from isaaclab_physx.sensors.pva.pva import Pva +from isaaclab_physx.sensors.pva.pva_data import PvaData + +from isaaclab.sensors.imu import BaseImu +from isaaclab.sensors.pva import BasePva + + +class _FakeBuffer: + """Wrap a typed array while counting typed-view construction.""" + + def __init__(self, array: wp.array, dtype): + self.array = array + self.dtype = dtype + self.view_count = 0 + + def view(self, dtype): + assert dtype == self.dtype + self.view_count += 1 + return self.array + + +class _FakeRigidView: + """Return stable PhysX-like buffers while counting getter calls.""" + + def __init__(self, transforms: wp.array, velocities: wp.array, coms: wp.array): + self.transforms = _FakeBuffer(transforms, wp.transformf) + self.velocities = _FakeBuffer(velocities, wp.spatial_vectorf) + self.coms = _FakeBuffer(coms, wp.transformf) + self.get_counts = {"transforms": 0, "velocities": 0, "coms": 0} + + def get_transforms(self): + self.get_counts["transforms"] += 1 + return self.transforms + + def get_velocities(self): + self.get_counts["velocities"] += 1 + return self.velocities + + def get_coms(self): + self.get_counts["coms"] += 1 + return self.coms + + +def _make_sensor(sensor_type: str, use_recorded_launch: bool = True): + """Create a two-environment IMU or PVA without a USD scene.""" + device = "cuda:0" + transforms_torch = torch.tensor( + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + ], + dtype=torch.float32, + device=device, + ) + velocities_torch = torch.tensor( + [ + [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + dtype=torch.float32, + device=device, + ) + coms_torch = torch.tensor( + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + ], + dtype=torch.float32, + ) + transforms = wp.from_torch(transforms_torch.contiguous()).view(wp.transformf) + velocities = wp.from_torch(velocities_torch.contiguous()).view(wp.spatial_vectorf) + coms = wp.from_torch(coms_torch.contiguous()).view(wp.transformf) + rigid_view = _FakeRigidView(transforms, velocities, coms) + + sensor_cls = Imu if sensor_type == "imu" else Pva + sensor = sensor_cls.__new__(sensor_cls) + sensor.cfg = SimpleNamespace(prim_path=f"/World/{sensor_type.upper()}") + sensor._device = device + sensor._num_envs = 2 + sensor._view = rigid_view + sensor._dt = 0.5 + sensor._timestamp = wp.ones(2, dtype=wp.float32, device=device) + sensor._offset_pos_b = wp.zeros(2, dtype=wp.vec3f, device=device) + sensor._offset_quat_b = wp.array( + [wp.quatf(0.0, 0.0, 0.0, 1.0), wp.quatf(0.0, 0.0, 0.0, 1.0)], dtype=wp.quatf, device=device + ) + sensor._prev_lin_vel_w = wp.zeros(2, dtype=wp.vec3f, device=device) + sensor._coms_buffer = wp.zeros(2, dtype=wp.transformf, device=device) + sensor._raw_transforms = None + sensor._raw_velocities = None + sensor._raw_coms = None + sensor._update_cmd = None + sensor._update_env_mask = None + sensor._update_inv_dt = None + sensor._use_recorded_launch = use_recorded_launch + sensor._initialize_handle = None + sensor._invalidate_initialize_handle = None + sensor._prim_deletion_handle = None + + if sensor_type == "imu": + sensor._data = ImuData() + sensor._data.create_buffers(num_envs=2, device=device) + sensor._gravity_bias_w = wp.zeros(2, dtype=wp.vec3f, device=device) + else: + sensor._data = PvaData() + sensor._data.create_buffers(num_envs=2, device=device) + sensor._prev_ang_vel_w = wp.zeros(2, dtype=wp.vec3f, device=device) + sensor.GRAVITY_VEC_W = wp.zeros(2, dtype=wp.vec3f, device=device) + + env_mask = wp.ones(2, dtype=wp.bool, device=device) + return sensor, rigid_view, velocities_torch, env_mask + + +@pytest.mark.parametrize("sensor_type", ["imu", "pva"]) +def test_sensor_caches_physx_typed_views(sensor_type): + """Repeated eager updates should reuse typed views over refreshed PhysX buffers.""" + sensor, rigid_view, _, env_mask = _make_sensor(sensor_type, use_recorded_launch=False) + + sensor._update_buffers_impl(env_mask) + sensor._update_buffers_impl(env_mask) + wp.synchronize_device(sensor.device) + + assert rigid_view.get_counts == {"transforms": 2, "velocities": 2, "coms": 2} + assert rigid_view.transforms.view_count == 1 + assert rigid_view.velocities.view_count == 1 + assert rigid_view.coms.view_count == 1 + + +@pytest.mark.parametrize("sensor_type", ["imu", "pva"]) +def test_sensor_records_and_replays_changed_runtime_inputs(sensor_type): + """Replay should observe refreshed buffers, a new mask, and a changed timestep.""" + sensor, _, velocities_torch, env_mask = _make_sensor(sensor_type) + + sensor._update_buffers_impl(env_mask) + wp.synchronize_device(sensor.device) + update_cmd = sensor._update_cmd + + assert update_cmd is not None + torch.testing.assert_close( + wp.to_torch(sensor._data._lin_acc_b)[:, 0], + torch.tensor([2.0, 2.0], device=sensor.device), + ) + + velocities_torch[:, 0] = torch.tensor([3.0, 5.0], device=sensor.device) + sensor._dt = 0.25 + changed_env_mask = wp.array([False, True], dtype=wp.bool, device=sensor.device) + sensor._update_buffers_impl(changed_env_mask) + wp.synchronize_device(sensor.device) + + assert sensor._update_cmd is update_cmd + torch.testing.assert_close( + wp.to_torch(sensor._data._lin_acc_b)[:, 0], + torch.tensor([2.0, 16.0], device=sensor.device), + ) + + +@pytest.mark.parametrize("sensor_type", ["imu", "pva"]) +def test_sensor_falls_back_when_recording_fails(monkeypatch, sensor_type): + """A recording failure should disable recording and execute the update eagerly.""" + sensor, _, _, env_mask = _make_sensor(sensor_type) + sensor_module = imu_module if sensor_type == "imu" else pva_module + original_launch = sensor_module.wp.launch + + def launch_with_recording_failure(*args, record_cmd=False, **kwargs): + if record_cmd: + raise RuntimeError("recording failed") + return original_launch(*args, **kwargs) + + monkeypatch.setattr(sensor_module.wp, "launch", launch_with_recording_failure) + sensor._update_buffers_impl(env_mask) + wp.synchronize_device(sensor.device) + + assert not sensor._use_recorded_launch + assert sensor._update_cmd is None + torch.testing.assert_close( + wp.to_torch(sensor._data._lin_acc_b)[:, 0], + torch.tensor([2.0, 2.0], device=sensor.device), + ) + + +@pytest.mark.parametrize("sensor_type", ["imu", "pva"]) +def test_sensor_invalidation_drops_cached_launch_state(monkeypatch, sensor_type): + """Physics invalidation should release cached PhysX views and the recorded command.""" + sensor, _, _, _ = _make_sensor(sensor_type) + sensor._raw_transforms = object() + sensor._raw_velocities = object() + sensor._raw_coms = object() + sensor._update_cmd = object() + sensor._update_env_mask = object() + sensor._update_inv_dt = 1.0 + base_cls = BaseImu if sensor_type == "imu" else BasePva + monkeypatch.setattr(base_cls, "_invalidate_initialize_callback", lambda self, event: None) + + sensor._invalidate_initialize_callback(None) + + assert sensor._view is None + assert sensor._raw_transforms is None + assert sensor._raw_velocities is None + assert sensor._raw_coms is None + assert sensor._update_cmd is None + assert sensor._update_env_mask is None + assert sensor._update_inv_dt is None From 5f922bc59f8b04b330f848b366f0a71e71f0ee78 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Tue, 7 Jul 2026 17:31:15 +0200 Subject: [PATCH 3/7] Move PhysX sensor benchmark Keep the IMU and PVA benchmark with the PhysX package sensor benchmarks so backend-specific performance tools share one discoverable location. --- .../isaaclab_physx/benchmark/sensors}/benchmark_imu_pva.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) rename {scripts/benchmarks => source/isaaclab_physx/benchmark/sensors}/benchmark_imu_pva.py (95%) diff --git a/scripts/benchmarks/benchmark_imu_pva.py b/source/isaaclab_physx/benchmark/sensors/benchmark_imu_pva.py similarity index 95% rename from scripts/benchmarks/benchmark_imu_pva.py rename to source/isaaclab_physx/benchmark/sensors/benchmark_imu_pva.py index eca69ea688c2..3d4a04f6459f 100644 --- a/scripts/benchmarks/benchmark_imu_pva.py +++ b/source/isaaclab_physx/benchmark/sensors/benchmark_imu_pva.py @@ -6,8 +6,10 @@ """Benchmark the PhysX IMU and PVA sensor update paths. Usage: - ./isaaclab.sh -p scripts/benchmarks/benchmark_imu_pva.py --sensor imu --num_envs 4096 - ./isaaclab.sh -p scripts/benchmarks/benchmark_imu_pva.py --sensor pva --num_envs 4096 + ./isaaclab.sh -p source/isaaclab_physx/benchmark/sensors/benchmark_imu_pva.py \ + --sensor imu --num_envs 4096 + ./isaaclab.sh -p source/isaaclab_physx/benchmark/sensors/benchmark_imu_pva.py \ + --sensor pva --num_envs 4096 """ from __future__ import annotations From d5b2cb99510defe03a2d1478de7b51e7eeb77694 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Wed, 8 Jul 2026 08:45:37 +0200 Subject: [PATCH 4/7] Skip recorded-launch tests without CUDA The recorded-launch unit tests allocate directly on cuda:0 and would error rather than skip on CPU-only runners. Gate the module on CUDA availability and tag it for the short isaacsim CI lane. --- .../test/sensors/test_imu_pva_recorded_launch.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py b/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py index 01e3a64b2e9d..c7ab9ef80b43 100644 --- a/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py +++ b/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py @@ -24,6 +24,11 @@ from isaaclab.sensors.imu import BaseImu from isaaclab.sensors.pva import BasePva +pytestmark = [ + pytest.mark.isaacsim_ci, + pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA is not available"), +] + class _FakeBuffer: """Wrap a typed array while counting typed-view construction.""" From f16e30e3747ea94b64336ab4f8e6dbf5061ed801 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Wed, 8 Jul 2026 10:04:54 +0200 Subject: [PATCH 5/7] Guard cached IMU and PVA views against re-backed buffers The cached typed views and the recorded launch assume the PhysX getters refresh pointer-stable buffers in place. Verify the pointers on every fetch so a re-backed buffer fails loudly instead of silently freezing the sensor data. --- .../isaaclab_physx/sensors/imu/imu.py | 14 ++++++++++++++ .../isaaclab_physx/sensors/pva/pva.py | 14 ++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py index e6d0244d86a1..178fbd2f18e4 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py @@ -164,6 +164,10 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): """Fills the buffers of the sensor data.""" env_mask = self._resolve_indices_and_mask(None, env_mask) + # Refresh the PhysX buffers every update, but create their typed Warp views only once: + # the getters lazily allocate their output buffers and refresh the same memory in place + # on every call, so the cached views (and the recorded launch that consumes them) stay + # valid. A re-backed buffer would silently freeze the sensor data, so fail loudly. transforms = self._view.get_transforms() velocities = self._view.get_velocities() coms = self._view.get_coms() @@ -171,6 +175,16 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): self._raw_transforms = transforms.view(wp.transformf) self._raw_velocities = velocities.view(wp.spatial_vectorf) self._raw_coms = coms.view(wp.transformf) + elif ( + transforms.ptr != self._raw_transforms.ptr + or velocities.ptr != self._raw_velocities.ptr + or coms.ptr != self._raw_coms.ptr + ): + raise RuntimeError( + f"A PhysX rigid body buffer of the sensor at '{self.cfg.prim_path}' was re-allocated" + " after its warp view was cached. The cached views and the recorded launch require" + " pointer-stable buffers refreshed in place." + ) wp.copy(self._coms_buffer, self._raw_coms) inv_dt = 1.0 / self._dt diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py index 6088f99ef985..581ccfef559e 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py @@ -197,6 +197,10 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): """Fills the buffers of the sensor data.""" env_mask = self._resolve_indices_and_mask(None, env_mask) + # Refresh the PhysX buffers every update, but create their typed Warp views only once: + # the getters lazily allocate their output buffers and refresh the same memory in place + # on every call, so the cached views (and the recorded launch that consumes them) stay + # valid. A re-backed buffer would silently freeze the sensor data, so fail loudly. transforms = self._view.get_transforms() velocities = self._view.get_velocities() coms = self._view.get_coms() @@ -204,6 +208,16 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): self._raw_transforms = transforms.view(wp.transformf) self._raw_velocities = velocities.view(wp.spatial_vectorf) self._raw_coms = coms.view(wp.transformf) + elif ( + transforms.ptr != self._raw_transforms.ptr + or velocities.ptr != self._raw_velocities.ptr + or coms.ptr != self._raw_coms.ptr + ): + raise RuntimeError( + f"A PhysX rigid body buffer of the sensor at '{self.cfg.prim_path}' was re-allocated" + " after its warp view was cached. The cached views and the recorded launch require" + " pointer-stable buffers refreshed in place." + ) wp.copy(self._coms_buffer, self._raw_coms) inv_dt = 1.0 / self._dt From 005bc41c9c410b8b31103a0aaa8d141f78e0a983 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Wed, 8 Jul 2026 10:04:54 +0200 Subject: [PATCH 6/7] Assert recorded launches stay active in IMU and PVA tests A recording failure only warns and falls back to eager launches, so nothing would notice the optimization silently dying. Assert the recorded command exists after CUDA integration runs, pass production- shaped ProxyArray gravity in the unit fixture, and assert the PhysX getters are still called on replay since they refresh the buffers. --- source/isaaclab_physx/test/sensors/test_imu.py | 6 ++++++ .../test/sensors/test_imu_pva_recorded_launch.py | 11 +++++++++-- source/isaaclab_physx/test/sensors/test_pva.py | 6 ++++++ 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_physx/test/sensors/test_imu.py b/source/isaaclab_physx/test/sensors/test_imu.py index 422aeeed1f39..19e0eea16c42 100644 --- a/source/isaaclab_physx/test/sensors/test_imu.py +++ b/source/isaaclab_physx/test/sensors/test_imu.py @@ -264,6 +264,12 @@ def test_constant_velocity(setup_sim): prev_lin_acc_ball = scene.sensors["imu_ball"].data.lin_acc_b.torch.clone() prev_lin_acc_cube = scene.sensors["imu_cube"].data.lin_acc_b.torch.clone() + # the recorded-launch optimization must be active on CUDA; a recording failure would only + # warn and silently fall back to eager launches, defeating the optimization. + if "cuda" in str(scene.device): + assert scene.sensors["imu_ball"]._update_cmd is not None + assert scene.sensors["imu_cube"]._update_cmd is not None + @pytest.mark.isaacsim_ci def test_constant_acceleration(setup_sim): diff --git a/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py b/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py index c7ab9ef80b43..eedac185e1c3 100644 --- a/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py +++ b/source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py @@ -23,6 +23,7 @@ from isaaclab.sensors.imu import BaseImu from isaaclab.sensors.pva import BasePva +from isaaclab.utils.warp import ProxyArray pytestmark = [ pytest.mark.isaacsim_ci, @@ -43,6 +44,10 @@ def view(self, dtype): self.view_count += 1 return self.array + @property + def ptr(self): + return self.array.ptr + class _FakeRigidView: """Return stable PhysX-like buffers while counting getter calls.""" @@ -130,7 +135,7 @@ def _make_sensor(sensor_type: str, use_recorded_launch: bool = True): sensor._data = PvaData() sensor._data.create_buffers(num_envs=2, device=device) sensor._prev_ang_vel_w = wp.zeros(2, dtype=wp.vec3f, device=device) - sensor.GRAVITY_VEC_W = wp.zeros(2, dtype=wp.vec3f, device=device) + sensor.GRAVITY_VEC_W = ProxyArray(wp.zeros(2, dtype=wp.vec3f, device=device)) env_mask = wp.ones(2, dtype=wp.bool, device=device) return sensor, rigid_view, velocities_torch, env_mask @@ -154,7 +159,7 @@ def test_sensor_caches_physx_typed_views(sensor_type): @pytest.mark.parametrize("sensor_type", ["imu", "pva"]) def test_sensor_records_and_replays_changed_runtime_inputs(sensor_type): """Replay should observe refreshed buffers, a new mask, and a changed timestep.""" - sensor, _, velocities_torch, env_mask = _make_sensor(sensor_type) + sensor, rigid_view, velocities_torch, env_mask = _make_sensor(sensor_type) sensor._update_buffers_impl(env_mask) wp.synchronize_device(sensor.device) @@ -173,6 +178,8 @@ def test_sensor_records_and_replays_changed_runtime_inputs(sensor_type): wp.synchronize_device(sensor.device) assert sensor._update_cmd is update_cmd + # replays must still call the PhysX getters: they are what refresh the underlying buffers + assert rigid_view.get_counts == {"transforms": 2, "velocities": 2, "coms": 2} torch.testing.assert_close( wp.to_torch(sensor._data._lin_acc_b)[:, 0], torch.tensor([2.0, 16.0], device=sensor.device), diff --git a/source/isaaclab_physx/test/sensors/test_pva.py b/source/isaaclab_physx/test/sensors/test_pva.py index f0c1982373b6..7dba55ec5f1f 100644 --- a/source/isaaclab_physx/test/sensors/test_pva.py +++ b/source/isaaclab_physx/test/sensors/test_pva.py @@ -310,6 +310,12 @@ def test_constant_velocity(setup_sim): prev_lin_acc_cube = scene.sensors["pva_cube"].data.lin_acc_b.torch.clone() prev_ang_acc_cube = scene.sensors["pva_cube"].data.ang_acc_b.torch.clone() + # the recorded-launch optimization must be active on CUDA; a recording failure would only + # warn and silently fall back to eager launches, defeating the optimization. + if "cuda" in str(scene.device): + assert scene.sensors["pva_ball"]._update_cmd is not None + assert scene.sensors["pva_cube"]._update_cmd is not None + @pytest.mark.isaacsim_ci def test_constant_acceleration(setup_sim): From ac8c0adc726d9c7165687649eb537364997cb666 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 9 Jul 2026 17:14:00 +0200 Subject: [PATCH 7/7] Fix lazy IMU and PVA acceleration Derive finite-difference acceleration from the elapsed time between sensor samples. This keeps lazy reads and nonzero update periods correct while preserving recorded Warp launches. --- ...oiner-fix-inertial-sensor-elapsed-time.rst | 0 .../isaaclab_physx/sensors/imu/imu.py | 19 ++------- .../isaaclab_physx/sensors/imu/kernels.py | 9 ++++- .../isaaclab_physx/sensors/pva/kernels.py | 9 ++++- .../isaaclab_physx/sensors/pva/pva.py | 21 ++-------- .../isaaclab_physx/test/sensors/test_imu.py | 32 +++++++++++++++ .../isaaclab_physx/test/sensors/test_pva.py | 39 +++++++++++++++++++ 7 files changed, 93 insertions(+), 36 deletions(-) create mode 100644 source/isaaclab_physx/changelog.d/antoiner-fix-inertial-sensor-elapsed-time.rst diff --git a/source/isaaclab_physx/changelog.d/antoiner-fix-inertial-sensor-elapsed-time.rst b/source/isaaclab_physx/changelog.d/antoiner-fix-inertial-sensor-elapsed-time.rst new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py index 178fbd2f18e4..a111a9a14e18 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py @@ -70,7 +70,6 @@ def __init__(self, cfg: ImuCfg): self._raw_coms: wp.array | None = None self._update_cmd: wp.Launch | None = None self._update_env_mask: wp.array | None = None - self._update_inv_dt: float | None = None self._use_recorded_launch: bool = False def __str__(self) -> str: @@ -115,10 +114,6 @@ def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None device=self._device, ) - def update(self, dt: float, force_recompute: bool = False): - self._dt = dt - super().update(dt, force_recompute) - """ Implementation. """ @@ -187,13 +182,11 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): ) wp.copy(self._coms_buffer, self._raw_coms) - inv_dt = 1.0 / self._dt if self._use_recorded_launch: if self._update_cmd is None: try: - self._update_cmd = self._launch_update(env_mask, inv_dt, record_cmd=True) + self._update_cmd = self._launch_update(env_mask, record_cmd=True) self._update_env_mask = env_mask - self._update_inv_dt = inv_dt except Exception as exc: self._use_recorded_launch = False logger.warning( @@ -204,15 +197,12 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): if env_mask is not self._update_env_mask: self._update_cmd.set_param_by_name("env_mask", env_mask) self._update_env_mask = env_mask - if inv_dt != self._update_inv_dt: - self._update_cmd.set_param_by_name("inv_dt", inv_dt) - self._update_inv_dt = inv_dt self._update_cmd.launch() return - self._launch_update(env_mask, inv_dt) + self._launch_update(env_mask) - def _launch_update(self, env_mask: wp.array, inv_dt: float, record_cmd: bool = False) -> wp.Launch | None: + def _launch_update(self, env_mask: wp.array, record_cmd: bool = False) -> wp.Launch | None: """Launch or record the kernel that updates the IMU data.""" return wp.launch( @@ -226,8 +216,8 @@ def _launch_update(self, env_mask: wp.array, inv_dt: float, record_cmd: bool = F self._offset_pos_b, self._offset_quat_b, self._gravity_bias_w, - inv_dt, self._timestamp, + self._timestamp_last_update, self._prev_lin_vel_w, self._data._ang_vel_b, self._data._lin_acc_b, @@ -258,4 +248,3 @@ def _invalidate_initialize_callback(self, event): self._raw_coms = None self._update_cmd = None self._update_env_mask = None - self._update_inv_dt = None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py index 90111579857b..e7f739377f44 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py @@ -16,8 +16,8 @@ def imu_update_kernel( offset_pos_b: wp.array(dtype=wp.vec3f), offset_quat_b: wp.array(dtype=wp.quatf), gravity_bias_w: wp.array(dtype=wp.vec3f), - inv_dt: wp.float32, timestamp: wp.array(dtype=wp.float32), + timestamp_last_update: wp.array(dtype=wp.float32), # inputs / outputs prev_lin_vel_w: wp.array(dtype=wp.vec3f), # outputs @@ -34,8 +34,8 @@ def imu_update_kernel( offset_pos_b: Offset positions of the sensors. offset_quat_b: Offset quaternions of the sensors. gravity_bias_w: Gravity bias in the world frame. - inv_dt: Inverse of the time step. timestamp: Timestamp of the environment. + timestamp_last_update: Timestamp of the previous sensor sample. prev_lin_vel_w: Previous linear velocity in the world frame. out_ang_vel_b: Output angular velocity in the body frame. out_lin_acc_b: Output linear acceleration in the body frame. @@ -49,6 +49,11 @@ def imu_update_kernel( if timestamp[idx] == 0.0: return + elapsed_time = timestamp[idx] - timestamp_last_update[idx] + if elapsed_time <= 0.0: + return + inv_dt = 1.0 / elapsed_time + body_quat = wp.transform_get_rotation(transforms[idx]) lin_vel_w = wp.spatial_top(velocities[idx]) diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py index f08b7f671fbf..efb1710c30c8 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py @@ -16,8 +16,8 @@ def pva_update_kernel( offset_pos_b: wp.array(dtype=wp.vec3f), offset_quat_b: wp.array(dtype=wp.quatf), gravity_vec_w: wp.array(dtype=wp.vec3f), - inv_dt: wp.float32, timestamp: wp.array(dtype=wp.float32), + timestamp_last_update: wp.array(dtype=wp.float32), # inputs / outputs prev_lin_vel_w: wp.array(dtype=wp.vec3f), prev_ang_vel_w: wp.array(dtype=wp.vec3f), @@ -40,8 +40,8 @@ def pva_update_kernel( offset_pos_b: Offset positions of the sensors. offset_quat_b: Offset quaternions of the sensors. gravity_vec_w: Gravity direction unit vector in the world frame. - inv_dt: Inverse of the time step. timestamp: Timestamp of the environment. + timestamp_last_update: Timestamp of the previous sensor sample. prev_lin_vel_w: Previous linear velocity in the world frame. prev_ang_vel_w: Previous angular velocity in the world frame. out_pos_w: Output position in the world frame. @@ -61,6 +61,11 @@ def pva_update_kernel( if timestamp[idx] == 0.0: return + elapsed_time = timestamp[idx] - timestamp_last_update[idx] + if elapsed_time <= 0.0: + return + inv_dt = 1.0 / elapsed_time + body_pos = wp.transform_get_translation(transforms[idx]) body_quat = wp.transform_get_rotation(transforms[idx]) diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py index 581ccfef559e..ca1a5e498806 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py @@ -83,7 +83,6 @@ def __init__(self, cfg: PvaCfg): self._raw_coms: wp.array | None = None self._update_cmd: wp.Launch | None = None self._update_env_mask: wp.array | None = None - self._update_inv_dt: float | None = None self._use_recorded_launch: bool = False def __str__(self) -> str: @@ -138,12 +137,6 @@ def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None device=self._device, ) - def update(self, dt: float, force_recompute: bool = False): - # save timestamp - self._dt = dt - # execute updating - super().update(dt, force_recompute) - """ Implementation. """ @@ -220,13 +213,11 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): ) wp.copy(self._coms_buffer, self._raw_coms) - inv_dt = 1.0 / self._dt if self._use_recorded_launch: if self._update_cmd is None: try: - self._update_cmd = self._launch_update(env_mask, inv_dt, record_cmd=True) + self._update_cmd = self._launch_update(env_mask, record_cmd=True) self._update_env_mask = env_mask - self._update_inv_dt = inv_dt except Exception as exc: self._use_recorded_launch = False logger.warning( @@ -237,15 +228,12 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): if env_mask is not self._update_env_mask: self._update_cmd.set_param_by_name("env_mask", env_mask) self._update_env_mask = env_mask - if inv_dt != self._update_inv_dt: - self._update_cmd.set_param_by_name("inv_dt", inv_dt) - self._update_inv_dt = inv_dt self._update_cmd.launch() return - self._launch_update(env_mask, inv_dt) + self._launch_update(env_mask) - def _launch_update(self, env_mask: wp.array, inv_dt: float, record_cmd: bool = False) -> wp.Launch | None: + def _launch_update(self, env_mask: wp.array, record_cmd: bool = False) -> wp.Launch | None: """Launch or record the kernel that updates the PVA data.""" return wp.launch( @@ -259,8 +247,8 @@ def _launch_update(self, env_mask: wp.array, inv_dt: float, record_cmd: bool = F self._offset_pos_b, self._offset_quat_b, self.GRAVITY_VEC_W, - inv_dt, self._timestamp, + self._timestamp_last_update, self._prev_lin_vel_w, self._prev_ang_vel_w, self._data._pos_w, @@ -303,7 +291,6 @@ def _invalidate_initialize_callback(self, event): self._raw_coms = None self._update_cmd = None self._update_env_mask = None - self._update_inv_dt = None def _set_debug_vis_impl(self, debug_vis: bool): # set visibility of markers diff --git a/source/isaaclab_physx/test/sensors/test_imu.py b/source/isaaclab_physx/test/sensors/test_imu.py index 19e0eea16c42..c501c83e708c 100644 --- a/source/isaaclab_physx/test/sensors/test_imu.py +++ b/source/isaaclab_physx/test/sensors/test_imu.py @@ -553,3 +553,35 @@ def test_sensor_print(setup_sim): sensor = scene.sensors["imu_ball"] # print info print(sensor) + + +@pytest.mark.parametrize("access_mode", ("lazy_read", "update_period")) +def test_acceleration_uses_elapsed_sensor_time(setup_sim, access_mode): + """Acceleration uses the elapsed time between sensor samples.""" + sim, scene = setup_sim + dt = sim.get_physics_dt() + body = scene.rigid_objects["balls"] + sensor = scene.sensors["imu_ball"] + velocity = torch.zeros((scene.num_envs, 6), dtype=torch.float32, device=scene.device) + + body.write_root_velocity_to_sim_index(root_velocity=velocity) + scene.write_data_to_sim() + sim.step() + scene.update(dt) + _ = sensor.data + + scene.cfg.lazy_sensor_update = True + if access_mode == "update_period": + sensor.cfg.update_period = 4 * dt + + for step in range(4): + velocity[:, 0] = 0.1 * (step + 1) + body.write_root_velocity_to_sim_index(root_velocity=velocity) + scene.write_data_to_sim() + sim.step() + scene.update(dt) + if access_mode == "update_period": + _ = sensor.data + + expected = torch.full((scene.num_envs,), 0.1 / dt, device=scene.device) + torch.testing.assert_close(sensor.data.lin_acc_b.torch[:, 0], expected) diff --git a/source/isaaclab_physx/test/sensors/test_pva.py b/source/isaaclab_physx/test/sensors/test_pva.py index 7dba55ec5f1f..539fdb73a4d0 100644 --- a/source/isaaclab_physx/test/sensors/test_pva.py +++ b/source/isaaclab_physx/test/sensors/test_pva.py @@ -826,3 +826,42 @@ def test_sensor_print(setup_sim): sensor = scene.sensors["pva_ball"] # print info print(sensor) + + +@pytest.mark.parametrize("access_mode", ("lazy_read", "update_period")) +def test_acceleration_uses_elapsed_sensor_time(setup_sim, access_mode): + """Linear and angular acceleration use the elapsed time between sensor samples.""" + sim, scene = setup_sim + dt = sim.get_physics_dt() + body = scene.rigid_objects["balls"] + sensor = scene.sensors["pva_ball"] + velocity = torch.zeros((scene.num_envs, 6), dtype=torch.float32, device=scene.device) + + body.write_root_velocity_to_sim_index(root_velocity=velocity) + scene.write_data_to_sim() + sim.step() + scene.update(dt) + _ = sensor.data + + scene.cfg.lazy_sensor_update = True + if access_mode == "update_period": + sensor.cfg.update_period = 4 * dt + + for step in range(4): + velocity[:, 0] = 0.1 * (step + 1) + velocity[:, 5] = 0.2 * (step + 1) + body.write_root_velocity_to_sim_index(root_velocity=velocity) + scene.write_data_to_sim() + sim.step() + scene.update(dt) + if access_mode == "update_period": + _ = sensor.data + + expected_lin_acc = torch.full((scene.num_envs,), 0.1 / dt, device=scene.device) + expected_ang_acc = torch.full((scene.num_envs,), 0.2 / dt, device=scene.device) + torch.testing.assert_close( + torch.linalg.vector_norm(sensor.data.lin_acc_b.torch, dim=-1), expected_lin_acc, rtol=1e-4, atol=1e-3 + ) + torch.testing.assert_close( + torch.linalg.vector_norm(sensor.data.ang_acc_b.torch, dim=-1), expected_ang_acc, rtol=1e-4, atol=1e-3 + )