From 02e3abff98e92db828510dd66a1dd1e285dface4 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 9 Jul 2026 17:14:00 +0200 Subject: [PATCH] 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 + )