diff --git a/source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_imu.minor.rst b/source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_imu.minor.rst new file mode 100644 index 000000000000..953e9acc8a10 --- /dev/null +++ b/source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_imu.minor.rst @@ -0,0 +1,10 @@ +Added +^^^^^ + +* Added :class:`~isaaclab_ovphysx.sensors.Imu` and + :class:`~isaaclab_ovphysx.sensors.ImuData` implementing the + :class:`~isaaclab.sensors.imu.BaseImu` / + :class:`~isaaclab.sensors.imu.BaseImuData` contracts on the OVPhysX + backend. Reports angular velocity and proper linear acceleration in + the sensor body frame using ovphysx tensor bindings on the rigid-body + ancestor of the sensor prim path. diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py index 160f6a12abc8..01803d51a769 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py @@ -390,6 +390,20 @@ def get_physx_instance(cls) -> Any: """Return the underlying ovphysx.PhysX instance (or None if not yet created).""" return cls._physx + @classmethod + def get_gravity(cls) -> tuple[float, float, float]: + """Return the world-frame gravity vector [m/s^2] from the active simulation cfg. + + Mirrors PhysX's ``SimulationView.get_gravity()`` so backend-agnostic sensor code + can read gravity through one classmethod. + + Raises: + RuntimeError: If no simulation is active. Call :meth:`initialize` first. + """ + if cls._sim is None or not hasattr(cls._sim, "cfg"): + raise RuntimeError("OvPhysxManager has not been initialized yet.") + return cls._sim.cfg.gravity + @classmethod def get_scene_data_backend(cls) -> SceneDataBackend: """Return the SceneDataBackend for the central SceneDataProvider. diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi index e00fc97cbbc6..7d41b088a908 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi @@ -7,6 +7,9 @@ __all__ = [ "ContactSensor", "ContactSensorCfg", "ContactSensorData", + "Imu", + "ImuData", ] from .contact_sensor import ContactSensor, ContactSensorCfg, ContactSensorData +from .imu import Imu, ImuData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/__init__.py new file mode 100644 index 000000000000..56104ed5830b --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/__init__.py @@ -0,0 +1,10 @@ +# 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 + +"""Sub-module for OVPhysX IMU sensor.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/__init__.pyi new file mode 100644 index 000000000000..7513020a3ac8 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/__init__.pyi @@ -0,0 +1,12 @@ +# 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 + +__all__ = [ + "Imu", + "ImuData", +] + +from .imu import Imu +from .imu_data import ImuData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/imu.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/imu.py new file mode 100644 index 000000000000..0ce9539bf8ed --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/imu.py @@ -0,0 +1,244 @@ +# 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 + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch +import warp as wp + +import isaaclab.utils.math as math_utils +from isaaclab.sensors.imu import BaseImu + +import isaaclab_ovphysx.tensor_types as TT +from isaaclab_ovphysx.physics import OvPhysxManager as SimulationManager + +from .imu_data import ImuData +from .kernels import imu_reset_kernel, imu_update_kernel + +if TYPE_CHECKING: + from isaaclab.sensors.imu import ImuCfg + + +class Imu(BaseImu): + """The OVPhysX Inertial Measurement Unit (IMU) sensor. + + This sensor models a real IMU that measures angular velocity (gyroscope) and + linear acceleration (accelerometer) in the sensor's body frame. Unlike the PVA + sensor, it does not provide pose, linear velocity, angular acceleration, or + projected gravity. + + Like a real accelerometer, the linear acceleration readings always include the + contribution of gravity. The gravity vector is queried from the simulation at + initialization. + + The sensor can be attached to any prim path with a rigid ancestor in its tree. + If the provided path is not a rigid body, the closest rigid-body ancestor is + used for simulation queries. The fixed transform from that ancestor to the + target prim is computed once during initialization and composed with the + configured sensor offset. + + .. note:: + + Linear acceleration is computed using numerical differentiation from + velocities. Consequently, the IMU sensor accuracy depends on the chosen + physics timestep. For sufficient accuracy, we recommend keeping the + timestep at least 200 Hz. + """ + + cfg: ImuCfg + """The configuration parameters.""" + + __backend_name__: str = "ovphysx" + """The name of the backend for the IMU sensor.""" + + def __init__(self, cfg: ImuCfg): + """Initializes the IMU sensor. + + Args: + cfg: The configuration parameters. + """ + super().__init__(cfg) + self._data = ImuData() + self._rigid_parent_expr: str | None = None + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Imu sensor @ '{self.cfg.prim_path}': \n" + f"\tbinding pattern : {self._rigid_parent_expr}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of sensors : {self._num_bodies}\n" + ) + + """ + Properties + """ + + @property + def data(self) -> ImuData: + self._update_outdated_buffers() + return self._data + + @property + def num_instances(self) -> int: + return self._num_bodies + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None): + env_mask = self._resolve_indices_and_mask(env_ids, env_mask) + super().reset(None, env_mask) + + wp.launch( + imu_reset_kernel, + dim=self._num_envs, + inputs=[ + env_mask, + self._data._ang_vel_b, + self._data._lin_acc_b, + self._prev_lin_vel_w, + ], + device=self._device, + ) + + def update(self, dt: float, force_recompute: bool = False): + self._dt = dt + super().update(dt, force_recompute) + + """ + Implementation. + """ + + def _initialize_impl(self): + """Initializes the sensor handles and internal buffers. + + - If the target prim path is a rigid body, bind directly to it. + - Otherwise find the closest rigid-body ancestor, cache the fixed transform + from that ancestor to the target prim, and bind to the ancestor pattern. + """ + super()._initialize_impl() + + physx_instance = SimulationManager.get_physx_instance() + if physx_instance is None: + raise RuntimeError("OvPhysxManager has not been initialized yet.") + + self._rigid_parent_expr, fixed_pos_b, fixed_quat_b = self._resolve_rigid_body_ancestor_expr() + + # Translate the regex-style path expression to an ovphysx fnmatch glob. + pattern = self._rigid_parent_expr.replace(".*", "*") + + self._pose_binding = physx_instance.create_tensor_binding(pattern=pattern, tensor_type=TT.RIGID_BODY_POSE) + self._vel_binding = physx_instance.create_tensor_binding(pattern=pattern, tensor_type=TT.RIGID_BODY_VELOCITY) + self._com_binding = physx_instance.create_tensor_binding(pattern=pattern, tensor_type=TT.RIGID_BODY_COM_POSE) + self._num_bodies = self._pose_binding.count + + if self._num_bodies != self._num_envs: + raise ValueError( + f"OvPhysx Imu: pattern '{pattern}' matched {self._num_bodies} rigid bodies; expected exactly one" + f" body per environment (num_envs={self._num_envs}). Check that the prim path or its rigid-body" + " ancestor is unique per env." + ) + + gravity = SimulationManager.get_gravity() + gravity_bias = torch.tensor((-gravity[0], -gravity[1], -gravity[2]), device=self._device) + gravity_bias_torch = gravity_bias.repeat(self._num_bodies, 1) + self._gravity_bias_w = wp.from_torch(gravity_bias_torch.contiguous(), dtype=wp.vec3f) + + self._initialize_buffers_impl() + + # Compose the configured offset with the fixed ancestor->target transform (done once). + if fixed_pos_b is not None and fixed_quat_b is not None: + fixed_p = torch.tensor(fixed_pos_b, device=self._device).repeat(self._num_bodies, 1) + fixed_q = torch.tensor(fixed_quat_b, device=self._device).repeat(self._num_bodies, 1) + + cfg_p = wp.to_torch(self._offset_pos_b).clone() + cfg_q = wp.to_torch(self._offset_quat_b).clone() + + composed_p = fixed_p + math_utils.quat_apply(fixed_q, cfg_p) + composed_q = math_utils.quat_mul(fixed_q, cfg_q) + + 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) + + 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) + + # ovphysx ``binding.read(dst)`` writes into the pre-allocated dst buffer; + # ``_*_view`` are float32 aliases of the structured-dtype buffers below. + self._pose_binding.read(self._transforms_view) + self._vel_binding.read(self._velocities_view) + # RIGID_BODY_COM_POSE is a CPU tensor type in the OVPhysX wheel. + # For GPU simulations, stage on CPU then copy into the kernel buffer. + self._com_binding.read(self._coms_read_view) + if self._coms_read_view is not self._coms_gpu_view: + wp.copy(self._coms_gpu_view, self._coms_read_view) + + wp.launch( + imu_update_kernel, + dim=self._num_envs, + inputs=[ + env_mask, + self._transforms, + self._velocities, + self._coms_buffer, + self._offset_pos_b, + self._offset_quat_b, + self._gravity_bias_w, + 1.0 / self._dt, + self._timestamp, + self._prev_lin_vel_w, + self._data._ang_vel_b, + self._data._lin_acc_b, + ], + device=self._device, + ) + + def _initialize_buffers_impl(self): + """Create buffers for storing data.""" + self._data.create_buffers(num_envs=self._num_bodies, device=self._device) + + self._prev_lin_vel_w = wp.zeros(self._num_bodies, dtype=wp.vec3f, device=self._device) + + offset_pos_torch = torch.tensor(list(self.cfg.offset.pos), device=self._device).repeat(self._num_bodies, 1) + offset_quat_torch = torch.tensor(list(self.cfg.offset.rot), device=self._device).repeat(self._num_bodies, 1) + self._offset_pos_b = wp.from_torch(offset_pos_torch.contiguous(), dtype=wp.vec3f) + self._offset_quat_b = wp.from_torch(offset_quat_torch.contiguous(), dtype=wp.quatf) + + # Structured-dtype buffers consumed by the kernel. + self._transforms = wp.zeros(self._num_bodies, dtype=wp.transformf, device=self._device) + self._velocities = wp.zeros(self._num_bodies, dtype=wp.spatial_vectorf, device=self._device) + self._coms_buffer = wp.zeros(self._num_bodies, dtype=wp.transformf, device=self._device) + + self._transforms_view = wp.array( + ptr=self._transforms.ptr, + shape=self._pose_binding.shape, + dtype=wp.float32, + device=self._device, + copy=False, + ) + self._velocities_view = wp.array( + ptr=self._velocities.ptr, + shape=self._vel_binding.shape, + dtype=wp.float32, + device=self._device, + copy=False, + ) + self._coms_gpu_view = wp.array( + ptr=self._coms_buffer.ptr, + shape=self._com_binding.shape, + dtype=wp.float32, + device=self._device, + copy=False, + ) + if self._device == "cpu": + self._coms_read_view = self._coms_gpu_view + else: + self._coms_read_view = wp.zeros(self._com_binding.shape, dtype=wp.float32, device="cpu", pinned=True) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/imu_data.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/imu_data.py new file mode 100644 index 000000000000..131e7bca9c45 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/imu_data.py @@ -0,0 +1,53 @@ +# 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 + +from __future__ import annotations + +import warp as wp + +from isaaclab.sensors.imu import BaseImuData +from isaaclab.utils.warp import ProxyArray + + +class ImuData(BaseImuData): + """Data container for the OVPhysX IMU sensor.""" + + @property + def ang_vel_b(self) -> ProxyArray: + """IMU frame angular velocity relative to the world expressed in IMU frame [rad/s]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._ang_vel_b_ta is None: + self._ang_vel_b_ta = ProxyArray(self._ang_vel_b) + return self._ang_vel_b_ta + + @property + def lin_acc_b(self) -> ProxyArray: + """Linear acceleration (proper) in the IMU frame [m/s^2]. + + Zero in freefall, +g upward at rest. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._lin_acc_b_ta is None: + self._lin_acc_b_ta = ProxyArray(self._lin_acc_b) + return self._lin_acc_b_ta + + def create_buffers(self, num_envs: int, device: str) -> None: + """Create internal buffers for sensor data. + + Args: + num_envs: Number of environments. + device: Device for tensor storage. + """ + self._num_envs = num_envs + self._device = device + self._ang_vel_b = wp.zeros(num_envs, dtype=wp.vec3f, device=device) + self._lin_acc_b = wp.zeros(num_envs, dtype=wp.vec3f, device=device) + + # -- Pinned ProxyArray cache (one per read property, lazily created on first access) + self._ang_vel_b_ta: ProxyArray | None = None + self._lin_acc_b_ta: ProxyArray | None = None diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/kernels.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/kernels.py new file mode 100644 index 000000000000..abc6888d827f --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/imu/kernels.py @@ -0,0 +1,91 @@ +# 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 + +import warp as wp + + +@wp.kernel +def imu_update_kernel( + # inputs + env_mask: wp.array(dtype=wp.bool), + transforms: wp.array(dtype=wp.transformf), + velocities: wp.array(dtype=wp.spatial_vectorf), + coms: wp.array(dtype=wp.transformf), + 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), + # inputs / outputs + prev_lin_vel_w: wp.array(dtype=wp.vec3f), + # outputs + out_ang_vel_b: wp.array(dtype=wp.vec3f), + out_lin_acc_b: wp.array(dtype=wp.vec3f), +): + """Update the IMU sensor data. + + Args: + env_mask: Mask of environments to update. + transforms: Transforms of the bodies. + velocities: Velocities of the bodies. + coms: COMs of the bodies. + 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. + 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. + """ + idx = wp.tid() + if not env_mask[idx]: + return + + # Skip envs that have not been stepped since their last reset: OVPhysX velocities still + # hold pre-reset values, so the finite-difference acceleration would be spurious. + if timestamp[idx] == 0.0: + return + + body_quat = wp.transform_get_rotation(transforms[idx]) + + lin_vel_w = wp.spatial_top(velocities[idx]) + ang_vel_w = wp.spatial_bottom(velocities[idx]) + + com_pos_b = wp.transform_get_translation(coms[idx]) + lever_arm = wp.quat_rotate(body_quat, offset_pos_b[idx] - com_pos_b) + lin_vel_w = lin_vel_w + wp.cross(ang_vel_w, lever_arm) + lin_acc_w = (lin_vel_w - prev_lin_vel_w[idx]) * inv_dt + gravity_bias_w[idx] + + sensor_quat = body_quat * offset_quat_b[idx] + out_ang_vel_b[idx] = wp.quat_rotate_inv(sensor_quat, ang_vel_w) + out_lin_acc_b[idx] = wp.quat_rotate_inv(sensor_quat, lin_acc_w) + + # Update previous velocity + prev_lin_vel_w[idx] = lin_vel_w + + +@wp.kernel +def imu_reset_kernel( + env_mask: wp.array(dtype=wp.bool), + out_ang_vel_b: wp.array(dtype=wp.vec3f), + out_lin_acc_b: wp.array(dtype=wp.vec3f), + prev_lin_vel_w: wp.array(dtype=wp.vec3f), +): + """Reset the IMU sensor data. + + Args: + env_mask: Mask of environments to reset. + out_ang_vel_b: Output angular velocity in the body frame. + out_lin_acc_b: Output linear acceleration in the body frame. + prev_lin_vel_w: Previous linear velocity in the world frame. + """ + idx = wp.tid() + if not env_mask[idx]: + return + + out_ang_vel_b[idx] = wp.vec3f(0.0, 0.0, 0.0) + out_lin_acc_b[idx] = wp.vec3f(0.0, 0.0, 0.0) + prev_lin_vel_w[idx] = wp.vec3f(0.0, 0.0, 0.0) diff --git a/source/isaaclab_ovphysx/test/sensors/check_imu.py b/source/isaaclab_ovphysx/test/sensors/check_imu.py new file mode 100644 index 000000000000..6a382cc13a3d --- /dev/null +++ b/source/isaaclab_ovphysx/test/sensors/check_imu.py @@ -0,0 +1,79 @@ +# 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 + +"""Standalone validation script for the OVPhysX IMU sensor. + +Run with: + ./isaaclab.sh -p source/isaaclab_ovphysx/test/sensors/check_imu.py + +Spawns a free-falling sphere with an IMU attached, steps the simulation for +100 steps, and prints the angular velocity and linear acceleration in the +IMU body frame. In freefall the proper acceleration is zero (only gravity +acts on the body), so the IMU reading should be approximately ``[0, 0, 0]`` +— the finite-difference world-frame acceleration ``(-g)`` cancels the IMU's +gravity bias ``(+g)``. + +Runs kitless under ``./isaaclab.sh -p`` — no AppLauncher needed. +""" + +from __future__ import annotations + +import torch +from isaaclab_ovphysx.physics import OvPhysxCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObject, RigidObjectCfg +from isaaclab.sensors.imu import Imu, ImuCfg +from isaaclab.sim import SimulationCfg, build_simulation_context + + +def main() -> None: + sim_cfg = SimulationCfg(physics=OvPhysxCfg(), device="cpu", dt=0.005) + with build_simulation_context(sim_cfg=sim_cfg) as sim: + # Spawn one ball per env at /World/env_/ball. + spawn_cfg = sim_utils.SphereCfg( + radius=0.25, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ) + # /World/env_ Xforms are siblings under /World — no envs container needed + num_envs = 2 + for i in range(num_envs): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i * 5.0, 0.0, 0.0)) + spawn_cfg.func(f"/World/env_{i}/ball", spawn_cfg, translation=(0.0, 0.0, 1.0)) + + balls = RigidObject( + RigidObjectCfg( + prim_path="/World/env_*/ball", + spawn=None, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + ) + imu = Imu(ImuCfg(prim_path="/World/env_*/ball")) + sim.reset() + + dt = sim.get_physics_dt() + for step in range(100): + balls.write_data_to_sim() + sim.step() + balls.update(dt) + imu.update(dt, force_recompute=True) + + if step in (0, 10, 50, 99): + ang = imu.data.ang_vel_b.torch + lin = imu.data.lin_acc_b.torch + print(f"step={step:3d} ang_vel_b={ang[0].cpu().numpy()} lin_acc_b={lin[0].cpu().numpy()}") + + # Final sanity check: in freefall the proper acceleration is zero + # (gravity cancels the bias). The norm should be near 0. + final_lin = imu.data.lin_acc_b.torch + magnitude = torch.linalg.norm(final_lin, dim=1).cpu().numpy() + print(f"final |lin_acc_b| per env = {magnitude}") + + +if __name__ == "__main__": + main() diff --git a/source/isaaclab_ovphysx/test/sensors/test_imu.py b/source/isaaclab_ovphysx/test/sensors/test_imu.py new file mode 100644 index 000000000000..307522b4d61a --- /dev/null +++ b/source/isaaclab_ovphysx/test/sensors/test_imu.py @@ -0,0 +1,705 @@ +# 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 + +# pyright: reportPrivateUsage=none + +"""Real-backend tests for the OVPhysX IMU sensor. + +Mirrors the structure of source/isaaclab_physx/test/sensors/test_imu.py +but runs kitless under ./isaaclab.sh -p -m pytest — no AppLauncher needed. +SimulationContext is instantiated directly (it does not require Kit), and +UsdFileCfg(usd_path=ISAAC_NUCLEUS_DIR/...) downloads Nucleus assets via +omni.client (which works standalone in Kit's Python). + +Tests that load the PhysX pendulum URDF (``test_single_dof_pendulum`` and +``test_indirect_attachment``) are skipped pending a USD-converted pendulum +asset. URDF→USD conversion requires the Kit URDF importer extension, which +is not loaded under the direct ./isaaclab.sh -p runner. + +Process-global wheel state: like the rigid-object test, this file mixes +procedural USD assets (``test_constant_velocity``, ``test_constant_acceleration``, +``test_attachment_validity``, ``test_sensor_print``) with Nucleus assets +(``test_offset_calculation``, ``test_env_ids_propagation``). ``omni.client`` +must be loaded before the first OVPhysX scene is torn down; otherwise a later +first import can fail native symbol resolution after ``ovphysx.reset()``. +""" + +from __future__ import annotations + +# --------------------------------------------------------------------------- +# Wheel gate: skip the whole file if the ovphysx wheel is missing or too old. +# --------------------------------------------------------------------------- +import pytest + +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") +_TT_module = pytest.importorskip( + "isaaclab_ovphysx.tensor_types", + reason="isaaclab_ovphysx.tensor_types not importable", +) +if not hasattr(_TT_module, "RIGID_BODY_POSE"): + pytest.skip( + "ovphysx wheel does not yet expose RIGID_BODY_POSE / RIGID_BODY_VELOCITY", + allow_module_level=True, + ) + +# --------------------------------------------------------------------------- +# Imports (after wheel gate) +# --------------------------------------------------------------------------- +import torch # noqa: E402 +import warp as wp # noqa: E402 +from isaaclab_ovphysx.physics import OvPhysxCfg # noqa: E402 + +# Preload Omni Client while Kit's native libraries are still in a clean loader +# state. Importing it for the first time after an OVPhysX reset can fail with an +# undefined symbol from omni.client's native extension. +import omni.client # noqa: E402,F401 + +import isaaclab.sim as sim_utils # noqa: E402 +import isaaclab.utils.math as math_utils # noqa: E402 +from isaaclab.assets import Articulation, RigidObject, RigidObjectCfg # noqa: E402 +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg # noqa: E402 +from isaaclab.sensors.imu import Imu, ImuCfg # noqa: E402 +from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 +from isaaclab.utils.configclass import configclass # noqa: E402 + +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # noqa: E402 + +wp.init() + +pytestmark = pytest.mark.device_split + +# --------------------------------------------------------------------------- +# Constants +# --------------------------------------------------------------------------- + +NUM_ENVS = 2 +"""Number of environment instances spawned in each test scene.""" + +# offset of imu_link from base_link on anymal_c +POS_OFFSET = (0.2488, 0.00835, 0.04628) +ROT_OFFSET = (0, 0, 0.7071068, 0.7071068) + + +# --------------------------------------------------------------------------- +# Scene-builder helpers (real backend, Nucleus / procedural USD assets) +# --------------------------------------------------------------------------- + + +def _spawn_envs(num_envs: int) -> None: + """Create per-env Xform containers at ``/World/env_``. + + These match the prim-path layout the IMU's attachment-validity test + expects, and provide a parent for per-env asset spawns. + """ + # /World/env_ Xforms are siblings under /World — no envs container needed + for i in range(num_envs): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i * 5.0, 0.0, 0.0)) + + +def _spawn_balls(num_envs: int, height: float = 0.5) -> RigidObject: + """Spawn a sphere rigid body at ``/World/env_/ball`` for each env. + + Returns the :class:`RigidObject` whose binding pattern matches all spawned + instances. The :class:`RigidObject` does the per-env spawning itself when + ``spawn`` is set; we only have to create the env Xform containers first + (handled by :func:`_spawn_envs`). Note the ovphysx pattern uses an + fnmatch glob (``env_*``), not a regex. + """ + spawn_cfg = sim_utils.SphereCfg( + radius=0.25, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ) + cfg = RigidObjectCfg( + prim_path="/World/env_*/ball", + spawn=spawn_cfg, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height)), + ) + return RigidObject(cfg) + + +def _spawn_cubes(num_envs: int, height: float = 0.5) -> RigidObject: + """Spawn a cube rigid body at ``/World/env_/cube`` for each env.""" + spawn_cfg = sim_utils.CuboidCfg( + size=(0.25, 0.25, 0.25), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ) + cfg = RigidObjectCfg( + prim_path="/World/env_*/cube", + spawn=spawn_cfg, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -2.0, height)), + ) + return RigidObject(cfg) + + +def _spawn_anymal(num_envs: int) -> Articulation: + """Spawn the Anymal-C articulation at ``/World/env_/robot`` for each env. + + Uses :data:`~isaaclab_assets.robots.anymal.ANYMAL_C_CFG` directly so the + actuator and init-state configuration matches the PhysX reference test. + The :class:`Articulation` performs the per-env spawn itself once the env + Xform containers exist; :func:`_spawn_envs` must be called first. + """ + cfg = ANYMAL_C_CFG.replace(prim_path="/World/env_.*/robot") + cfg.init_state.pos = (0.0, 2.0, 1.0) + # bump solver iteration counts to match the PhysX test's scene cfg + cfg.spawn.articulation_props.solver_position_iteration_count = 32 + cfg.spawn.articulation_props.solver_velocity_iteration_count = 32 + return Articulation(cfg) + + +def _make_imu(prim_path: str, offset: ImuCfg.OffsetCfg | None = None) -> Imu: + """Create an :class:`Imu` with the given prim path and optional offset.""" + cfg = ImuCfg(prim_path=prim_path) + if offset is not None: + cfg.offset = offset + return Imu(cfg) + + +@configclass +class _StaleResetSceneCfg(InteractiveSceneCfg): + """Minimal scene for the post-reset staleness regression test.""" + + cube = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/cube", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), + spawn=sim_utils.CuboidCfg( + size=(0.25, 0.25, 0.25), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + ) + imu_cube: ImuCfg = ImuCfg(prim_path="{ENV_REGEX_NS}/cube") + + +# --------------------------------------------------------------------------- +# Process-global device-mode lock (matches the rigid-object and contact-sensor +# tests). The ovphysx wheel can only run one device per process; parametrized +# tests skip on the unlocked device so single-device runs finish cleanly. +# --------------------------------------------------------------------------- + +_LOCKED_DEVICE: list[str | None] = [None] + + +@pytest.fixture(autouse=True) +def _ovphysx_skip_other_device(request): + """Skip parametrized tests on the device the session is not pinned to.""" + callspec = getattr(request.node, "callspec", None) + device = callspec.params.get("device") if callspec is not None else None + if device is None: + return + locked = _LOCKED_DEVICE[0] + if locked is None: + _LOCKED_DEVICE[0] = device + return + if device != locked: + pytest.skip( + f"ovphysx process-global device lock is held by '{locked}'; cannot run '{device}' " + "tests in the same session. Run pytest twice (once per device) for full coverage." + ) + + +# --------------------------------------------------------------------------- +# Sim context fixture (real OVPhysX backend, device-parametrized) +# --------------------------------------------------------------------------- + + +@pytest.fixture +def sim_ctx(device: str): + """Build an OVPhysX-backed :class:`SimulationContext` on the requested device. + + Yields: + The simulation context, set up with a small fixed timestep matching + the PhysX reference test (``dt=0.001``) for IMU numerical-differentiation + accuracy. + """ + with build_simulation_context( + sim_cfg=SimulationCfg(physics=OvPhysxCfg(), device=device, dt=0.001), + ) as sim: + yield sim + + +_DEVICES = ["cpu", "cuda:0"] + + +# =========================================================================== +# Constant-velocity / constant-acceleration tests (rigid bodies) +# =========================================================================== + + +@pytest.mark.parametrize("device", _DEVICES) +def test_constant_velocity(sim_ctx, device): + """Test the IMU sensor with a constant velocity. + + Expected behavior is that the linear acceleration is approximately the + same at every time step: in each step we set the same velocity, so the + finite-difference derivative settles to zero (plus the gravity bias). + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + cubes = _spawn_cubes(NUM_ENVS) + imu_ball = _make_imu("/World/env_*/ball") + imu_cube = _make_imu("/World/env_*/cube") + sim_ctx.reset() + + prev_lin_acc_ball = torch.zeros((NUM_ENVS, 3), dtype=torch.float32, device=device) + prev_lin_acc_cube = torch.zeros((NUM_ENVS, 3), dtype=torch.float32, device=device) + + for idx in range(200): + # set velocity + velocity = torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device).repeat( + NUM_ENVS, 1 + ) + balls.write_root_velocity_to_sim(velocity) + cubes.write_root_velocity_to_sim(velocity) + # write data to sim + balls.write_data_to_sim() + cubes.write_data_to_sim() + # perform step + sim_ctx.step() + # read data from sim + dt = sim_ctx.get_physics_dt() + balls.update(dt) + cubes.update(dt) + imu_ball.update(dt, force_recompute=True) + imu_cube.update(dt, force_recompute=True) + + if idx > 1: + # check the imu accelerations + torch.testing.assert_close( + imu_ball.data.lin_acc_b.torch, + prev_lin_acc_ball, + rtol=1e-3, + atol=1e-3, + ) + torch.testing.assert_close( + imu_cube.data.lin_acc_b.torch, + prev_lin_acc_cube, + rtol=1e-3, + atol=1e-3, + ) + + # update previous values + prev_lin_acc_ball = imu_ball.data.lin_acc_b.torch.clone() + prev_lin_acc_cube = imu_cube.data.lin_acc_b.torch.clone() + + +@pytest.mark.parametrize("device", _DEVICES) +def test_constant_acceleration(sim_ctx, device): + """Test the IMU sensor with a constant acceleration.""" + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + imu_ball = _make_imu("/World/env_*/ball") + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + + for idx in range(100): + # set acceleration via increasing velocity per step + velocity = torch.tensor([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device).repeat( + NUM_ENVS, 1 + ) * (idx + 1) + balls.write_root_velocity_to_sim(velocity) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + imu_ball.update(dt, force_recompute=True) + + # skip first step where initial velocity is zero + if idx < 1: + continue + + # check the imu linear acceleration data (includes gravity) + torch.testing.assert_close( + imu_ball.data.lin_acc_b.torch, + math_utils.quat_apply_inverse( + balls.data.root_quat_w.torch, + torch.tensor([[0.1, 0.0, 0.0]], dtype=torch.float32, device=device).repeat(NUM_ENVS, 1) / dt + + torch.tensor([[0.0, 0.0, 9.81]], dtype=torch.float32, device=device).repeat(NUM_ENVS, 1), + ), + rtol=1e-4, + atol=1e-4, + ) + + # check the angular velocity + torch.testing.assert_close( + imu_ball.data.ang_vel_b.torch, + balls.data.root_ang_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + + +# =========================================================================== +# Articulation tests (anymal-C, USD asset from Nucleus) +# =========================================================================== + + +@pytest.mark.parametrize("device", _DEVICES) +def test_offset_calculation(sim_ctx, device): + """Test offset configuration argument. + + Two IMUs on the anymal-C robot — one at ``base`` with a configured offset + matching the location of ``imu_link``, and one directly at ``imu_link`` + — should produce identical readings. + """ + _spawn_envs(NUM_ENVS) + robot = _spawn_anymal(NUM_ENVS) + imu_robot_imu_link = _make_imu("/World/env_*/robot/base/imu_link") + imu_robot_base = _make_imu( + "/World/env_*/robot/base", + offset=ImuCfg.OffsetCfg(pos=POS_OFFSET, rot=ROT_OFFSET), + ) + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + + for idx in range(500): + # apply increasing root velocity + velocity = torch.tensor([[0.05, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device).repeat( + NUM_ENVS, 1 + ) * (idx + 1) + robot.write_root_velocity_to_sim(velocity) + robot.write_data_to_sim() + sim_ctx.step() + robot.update(dt) + imu_robot_imu_link.update(dt, force_recompute=True) + imu_robot_base.update(dt, force_recompute=True) + + # skip first step where initial velocity is zero + if idx < 1: + continue + + torch.testing.assert_close( + imu_robot_base.data.lin_acc_b.torch, + imu_robot_imu_link.data.lin_acc_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + imu_robot_base.data.ang_vel_b.torch, + imu_robot_imu_link.data.ang_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_env_ids_propagation(sim_ctx, device): + """Test that ``env_ids`` argument propagates through update and reset methods.""" + _spawn_envs(NUM_ENVS) + robot = _spawn_anymal(NUM_ENVS) + imu_robot_imu_link = _make_imu("/World/env_*/robot/base/imu_link") + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + + for idx in range(10): + velocity = torch.tensor([[0.5, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device).repeat( + NUM_ENVS, 1 + ) * (idx + 1) + robot.write_root_velocity_to_sim(velocity) + robot.write_data_to_sim() + sim_ctx.step() + robot.update(dt) + imu_robot_imu_link.update(dt, force_recompute=True) + + # reset only env 1 + imu_robot_imu_link.reset(env_ids=[1]) + imu_robot_imu_link.update(dt, force_recompute=True) + sim_ctx.step() + imu_robot_imu_link.update(dt, force_recompute=True) + + +# =========================================================================== +# Physics-correctness sanity tests (ported from Newton) +# =========================================================================== + + +@pytest.mark.parametrize("device", _DEVICES) +def test_sensor_initialization(sim_ctx, device): + """Test that the OVPhysX IMU sensor initializes correctly.""" + _spawn_envs(NUM_ENVS) + _spawn_balls(NUM_ENVS) + imu_ball = _make_imu("/World/env_*/ball") + sim_ctx.reset() + + assert imu_ball.num_instances == NUM_ENVS + # Inspect the raw warp buffers directly — accessing ``imu.data`` triggers a + # lazy FD-acceleration recompute that needs ``_dt`` (set by ``update``). + assert imu_ball._data._ang_vel_b.shape == (NUM_ENVS,) + assert imu_ball._data._lin_acc_b.shape == (NUM_ENVS,) + assert imu_ball._data._ang_vel_b.dtype == wp.vec3f + assert imu_ball._data._lin_acc_b.dtype == wp.vec3f + + +@pytest.mark.parametrize("device", _DEVICES) +def test_gravity_at_rest(sim_ctx, device): + """Test that an IMU at rest measures gravity (~9.81 m/s^2 upward). + + Without InteractiveScene's terrain plumbing the ball falls forever, so we + drive it kinematically: hold zero velocity for enough steps that the + finite-difference acceleration of the *applied* velocity converges to zero + and only the +g gravity bias remains. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + imu_ball = _make_imu("/World/env_*/ball") + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + zero_vel = torch.zeros((NUM_ENVS, 6), dtype=torch.float32, device=device) + for _ in range(5): + balls.write_root_velocity_to_sim(zero_vel) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + imu_ball.update(dt, force_recompute=True) + + lin_acc = imu_ball.data.lin_acc_b.torch + torch.testing.assert_close( + lin_acc[:, 2], + torch.full((NUM_ENVS,), 9.81, dtype=lin_acc.dtype, device=lin_acc.device), + atol=0.5, + rtol=0.0, + ) + torch.testing.assert_close( + lin_acc[:, :2], + torch.zeros(NUM_ENVS, 2, dtype=lin_acc.dtype, device=lin_acc.device), + atol=0.5, + rtol=0.0, + ) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_freefall_acceleration(sim_ctx, device): + """Test that a freefalling IMU measures near-zero proper acceleration. + + In freefall the finite-difference world-frame acceleration (~``-g``) cancels + the IMU's gravity bias (``+g``), so the reading converges to ``[0, 0, 0]``. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS, height=5.0) + imu_ball = _make_imu("/World/env_*/ball") + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + # Let physics integrate gravity for a few steps with no external velocity write. + for _ in range(10): + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + imu_ball.update(dt, force_recompute=True) + + lin_acc = imu_ball.data.lin_acc_b.torch + acc_magnitude = torch.linalg.norm(lin_acc, dim=-1) + torch.testing.assert_close( + acc_magnitude, + torch.zeros_like(acc_magnitude), + atol=0.5, + rtol=0.0, + ) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_reset(sim_ctx, device): + """Test that ``reset`` zeroes out the IMU output and previous-velocity buffers. + + Mirrors the Newton ``test_reset`` parity check: drive the IMU until its + buffers hold non-zero data, then ``reset()`` and assert the raw warp + buffers are zero. We read the raw warp arrays directly because accessing + ``imu.data`` triggers a lazy re-fill that masks reset bugs. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + imu_ball = _make_imu("/World/env_*/ball") + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + nonzero_vel = torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device).repeat(NUM_ENVS, 1) + for _ in range(5): + balls.write_root_velocity_to_sim(nonzero_vel) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + imu_ball.update(dt, force_recompute=True) + + # Buffers should hold non-zero gravity-bias + numerical-diff state before reset. + assert torch.any(wp.to_torch(imu_ball._data._lin_acc_b) != 0), "expected non-zero data before reset" + + imu_ball.reset() + + # Read raw warp buffers directly — ``imu.data`` would trigger a lazy re-fill that + # bypasses the reset. + ang_vel_after = wp.to_torch(imu_ball._data._ang_vel_b) + lin_acc_after = wp.to_torch(imu_ball._data._lin_acc_b) + prev_vel_after = wp.to_torch(imu_ball._prev_lin_vel_w) + 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)) + torch.testing.assert_close(prev_vel_after, torch.zeros_like(prev_vel_after)) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_no_stale_data_after_scene_reset(sim_ctx, device): + """Test ``scene.reset(env_ids)`` does not expose stale native velocity through ``imu.data``.""" + scene_cfg = _StaleResetSceneCfg(num_envs=1, env_spacing=2.0, lazy_sensor_update=False) + scene = InteractiveScene(scene_cfg) + sim_ctx.reset() + scene.reset() + + sensor: Imu = scene["imu_cube"] + + # Drive the native rigid-body velocity buffer non-zero. A freely falling body + # can read zero proper acceleration, so assert the cached velocity instead. + cube: RigidObject = scene["cube"] + nonzero_vel = torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device) + cube.write_root_velocity_to_sim_index(root_velocity=nonzero_vel) + scene.write_data_to_sim() + sim_ctx.step() + scene.update(dt=sim_ctx.get_physics_dt()) + + assert torch.any(wp.to_torch(sensor._prev_lin_vel_w) != 0), "expected non-zero cached velocity before reset" + + # Reset without another physics step. The public accessor must keep reset outputs + # instead of lazy-refetching stale native velocity. + scene.reset(env_ids=torch.tensor([0], device=device)) + + post_reset_lin_acc = sensor.data.lin_acc_b.torch + post_reset_ang_vel = sensor.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)) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_indirect_attachment_usd(sim_ctx, device): + """Test that an IMU attached to a non-physics Xform under a rigid ancestor matches a direct attachment. + + USD-only port of PhysX's ``test_indirect_attachment``: the URDF pendulum is + not available kitless, but the indirect-attachment code path is reachable + by attaching a non-physics Xform child to a rigid ball and pointing the + IMU at it. The composed offset should match the directly-configured + offset; ``ang_vel_b`` and ``lin_acc_b`` should agree. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + # Add a non-physics Xform child under each ball at a known offset; the IMU + # must resolve the rigid-body ancestor (the ball) and recover the offset. + sub_pos = (0.4, 0.0, 0.1) + sub_rot = (0.5, 0.5, 0.5, 0.5) + for i in range(NUM_ENVS): + sim_utils.create_prim(f"/World/env_{i}/ball/imu_sub", "Xform", translation=sub_pos, orientation=sub_rot) + imu_indirect = _make_imu("/World/env_*/ball/imu_sub") + imu_direct = _make_imu("/World/env_*/ball", offset=ImuCfg.OffsetCfg(pos=sub_pos, rot=sub_rot)) + sim_ctx.reset() + + torch.testing.assert_close( + wp.to_torch(imu_indirect._offset_pos_b), + wp.to_torch(imu_direct._offset_pos_b), + ) + torch.testing.assert_close( + wp.to_torch(imu_indirect._offset_quat_b), + wp.to_torch(imu_direct._offset_quat_b), + rtol=1e-4, + atol=1e-4, + ) + + dt = sim_ctx.get_physics_dt() + drive_vel = torch.tensor([[0.05, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=device).repeat(NUM_ENVS, 1) + for idx in range(50): + balls.write_root_velocity_to_sim(drive_vel * (idx + 1)) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + imu_indirect.update(dt, force_recompute=True) + imu_direct.update(dt, force_recompute=True) + + if idx < 2: + continue + + torch.testing.assert_close( + imu_indirect.data.ang_vel_b.torch, + imu_direct.data.ang_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + imu_indirect.data.lin_acc_b.torch, + imu_direct.data.lin_acc_b.torch, + rtol=1e-4, + atol=1e-4, + ) + + +# =========================================================================== +# Validation tests (no asset state required) +# =========================================================================== + + +@pytest.mark.parametrize("device", _DEVICES) +def test_attachment_validity(sim_ctx, device): + """Test invalid IMU attachment. + + An IMU cannot be attached directly to the world Xform — it must have a + rigid-body ancestor in its prim tree. + """ + _spawn_envs(NUM_ENVS) + sim_ctx.reset() + + imu_world_cfg = ImuCfg(prim_path="/World/env_0") + with pytest.raises(RuntimeError) as exc_info: + imu_world = Imu(imu_world_cfg) + imu_world._initialize_impl() + assert exc_info.type is RuntimeError and "find a rigid body ancestor prim" in str(exc_info.value) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_sensor_print(sim_ctx, device): + """Test ``__str__`` is implemented and exposes the prim path and binding pattern.""" + _spawn_envs(NUM_ENVS) + _spawn_balls(NUM_ENVS) + imu_ball = _make_imu("/World/env_*/ball") + sim_ctx.reset() + + s = str(imu_ball) + print(s) + assert "Imu sensor @ '/World/env_*/ball'" in s + assert "binding pattern" in s + assert "/World/env_*/ball" in s + assert "number of sensors : 2" in s + + +# =========================================================================== +# URDF-dependent tests — skipped pending USD pendulum asset +# =========================================================================== + + +@pytest.mark.skip( + reason=( + "Blocked on a USD-converted pendulum asset (the PhysX test loads" + " source/isaaclab_physx/test/sensors/urdfs/simple_2_link.urdf via the Kit URDF importer," + " which is not loaded under the direct ./isaaclab.sh -p runner). Re-enable" + " once a pre-converted USD pendulum is available." + ) +) +def test_single_dof_pendulum(): + """Test imu against analytical pendulum problem.""" + + +@pytest.mark.skip( + reason=( + "Blocked on a USD-converted pendulum asset (the PhysX test loads" + " source/isaaclab_physx/test/sensors/urdfs/simple_2_link.urdf via the Kit URDF importer," + " which is not loaded under the direct ./isaaclab.sh -p runner). Re-enable" + " once a pre-converted USD pendulum is available." + ) +) +def test_indirect_attachment(): + """Test attaching the IMU through an Xform primitive offset chain.""" diff --git a/source/isaaclab_physx/changelog.d/antoiner-feat-ovphysx_imu.skip b/source/isaaclab_physx/changelog.d/antoiner-feat-ovphysx_imu.skip new file mode 100644 index 000000000000..83f984303002 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/antoiner-feat-ovphysx_imu.skip @@ -0,0 +1 @@ +Internal-only: tidied IMU kernel comments and docstrings to match OVPhysX. diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py index 21820ecb5f0b..ce7724fbdf6f 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/imu/imu.py @@ -167,9 +167,9 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): self._offset_pos_b, self._offset_quat_b, self._gravity_bias_w, - self._prev_lin_vel_w, 1.0 / self._dt, self._timestamp, + self._prev_lin_vel_w, self._data._ang_vel_b, self._data._lin_acc_b, ], diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py index f3c0c54d5992..90111579857b 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/imu/kernels.py @@ -8,60 +8,60 @@ @wp.kernel def imu_update_kernel( - # indexing + # inputs env_mask: wp.array(dtype=wp.bool), - # PhysX view data transforms: wp.array(dtype=wp.transformf), velocities: wp.array(dtype=wp.spatial_vectorf), coms: wp.array(dtype=wp.transformf), - # sensor config (per-env) offset_pos_b: wp.array(dtype=wp.vec3f), offset_quat_b: wp.array(dtype=wp.quatf), gravity_bias_w: wp.array(dtype=wp.vec3f), - # previous velocities (read + write) - prev_lin_vel_w: wp.array(dtype=wp.vec3f), - # scalar inv_dt: wp.float32, - # timestamp guard (see #4970): skip envs not stepped since reset. timestamp: wp.array(dtype=wp.float32), - # outputs (written in-place) + # inputs / outputs + prev_lin_vel_w: wp.array(dtype=wp.vec3f), + # outputs out_ang_vel_b: wp.array(dtype=wp.vec3f), out_lin_acc_b: wp.array(dtype=wp.vec3f), ): + """Update the IMU sensor data. + + Args: + env_mask: Mask of environments to update. + transforms: Transforms of the bodies. + velocities: Velocities of the bodies. + coms: COMs of the bodies. + 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. + 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. + """ idx = wp.tid() if not env_mask[idx]: return - # Skip envs that have not been stepped since their last reset: PhysX's velocities still - # hold pre-reset values, so the finite-difference acceleration would be spurious (#4970). + # Skip envs that have not been stepped since their last reset: PhysX velocities still + # hold pre-reset values, so the finite-difference acceleration would be spurious. if timestamp[idx] == 0.0: return - # 1. Extract body orientation body_quat = wp.transform_get_rotation(transforms[idx]) - # 2. Apply sensor offset to get sensor orientation - sensor_quat = body_quat * offset_quat_b[idx] - - # 3. Extract lin/ang velocity lin_vel_w = wp.spatial_top(velocities[idx]) ang_vel_w = wp.spatial_bottom(velocities[idx]) - # 4. COM correction: lin_vel += cross(ang_vel, quat_rotate(body_quat, offset_pos - com_pos)) com_pos_b = wp.transform_get_translation(coms[idx]) lever_arm = wp.quat_rotate(body_quat, offset_pos_b[idx] - com_pos_b) lin_vel_w = lin_vel_w + wp.cross(ang_vel_w, lever_arm) - - # 5. Numerical differentiation for linear acceleration (world frame) lin_acc_w = (lin_vel_w - prev_lin_vel_w[idx]) * inv_dt + gravity_bias_w[idx] - # 6. Rotate world -> body using sensor orientation - ang_vel_b = wp.quat_rotate_inv(sensor_quat, ang_vel_w) - lin_acc_b = wp.quat_rotate_inv(sensor_quat, lin_acc_w) - - # 7. Store results - out_ang_vel_b[idx] = ang_vel_b - out_lin_acc_b[idx] = lin_acc_b + sensor_quat = body_quat * offset_quat_b[idx] + out_ang_vel_b[idx] = wp.quat_rotate_inv(sensor_quat, ang_vel_w) + out_lin_acc_b[idx] = wp.quat_rotate_inv(sensor_quat, lin_acc_w) # Update previous velocity prev_lin_vel_w[idx] = lin_vel_w @@ -74,6 +74,14 @@ def imu_reset_kernel( out_lin_acc_b: wp.array(dtype=wp.vec3f), prev_lin_vel_w: wp.array(dtype=wp.vec3f), ): + """Reset the IMU sensor data. + + Args: + env_mask: Mask of environments to reset. + out_ang_vel_b: Output angular velocity in the body frame. + out_lin_acc_b: Output linear acceleration in the body frame. + prev_lin_vel_w: Previous linear velocity in the world frame. + """ idx = wp.tid() if not env_mask[idx]: return