Fix lazy PhysX IMU and PVA acceleration

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