Fix lazy PhysX IMU and PVA acceleration (#6439)

## Summary

- compute PhysX IMU and PVA finite-difference acceleration from the
elapsed time between sensor samples
- keep timestamp inputs stable inside the recorded Warp launches from
#6390
- remove mutable scalar inverse-dt launch parameters
- add lazy-read and nonzero-update-period regressions for IMU linear
acceleration and PVA linear/angular acceleration

This PR is stacked on #6390. Until #6390 merges, its commits are
included in this comparison against develop.

## Root cause

IMU and PVA cached velocity only when their buffers were recomputed, but
divided the next velocity delta by the most recent physics-step dt. With
four lazy physics updates or an update period of four physics steps,
both sensors reported 4x acceleration.

The kernels now derive a per-environment interval from timestamp minus
timestamp_last_update.

## Validation

- Unmodified #6390 base: 4 regressions failed with the expected 4x
acceleration error
- Fixed targeted regressions: 4 passed
- Complete PhysX IMU suite: 11 passed
- Complete PhysX PVA suite: 11 passed
- Final pre-commit hooks: passed
- Existing recorded-launch assertions remain enabled and pass

## Performance

Benchmarking is deferred until the test machine is connected to AC
power. The change removes scalar launch-parameter updates and retains
the recorded-launch path; this PR makes no performance claims.

---------

Co-authored-by: Kelly Guo <kellyg@nvidia.com>

Author: AntoineRichard

source/isaaclab_physx/changelog.d/antoiner-fix-inertial-sensor-elapsed-time.rst

@@ -0,0 +1,4 @@
+Fixed
+^^^^^
+
+* Fixed PhysX IMU and PVA acceleration for lazy reads and nonzero update periods.

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

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])

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])
 

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

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)

source/isaaclab_physx/test/sensors/test_imu_pva_recorded_launch.py

@@ -108,8 +108,8 @@ def _make_sensor(sensor_type: str, use_recorded_launch: bool = True):
     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._timestamp_last_update = wp.full(2, value=0.5, 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
@@ -121,7 +121,6 @@ def _make_sensor(sensor_type: str, use_recorded_launch: bool = True):
     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
@@ -158,7 +157,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."""
+    """Replay should observe refreshed buffers, a new mask, and a changed sample interval."""
     sensor, rigid_view, velocities_torch, env_mask = _make_sensor(sensor_type)
 
     sensor._update_buffers_impl(env_mask)
@@ -172,7 +171,8 @@ def test_sensor_records_and_replays_changed_runtime_inputs(sensor_type):
     )
 
     velocities_torch[:, 0] = torch.tensor([3.0, 5.0], device=sensor.device)
-    sensor._dt = 0.25
+    wp.to_torch(sensor._timestamp_last_update).fill_(1.0)
+    wp.to_torch(sensor._timestamp).fill_(1.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)
@@ -219,7 +219,6 @@ def test_sensor_invalidation_drops_cached_launch_state(monkeypatch, sensor_type)
     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)
 
@@ -231,4 +230,3 @@ def test_sensor_invalidation_drops_cached_launch_state(monkeypatch, sensor_type)
     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

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
+    )