diff --git a/source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_pva.minor.rst b/source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_pva.minor.rst new file mode 100644 index 000000000000..5600875c9f2f --- /dev/null +++ b/source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_pva.minor.rst @@ -0,0 +1,18 @@ +Added +^^^^^ + +* Added :class:`~isaaclab_ovphysx.sensors.Pva` and + :class:`~isaaclab_ovphysx.sensors.PvaData` implementing the + :class:`~isaaclab.sensors.pva.BasePva` / + :class:`~isaaclab.sensors.pva.BasePvaData` contracts on the OVPhysX + backend. Reports world-frame pose, body-frame linear and angular + velocities, body-frame coordinate linear and angular accelerations, + and projected gravity using ovphysx tensor bindings on the rigid-body + ancestor of the sensor prim path. Linear and angular accelerations + are coordinate accelerations (zero at rest, ``-g`` in freefall) and + do not include the IMU's gravity bias — projected gravity is reported + separately as the unit gravity direction vector. +* Added :meth:`~isaaclab_ovphysx.physics.OvPhysxManager.get_gravity` + classmethod mirroring PhysX's ``SimulationView.get_gravity()`` so + backend-agnostic sensor code can read scene gravity through a single + entry point. diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi index 7d41b088a908..50f4b9c080a6 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi @@ -9,7 +9,10 @@ __all__ = [ "ContactSensorData", "Imu", "ImuData", + "Pva", + "PvaData", ] from .contact_sensor import ContactSensor, ContactSensorCfg, ContactSensorData from .imu import Imu, ImuData +from .pva import Pva, PvaData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/__init__.py new file mode 100644 index 000000000000..5607f62701f0 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/__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 PVA sensor.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/__init__.pyi new file mode 100644 index 000000000000..3ad9f6fff741 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/__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__ = [ + "Pva", + "PvaData", +] + +from .pva import Pva +from .pva_data import PvaData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/kernels.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/kernels.py new file mode 100644 index 000000000000..5245178a5708 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/kernels.py @@ -0,0 +1,132 @@ +# 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 pva_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_vec_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), + prev_ang_vel_w: wp.array(dtype=wp.vec3f), + # outputs + out_pos_w: wp.array(dtype=wp.vec3f), + out_quat_w: wp.array(dtype=wp.quatf), + out_lin_vel_b: wp.array(dtype=wp.vec3f), + out_ang_vel_b: wp.array(dtype=wp.vec3f), + out_lin_acc_b: wp.array(dtype=wp.vec3f), + out_ang_acc_b: wp.array(dtype=wp.vec3f), + out_projected_gravity_b: wp.array(dtype=wp.vec3f), +): + """Update the PVA 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_vec_w: Gravity direction unit vector 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. + prev_ang_vel_w: Previous angular velocity in the world frame. + out_pos_w: Output position in the world frame. + out_quat_w: Output orientation in the world frame. + out_lin_vel_b: Output linear velocity in the body frame. + out_ang_vel_b: Output angular velocity in the body frame. + out_lin_acc_b: Output linear acceleration in the body frame. + out_ang_acc_b: Output angular acceleration in the body frame. + out_projected_gravity_b: Output projected gravity 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 finite-difference acceleration would be spurious. + if timestamp[idx] == 0.0: + return + + body_pos = wp.transform_get_translation(transforms[idx]) + body_quat = wp.transform_get_rotation(transforms[idx]) + + sensor_pos = body_pos + wp.quat_rotate(body_quat, offset_pos_b[idx]) + sensor_quat = body_quat * offset_quat_b[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 + ang_acc_w = (ang_vel_w - prev_ang_vel_w[idx]) * inv_dt + + out_pos_w[idx] = sensor_pos + out_quat_w[idx] = sensor_quat + out_lin_vel_b[idx] = wp.quat_rotate_inv(sensor_quat, lin_vel_w) + 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) + out_ang_acc_b[idx] = wp.quat_rotate_inv(sensor_quat, ang_acc_w) + out_projected_gravity_b[idx] = wp.quat_rotate_inv(sensor_quat, gravity_vec_w[idx]) + + # Update previous velocities. + prev_lin_vel_w[idx] = lin_vel_w + prev_ang_vel_w[idx] = ang_vel_w + + +@wp.kernel +def pva_reset_kernel( + env_mask: wp.array(dtype=wp.bool), + out_pos_w: wp.array(dtype=wp.vec3f), + out_quat_w: wp.array(dtype=wp.quatf), + out_lin_vel_b: wp.array(dtype=wp.vec3f), + out_ang_vel_b: wp.array(dtype=wp.vec3f), + out_lin_acc_b: wp.array(dtype=wp.vec3f), + out_ang_acc_b: wp.array(dtype=wp.vec3f), + out_projected_gravity_b: wp.array(dtype=wp.vec3f), + prev_lin_vel_w: wp.array(dtype=wp.vec3f), + prev_ang_vel_w: wp.array(dtype=wp.vec3f), +): + """Reset the PVA sensor data. + + Args: + env_mask: Mask of environments to reset. + out_pos_w: Output position in the world frame. + out_quat_w: Output orientation in the world frame. + out_lin_vel_b: Output linear velocity in the body frame. + out_ang_vel_b: Output angular velocity in the body frame. + out_lin_acc_b: Output linear acceleration in the body frame. + out_ang_acc_b: Output angular acceleration in the body frame. + out_projected_gravity_b: Output projected gravity in the body frame. + prev_lin_vel_w: Previous linear velocity in the world frame. + prev_ang_vel_w: Previous angular velocity in the world frame. + """ + idx = wp.tid() + if not env_mask[idx]: + return + + out_pos_w[idx] = wp.vec3f(0.0, 0.0, 0.0) + out_quat_w[idx] = wp.quatf(0.0, 0.0, 0.0, 1.0) + out_lin_vel_b[idx] = wp.vec3f(0.0, 0.0, 0.0) + 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) + out_ang_acc_b[idx] = wp.vec3f(0.0, 0.0, 0.0) + out_projected_gravity_b[idx] = wp.vec3f(0.0, 0.0, -1.0) + prev_lin_vel_w[idx] = wp.vec3f(0.0, 0.0, 0.0) + prev_ang_vel_w[idx] = wp.vec3f(0.0, 0.0, 0.0) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/pva.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/pva.py new file mode 100644 index 000000000000..7fb30e71e622 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/pva.py @@ -0,0 +1,308 @@ +# 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 + +from pxr import UsdGeom + +import isaaclab.utils.math as math_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.sensors.pva import BasePva + +import isaaclab_ovphysx.tensor_types as TT +from isaaclab_ovphysx.physics import OvPhysxManager as SimulationManager + +from .kernels import pva_reset_kernel, pva_update_kernel +from .pva_data import PvaData + +if TYPE_CHECKING: + from isaaclab.sensors.pva import PvaCfg + + +class Pva(BasePva): + """The OVPhysX Pose Velocity Acceleration (PVA) sensor. + + The sensor reports world-frame pose, body-frame linear and angular velocities, + body-frame linear and angular accelerations, and projected gravity. Unlike the + :class:`~isaaclab.sensors.imu.BaseImu` sensor, linear acceleration here is the + coordinate acceleration of the sensor frame (zero at rest, ``-g`` in freefall) + and does not include the gravity bias. + + 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 and angular accelerations are computed using numerical differentiation + of the corresponding velocities. Consequently, the PVA sensor accuracy + depends on the chosen physics timestep. For sufficient accuracy, we + recommend keeping the timestep at least 200 Hz. + """ + + cfg: PvaCfg + """The configuration parameters.""" + + __backend_name__: str = "ovphysx" + """The name of the backend for the PVA sensor.""" + + def __init__(self, cfg: PvaCfg): + """Initializes the PVA sensor. + + Args: + cfg: The configuration parameters. + """ + super().__init__(cfg) + self._data = PvaData() + self._rigid_parent_expr: str | None = None + # Sentinel — set in :meth:`_initialize_impl`; ``None`` means the sensor has not been bound yet + # (used by :meth:`_debug_vis_callback` to safely no-op before init). + self._pose_binding = None + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Pva 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) -> PvaData: + 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( + pva_reset_kernel, + dim=self._num_envs, + inputs=[ + env_mask, + self._data._pos_w, + self._data._quat_w, + self._data._lin_vel_b, + self._data._ang_vel_b, + self._data._lin_acc_b, + self._data._ang_acc_b, + self._data._projected_gravity_b, + self._prev_lin_vel_w, + self._prev_ang_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 Pva: 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." + ) + + # PVA reports projected gravity as the unit direction vector (not the bias the IMU uses). + gravity = SimulationManager.get_gravity() + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self._device) + gravity_dir = math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0) + gravity_dir_repeated = gravity_dir.repeat(self._num_bodies, 1) + self._gravity_vec_w = wp.from_torch(gravity_dir_repeated.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( + pva_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_vec_w, + 1.0 / self._dt, + self._timestamp, + self._prev_lin_vel_w, + self._prev_ang_vel_w, + self._data._pos_w, + self._data._quat_w, + self._data._lin_vel_b, + self._data._ang_vel_b, + self._data._lin_acc_b, + self._data._ang_acc_b, + self._data._projected_gravity_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) + + # Sensor-internal buffers for velocity tracking (not exposed via data). + self._prev_lin_vel_w = wp.zeros(self._num_bodies, dtype=wp.vec3f, device=self._device) + self._prev_ang_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) + + def _set_debug_vis_impl(self, debug_vis: bool): + if debug_vis: + if not hasattr(self, "acceleration_visualizer"): + self.acceleration_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + self.acceleration_visualizer.set_visibility(True) + else: + if hasattr(self, "acceleration_visualizer"): + self.acceleration_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # safely return if the sensor has not been bound yet (matches the PhysX `_view is None` idiom) + if self._pose_binding is None: + return + # get marker location + # -- base state (convert warp -> torch for visualization) + base_pos_w = self._data.pos_w.torch.clone() + base_pos_w[:, 2] += 0.5 + # -- resolve the scales + default_scale = self.acceleration_visualizer.cfg.markers["arrow"].scale + arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.torch.shape[0], 1) + # get up axis of current stage + up_axis = UsdGeom.GetStageUpAxis(self.stage) + # arrow-direction; filter out bodies with effectively zero accel (no defined direction) + pos_w_torch = self._data.pos_w.torch + accel_w = math_utils.quat_apply(self._data.quat_w.torch, self._data.lin_acc_b.torch) + valid_indices = (torch.linalg.norm(accel_w, dim=-1) > 1e-5).nonzero(as_tuple=True)[0] + if valid_indices.numel() == 0: + return + pos_filtered = pos_w_torch.index_select(0, valid_indices) + accel_filtered = accel_w.index_select(0, valid_indices) + rotation_matrix = math_utils.create_rotation_matrix_from_view( + pos_filtered, + pos_filtered + accel_filtered, + up_axis=up_axis, + device=self._device, + ) + quat_opengl = math_utils.quat_from_matrix(rotation_matrix) + quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world") + # display markers + self.acceleration_visualizer.visualize( + base_pos_w.index_select(0, valid_indices), + quat_w, + arrow_scale.index_select(0, valid_indices), + ) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/pva_data.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/pva_data.py new file mode 100644 index 000000000000..40f4881016ca --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/pva/pva_data.py @@ -0,0 +1,143 @@ +# 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.pva import BasePvaData +from isaaclab.utils.warp import ProxyArray + +from isaaclab_ovphysx.sensors.kernels import concat_pos_and_quat_to_pose_1d_kernel + + +class PvaData(BasePvaData): + """Data container for the OVPhysX PVA sensor.""" + + @property + def pose_w(self) -> ProxyArray: + """Pose of the sensor origin in world frame [m, unitless]. + + Shape is (num_instances,), dtype = wp.transformf. In torch this resolves to (num_instances, 7). + The pose is provided in (x, y, z, qx, qy, qz, qw) format. + """ + wp.launch( + concat_pos_and_quat_to_pose_1d_kernel, + dim=self._num_envs, + inputs=[self._pos_w, self._quat_w], + outputs=[self._pose_w], + device=self._device, + ) + if self._pose_w_ta is None: + self._pose_w_ta = ProxyArray(self._pose_w) + return self._pose_w_ta + + @property + def pos_w(self) -> ProxyArray: + """Position of the sensor origin in world frame [m]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._pos_w_ta is None: + self._pos_w_ta = ProxyArray(self._pos_w) + return self._pos_w_ta + + @property + def quat_w(self) -> ProxyArray: + """Orientation of the sensor origin in world frame. + + Shape is (num_instances,), dtype = wp.quatf. In torch this resolves to (num_instances, 4). + The orientation is provided in (x, y, z, w) format. + """ + if self._quat_w_ta is None: + self._quat_w_ta = ProxyArray(self._quat_w) + return self._quat_w_ta + + @property + def projected_gravity_b(self) -> ProxyArray: + """Gravity direction unit vector projected on the PVA frame [unitless]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._projected_gravity_b_ta is None: + self._projected_gravity_b_ta = ProxyArray(self._projected_gravity_b) + return self._projected_gravity_b_ta + + @property + def lin_vel_b(self) -> ProxyArray: + """PVA frame linear velocity relative to the world expressed in PVA frame [m/s]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._lin_vel_b_ta is None: + self._lin_vel_b_ta = ProxyArray(self._lin_vel_b) + return self._lin_vel_b_ta + + @property + def ang_vel_b(self) -> ProxyArray: + """PVA frame angular velocity relative to the world expressed in PVA 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 (coordinate) in the PVA frame [m/s^2]. + + Equal to ``-g`` in freefall, zero 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 + + @property + def ang_acc_b(self) -> ProxyArray: + """PVA frame angular acceleration relative to the world expressed in PVA frame [rad/s^2]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._ang_acc_b_ta is None: + self._ang_acc_b_ta = ProxyArray(self._ang_acc_b) + return self._ang_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._pose_w = wp.zeros(num_envs, dtype=wp.transformf, device=device) + self._pos_w = wp.zeros(num_envs, dtype=wp.vec3f, device=device) + self._quat_w = wp.zeros(num_envs, dtype=wp.quatf, device=device) + # Initialize quaternion to identity (w=1): warp quatf is (x,y,z,w) + # Use torch interop to set the w component + quat_torch = wp.to_torch(self._quat_w) + quat_torch[:, 3] = 1.0 + self._projected_gravity_b = wp.zeros(num_envs, dtype=wp.vec3f, device=device) + # Initialize projected gravity to (0, 0, -1) + pg_torch = wp.to_torch(self._projected_gravity_b) + pg_torch[:, 2] = -1.0 + self._lin_vel_b = wp.zeros(num_envs, dtype=wp.vec3f, 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) + self._ang_acc_b = wp.zeros(num_envs, dtype=wp.vec3f, device=device) + + # -- Pinned ProxyArray cache (one per read property, lazily created on first access) + self._pose_w_ta: ProxyArray | None = None + self._pos_w_ta: ProxyArray | None = None + self._quat_w_ta: ProxyArray | None = None + self._projected_gravity_b_ta: ProxyArray | None = None + self._lin_vel_b_ta: ProxyArray | None = None + self._ang_vel_b_ta: ProxyArray | None = None + self._lin_acc_b_ta: ProxyArray | None = None + self._ang_acc_b_ta: ProxyArray | None = None diff --git a/source/isaaclab_ovphysx/test/sensors/check_pva.py b/source/isaaclab_ovphysx/test/sensors/check_pva.py new file mode 100644 index 000000000000..3ffbebfe1855 --- /dev/null +++ b/source/isaaclab_ovphysx/test/sensors/check_pva.py @@ -0,0 +1,92 @@ +# 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 PVA sensor. + +Run with: + ./isaaclab.sh -p source/isaaclab_ovphysx/test/sensors/check_pva.py + +Spawns a free-falling sphere with a PVA sensor attached, steps the simulation +for 100 steps, and prints the world-frame position, body-frame linear/angular +velocities, body-frame linear/angular accelerations, and projected gravity. +In freefall the coordinate linear acceleration is ``-g`` rotated into the +body frame, so its magnitude should converge to ``~9.81 m/s^2``. The +projected gravity unit vector should remain ``≈ (0, 0, -1)`` for a body that +stays upright. + +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.pva import Pva, PvaCfg +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)), + ) + ) + pva = Pva(PvaCfg(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) + pva.update(dt, force_recompute=True) + + if step in (0, 10, 50, 99): + data = pva.data + pos = data.pos_w.torch + lin_v = data.lin_vel_b.torch + ang_v = data.ang_vel_b.torch + lin_a = data.lin_acc_b.torch + ang_a = data.ang_acc_b.torch + pg = data.projected_gravity_b.torch + print( + f"step={step:3d} pos_w={pos[0].cpu().numpy()} lin_vel_b={lin_v[0].cpu().numpy()} " + f"ang_vel_b={ang_v[0].cpu().numpy()} lin_acc_b={lin_a[0].cpu().numpy()} " + f"ang_acc_b={ang_a[0].cpu().numpy()} proj_g={pg[0].cpu().numpy()}" + ) + + # Final sanity check: in freefall the coordinate acceleration magnitude is ~g. + final_lin = pva.data.lin_acc_b.torch + magnitude = torch.linalg.norm(final_lin, dim=1).cpu().numpy() + print(f"final |lin_acc_b| per env = {magnitude} (expected ~9.81)") + # projected gravity unit vector magnitude is 1. + final_pg = pva.data.projected_gravity_b.torch + pg_magnitude = torch.linalg.norm(final_pg, dim=1).cpu().numpy() + print(f"final |projected_gravity_b| per env = {pg_magnitude} (expected ~1.0)") + + +if __name__ == "__main__": + main() diff --git a/source/isaaclab_ovphysx/test/sensors/test_pva.py b/source/isaaclab_ovphysx/test/sensors/test_pva.py new file mode 100644 index 000000000000..8adfc8b8e0c2 --- /dev/null +++ b/source/isaaclab_ovphysx/test/sensors/test_pva.py @@ -0,0 +1,867 @@ +# 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 PVA sensor. + +Mirrors the structure of source/isaaclab_physx/test/sensors/test_pva.py +but runs kitless under ./isaaclab.sh -p -m pytest — no AppLauncher needed. +SimulationContext is instantiated directly (it does not require Kit). + +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. + +All tests use procedural USD assets so the kitless suite does not depend on +Nucleus or ``omni.client`` loader state. +""" + +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 + +import isaaclab.sim as sim_utils # noqa: E402 +import isaaclab.utils.math as math_utils # noqa: E402 +from isaaclab.assets import RigidObject, RigidObjectCfg # noqa: E402 +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg # noqa: E402 +from isaaclab.sensors.pva import Pva, PvaCfg # noqa: E402 +from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 +from isaaclab.utils.configclass import configclass # noqa: E402 + +wp.init() + +pytestmark = pytest.mark.device_split + +# --------------------------------------------------------------------------- +# Constants +# --------------------------------------------------------------------------- + +NUM_ENVS = 2 +"""Number of environment instances spawned in each test scene.""" + +MOUNT_POS_OFFSET = (0.4, 0.0, 0.1) +"""Known position offset from the rigid cube to the procedural PVA child frame [m].""" + +MOUNT_ROT_OFFSET = (0.5, 0.5, 0.5, 0.5) +"""Known rotation offset from the rigid cube to the procedural PVA child frame.""" + + +# --------------------------------------------------------------------------- +# Scene-builder helpers (real backend, 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 PVA'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 _add_pva_mount_xforms(num_envs: int, parent_name: str = "cube", child_name: str = "pva_mount") -> None: + """Add a procedural non-physics child Xform under each rigid body.""" + for i in range(num_envs): + sim_utils.create_prim( + f"/World/env_{i}/{parent_name}/{child_name}", + "Xform", + translation=MOUNT_POS_OFFSET, + orientation=MOUNT_ROT_OFFSET, + ) + + +def _make_pva(prim_path: str, offset: PvaCfg.OffsetCfg | None = None) -> Pva: + """Create a :class:`Pva` with the given prim path and optional offset.""" + cfg = PvaCfg(prim_path=prim_path) + if offset is not None: + cfg.offset = offset + return Pva(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(), + ), + ) + pva_cube: PvaCfg = PvaCfg(prim_path="{ENV_REGEX_NS}/cube") + + +# --------------------------------------------------------------------------- +# Process-global device-mode lock (matches the IMU 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 PVA 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 PVA sensor with a constant velocity. + + Expected behavior is that the linear and angular accelerations are + approximately the same at every time step: in each step we set the same + velocity, so the finite-difference derivative settles to zero. The PVA + linear acceleration is the coordinate acceleration and does not include + gravity (the IMU's gravity bias is absent here). + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + cubes = _spawn_cubes(NUM_ENVS) + pva_ball = _make_pva("/World/env_*/ball") + pva_cube = _make_pva("/World/env_*/cube") + sim_ctx.reset() + + prev_lin_acc_ball = torch.zeros((NUM_ENVS, 3), dtype=torch.float32, device=device) + prev_ang_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) + prev_ang_acc_cube = torch.zeros((NUM_ENVS, 3), dtype=torch.float32, device=device) + + dt = sim_ctx.get_physics_dt() + + 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_index(root_velocity=velocity) + cubes.write_root_velocity_to_sim_index(root_velocity=velocity) + # write data to sim + balls.write_data_to_sim() + cubes.write_data_to_sim() + # perform step + sim_ctx.step() + # read data from sim + balls.update(dt) + cubes.update(dt) + pva_ball.update(dt, force_recompute=True) + pva_cube.update(dt, force_recompute=True) + + if idx > 1: + # check the pva accelerations + torch.testing.assert_close( + pva_ball.data.lin_acc_b.torch, + prev_lin_acc_ball, + rtol=1e-3, + atol=1e-3, + ) + torch.testing.assert_close( + pva_ball.data.ang_acc_b.torch, + prev_ang_acc_ball, + rtol=1e-3, + atol=1e-3, + ) + torch.testing.assert_close( + pva_cube.data.lin_acc_b.torch, + prev_lin_acc_cube, + rtol=1e-3, + atol=1e-3, + ) + torch.testing.assert_close( + pva_cube.data.ang_acc_b.torch, + prev_ang_acc_cube, + rtol=1e-3, + atol=1e-3, + ) + + # check the pva velocities: write_root_velocity_to_sim sets v_0 every step, + # the solver then integrates one step under gravity → v_i = v_0 + g*dt. + expected_vel = torch.tensor([[1.0, 0.0, -dt * 9.81]], dtype=torch.float32, device=device).repeat( + NUM_ENVS, 1 + ) + torch.testing.assert_close( + pva_ball.data.lin_vel_b.torch, + expected_vel, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_cube.data.lin_vel_b.torch, + expected_vel, + rtol=1e-4, + atol=1e-4, + ) + + # update previous values + prev_lin_acc_ball = pva_ball.data.lin_acc_b.torch.clone() + prev_ang_acc_ball = pva_ball.data.ang_acc_b.torch.clone() + prev_lin_acc_cube = pva_cube.data.lin_acc_b.torch.clone() + prev_ang_acc_cube = pva_cube.data.ang_acc_b.torch.clone() + + +@pytest.mark.parametrize("device", _DEVICES) +def test_constant_acceleration(sim_ctx, device): + """Test the PVA sensor with a constant acceleration. + + The PVA linear acceleration is a coordinate acceleration (no gravity bias), + so it should equal the imposed dv/dt rotated into the body frame. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + pva_ball = _make_pva("/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_index(root_velocity=velocity) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + pva_ball.update(dt, force_recompute=True) + + # skip first step where initial velocity is zero + if idx < 1: + continue + + # PVA linear acceleration (coordinate) — no gravity bias. + torch.testing.assert_close( + pva_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, + ), + rtol=1e-4, + atol=1e-4, + ) + + # check the angular velocity + torch.testing.assert_close( + pva_ball.data.ang_vel_b.torch, + balls.data.root_ang_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + + +# =========================================================================== +# Offset and env-id tests (procedural USD assets) +# =========================================================================== + + +@pytest.mark.parametrize("device", _DEVICES) +def test_offset_calculation(sim_ctx, device): + """Test offset configuration argument. + + Two PVA sensors on the same cube -- one attached to a non-physics child + Xform and one attached to the cube with the same configured offset -- should + produce identical readings across all outputs. + """ + _spawn_envs(NUM_ENVS) + cubes = _spawn_cubes(NUM_ENVS) + _add_pva_mount_xforms(NUM_ENVS) + pva_child = _make_pva("/World/env_*/cube/pva_mount") + pva_direct = _make_pva( + "/World/env_*/cube", + offset=PvaCfg.OffsetCfg(pos=MOUNT_POS_OFFSET, rot=MOUNT_ROT_OFFSET), + ) + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + + for idx in range(50): + 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) + cubes.write_root_velocity_to_sim_index(root_velocity=velocity) + cubes.write_data_to_sim() + sim_ctx.step() + cubes.update(dt) + pva_child.update(dt, force_recompute=True) + pva_direct.update(dt, force_recompute=True) + + # skip first step where initial velocity is zero + if idx < 1: + continue + + # accelerations + torch.testing.assert_close( + pva_direct.data.lin_acc_b.torch, + pva_child.data.lin_acc_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_direct.data.ang_acc_b.torch, + pva_child.data.ang_acc_b.torch, + rtol=1e-4, + atol=1e-4, + ) + # velocities + torch.testing.assert_close( + pva_direct.data.lin_vel_b.torch, + pva_child.data.lin_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_direct.data.ang_vel_b.torch, + pva_child.data.ang_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + # pose + torch.testing.assert_close( + pva_direct.data.pos_w.torch, + pva_child.data.pos_w.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_direct.data.quat_w.torch, + pva_child.data.quat_w.torch, + rtol=1e-4, + atol=1e-4, + ) + # projected gravity + torch.testing.assert_close( + pva_direct.data.projected_gravity_b.torch, + pva_child.data.projected_gravity_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) + cubes = _spawn_cubes(NUM_ENVS) + pva_cube = _make_pva("/World/env_*/cube") + 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) + cubes.write_root_velocity_to_sim_index(root_velocity=velocity) + cubes.write_data_to_sim() + sim_ctx.step() + cubes.update(dt) + pva_cube.update(dt, force_recompute=True) + + assert torch.any(pva_cube.data.lin_vel_b.torch[1] != 0), "expected env 1 to have non-zero data before reset" + + # reset only env 1 + pva_cube.reset(env_ids=[1]) + torch.testing.assert_close( + wp.to_torch(pva_cube._data._lin_vel_b)[1], + torch.zeros(3, dtype=torch.float32, device=device), + ) + assert torch.any(wp.to_torch(pva_cube._data._lin_vel_b)[0] != 0), "env 0 should not be reset" + + pva_cube.update(dt, force_recompute=True) + sim_ctx.step() + pva_cube.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 PVA sensor initializes correctly.""" + _spawn_envs(NUM_ENVS) + _spawn_balls(NUM_ENVS) + pva_ball = _make_pva("/World/env_*/ball") + sim_ctx.reset() + + assert pva_ball.num_instances == NUM_ENVS + # Inspect the raw warp buffers directly — accessing ``pva.data`` triggers a + # lazy FD-acceleration recompute that needs ``_dt`` (set by ``update``). + for name, expected_dtype in [ + ("_pose_w", wp.transformf), + ("_pos_w", wp.vec3f), + ("_quat_w", wp.quatf), + ("_lin_vel_b", wp.vec3f), + ("_ang_vel_b", wp.vec3f), + ("_lin_acc_b", wp.vec3f), + ("_ang_acc_b", wp.vec3f), + ("_projected_gravity_b", wp.vec3f), + ]: + buf = getattr(pva_ball._data, name) + assert buf.shape == (NUM_ENVS,), f"{name} shape mismatch" + assert buf.dtype == expected_dtype, f"{name} dtype mismatch" + + +@pytest.mark.parametrize("device", _DEVICES) +def test_pose_w_packing(sim_ctx, device): + """Verify the lazy ``pose_w`` property packs ``[pos_w | quat_w]`` correctly. + + Regression guard for the ``concat_pos_and_quat_to_pose_1d_kernel`` launch in + :meth:`PvaData.pose_w` — without this test only buffer shape/dtype is + checked, and a regression that, say, swaps the kernel inputs or returns + stale data would not be caught. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + pva_ball = _make_pva("/World/env_*/ball") + sim_ctx.reset() + + dt = sim_ctx.get_physics_dt() + # Step a few times so pos_w / quat_w are populated by the simulation. + for _ in range(3): + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + pva_ball.update(dt, force_recompute=True) + + pose = pva_ball.data.pose_w.torch + assert pose.shape == (NUM_ENVS, 7) + torch.testing.assert_close(pose[:, :3], pva_ball.data.pos_w.torch) + torch.testing.assert_close(pose[:, 3:], pva_ball.data.quat_w.torch) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_projected_gravity_at_rest(sim_ctx, device): + """Test that a PVA at rest reports projected gravity ≈ (0, 0, -1) in body frame. + + Without InteractiveScene's terrain plumbing the ball falls forever, so we + drive it kinematically: hold zero velocity for enough steps to settle, then + check that the projected gravity unit vector points along world ``-z`` — + which for a body whose orientation is identity is also ``(0, 0, -1)`` in + the body frame. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + pva_ball = _make_pva("/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_index(root_velocity=zero_vel) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + pva_ball.update(dt, force_recompute=True) + + pg = pva_ball.data.projected_gravity_b.torch + expected = torch.tensor([[0.0, 0.0, -1.0]], dtype=pg.dtype, device=pg.device).repeat(NUM_ENVS, 1) + torch.testing.assert_close(pg, expected, atol=1e-3, rtol=0.0) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_freefall_lin_acc(sim_ctx, device): + """Test that a freefalling PVA reports lin_acc_b ≈ rotated ``-g`` in body frame. + + The PVA reports the coordinate acceleration of the sensor frame. In + freefall this is ``-g`` (the body is accelerating downward at ``g``), so + the magnitude of ``lin_acc_b`` should converge to ``g ≈ 9.81 m/s^2``. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS, height=5.0) + pva_ball = _make_pva("/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) + pva_ball.update(dt, force_recompute=True) + + lin_acc = pva_ball.data.lin_acc_b.torch + acc_magnitude = torch.linalg.norm(lin_acc, dim=-1) + torch.testing.assert_close( + acc_magnitude, + torch.full((NUM_ENVS,), 9.81, dtype=acc_magnitude.dtype, device=acc_magnitude.device), + atol=0.5, + rtol=0.0, + ) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_reset(sim_ctx, device): + """Test that ``reset`` zeroes out the PVA output and previous-velocity buffers. + + Mirrors the Newton ``test_reset`` parity check: drive the PVA 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 + ``pva.data`` triggers a lazy re-fill that masks reset bugs. + """ + _spawn_envs(NUM_ENVS) + balls = _spawn_balls(NUM_ENVS) + pva_ball = _make_pva("/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_index(root_velocity=nonzero_vel) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + pva_ball.update(dt, force_recompute=True) + + # Buffers should hold non-zero state before reset. + assert torch.any(wp.to_torch(pva_ball._data._lin_vel_b) != 0), "expected non-zero data before reset" + + pva_ball.reset() + + # Read raw warp buffers directly — ``pva.data`` would trigger a lazy re-fill that + # bypasses the reset. + for name in ( + "_pos_w", + "_lin_vel_b", + "_ang_vel_b", + "_lin_acc_b", + "_ang_acc_b", + ): + buf = wp.to_torch(getattr(pva_ball._data, name)) + torch.testing.assert_close(buf, torch.zeros_like(buf), msg=f"{name} not zeroed by reset()") + + # quat_w resets to identity (x, y, z, w) = (0, 0, 0, 1) + quat = wp.to_torch(pva_ball._data._quat_w) + expected_quat = torch.zeros_like(quat) + expected_quat[:, 3] = 1.0 + torch.testing.assert_close(quat, expected_quat) + + # projected_gravity_b resets to (0, 0, -1) + pg = wp.to_torch(pva_ball._data._projected_gravity_b) + expected_pg = torch.zeros_like(pg) + expected_pg[:, 2] = -1.0 + torch.testing.assert_close(pg, expected_pg) + + # previous-velocity buffers cleared + torch.testing.assert_close( + wp.to_torch(pva_ball._prev_lin_vel_w), torch.zeros_like(wp.to_torch(pva_ball._prev_lin_vel_w)) + ) + torch.testing.assert_close( + wp.to_torch(pva_ball._prev_ang_vel_w), torch.zeros_like(wp.to_torch(pva_ball._prev_ang_vel_w)) + ) + + +@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 ``pva.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: Pva = scene["pva_cube"] + + # Drive the native rigid-body velocity buffer non-zero. Freefall can make + # acceleration assertions depend on the exact step, so assert the cached + # finite-difference state 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_acc = sensor.data.ang_acc_b.torch + post_reset_lin_vel = sensor.data.lin_vel_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_acc, torch.zeros_like(post_reset_ang_acc)) + torch.testing.assert_close(post_reset_lin_vel, torch.zeros_like(post_reset_lin_vel)) + 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 a PVA 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 + PVA at it. The composed offset should match the directly-configured + offset; all output channels 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 PVA + # 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/pva_sub", "Xform", translation=sub_pos, orientation=sub_rot) + pva_indirect = _make_pva("/World/env_*/ball/pva_sub") + pva_direct = _make_pva("/World/env_*/ball", offset=PvaCfg.OffsetCfg(pos=sub_pos, rot=sub_rot)) + sim_ctx.reset() + + torch.testing.assert_close( + wp.to_torch(pva_indirect._offset_pos_b), + wp.to_torch(pva_direct._offset_pos_b), + ) + torch.testing.assert_close( + wp.to_torch(pva_indirect._offset_quat_b), + wp.to_torch(pva_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_index(root_velocity=drive_vel * (idx + 1)) + balls.write_data_to_sim() + sim_ctx.step() + balls.update(dt) + pva_indirect.update(dt, force_recompute=True) + pva_direct.update(dt, force_recompute=True) + + if idx < 2: + continue + + torch.testing.assert_close( + pva_indirect.data.pos_w.torch, + pva_direct.data.pos_w.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_indirect.data.quat_w.torch, + pva_direct.data.quat_w.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_indirect.data.lin_vel_b.torch, + pva_direct.data.lin_vel_b.torch, + rtol=1e-2, + atol=5e-3, + ) + torch.testing.assert_close( + pva_indirect.data.ang_vel_b.torch, + pva_direct.data.ang_vel_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_indirect.data.lin_acc_b.torch, + pva_direct.data.lin_acc_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_indirect.data.ang_acc_b.torch, + pva_direct.data.ang_acc_b.torch, + rtol=1e-4, + atol=1e-4, + ) + torch.testing.assert_close( + pva_indirect.data.projected_gravity_b.torch, + pva_direct.data.projected_gravity_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 PVA attachment. + + A PVA sensor 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() + + pva_world_cfg = PvaCfg(prim_path="/World/env_0") + with pytest.raises(RuntimeError) as exc_info: + pva_world = Pva(pva_world_cfg) + pva_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) + pva_ball = _make_pva("/World/env_*/ball") + sim_ctx.reset() + + s = str(pva_ball) + print(s) + assert "Pva 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 +# =========================================================================== + + +_PENDULUM_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, and port the assertion body from" + " source/isaaclab_physx/test/sensors/test_pva.py." +) + + +@pytest.mark.skip(reason=_PENDULUM_SKIP_REASON) +def test_single_dof_pendulum(): + """Test PVA against analytical pendulum problem.""" + # If this test is ever un-skipped without porting the PhysX assertions, fail + # explicitly rather than passing vacuously. + pytest.fail( + "test_single_dof_pendulum was un-skipped without a body — port the assertions from" + " source/isaaclab_physx/test/sensors/test_pva.py::test_single_dof_pendulum and adapt" + " them to the kitless RigidObject/Articulation pattern used by the rest of this file." + ) + + +@pytest.mark.skip(reason=_PENDULUM_SKIP_REASON) +def test_indirect_attachment(): + """Test attaching the PVA through an Xform primitive offset chain.""" + # If this test is ever un-skipped without porting the PhysX assertions, fail + # explicitly rather than passing vacuously. + pytest.fail( + "test_indirect_attachment was un-skipped without a body — port the assertions from" + " source/isaaclab_physx/test/sensors/test_pva.py::test_indirect_attachment and adapt" + " them to the kitless RigidObject/Articulation pattern used by the rest of this file." + ) diff --git a/source/isaaclab_physx/changelog.d/antoiner-feat-ovphysx_pva.skip b/source/isaaclab_physx/changelog.d/antoiner-feat-ovphysx_pva.skip new file mode 100644 index 000000000000..e3f7163ac20d --- /dev/null +++ b/source/isaaclab_physx/changelog.d/antoiner-feat-ovphysx_pva.skip @@ -0,0 +1 @@ +Internal-only: tidied PVA kernel comments and docstrings to match OVPhysX. diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py index c595997e6d8c..f08b7f671fbf 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/kernels.py @@ -8,24 +8,20 @@ @wp.kernel def pva_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_vec_w: wp.array(dtype=wp.vec3f), - # previous velocities (read + write) - prev_lin_vel_w: wp.array(dtype=wp.vec3f), - prev_ang_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), + prev_ang_vel_w: wp.array(dtype=wp.vec3f), + # outputs out_pos_w: wp.array(dtype=wp.vec3f), out_quat_w: wp.array(dtype=wp.quatf), out_lin_vel_b: wp.array(dtype=wp.vec3f), @@ -34,53 +30,62 @@ def pva_update_kernel( out_ang_acc_b: wp.array(dtype=wp.vec3f), out_projected_gravity_b: wp.array(dtype=wp.vec3f), ): + """Update the PVA 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_vec_w: Gravity direction unit vector 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. + prev_ang_vel_w: Previous angular velocity in the world frame. + out_pos_w: Output position in the world frame. + out_quat_w: Output orientation in the world frame. + out_lin_vel_b: Output linear velocity in the body frame. + out_ang_vel_b: Output angular velocity in the body frame. + out_lin_acc_b: Output linear acceleration in the body frame. + out_ang_acc_b: Output angular acceleration in the body frame. + out_projected_gravity_b: Output projected gravity 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 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 finite-difference acceleration would be spurious. if timestamp[idx] == 0.0: return - # 1. Extract body pose body_pos = wp.transform_get_translation(transforms[idx]) body_quat = wp.transform_get_rotation(transforms[idx]) - # 2. Apply sensor offset sensor_pos = body_pos + wp.quat_rotate(body_quat, offset_pos_b[idx]) 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 (world frame) lin_acc_w = (lin_vel_w - prev_lin_vel_w[idx]) * inv_dt ang_acc_w = (ang_vel_w - prev_ang_vel_w[idx]) * inv_dt - # 6. Rotate world -> body using sensor orientation - lin_vel_b = wp.quat_rotate_inv(sensor_quat, lin_vel_w) - ang_vel_b = wp.quat_rotate_inv(sensor_quat, ang_vel_w) - lin_acc_b = wp.quat_rotate_inv(sensor_quat, lin_acc_w) - ang_acc_b = wp.quat_rotate_inv(sensor_quat, ang_acc_w) - projected_gravity_b = wp.quat_rotate_inv(sensor_quat, gravity_vec_w[idx]) - - # 7. Store results out_pos_w[idx] = sensor_pos out_quat_w[idx] = sensor_quat - out_lin_vel_b[idx] = lin_vel_b - out_ang_vel_b[idx] = ang_vel_b - out_lin_acc_b[idx] = lin_acc_b - out_ang_acc_b[idx] = ang_acc_b - out_projected_gravity_b[idx] = projected_gravity_b + out_lin_vel_b[idx] = wp.quat_rotate_inv(sensor_quat, lin_vel_w) + 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) + out_ang_acc_b[idx] = wp.quat_rotate_inv(sensor_quat, ang_acc_w) + out_projected_gravity_b[idx] = wp.quat_rotate_inv(sensor_quat, gravity_vec_w[idx]) - # Update previous velocities + # Update previous velocities. prev_lin_vel_w[idx] = lin_vel_w prev_ang_vel_w[idx] = ang_vel_w @@ -98,6 +103,20 @@ def pva_reset_kernel( prev_lin_vel_w: wp.array(dtype=wp.vec3f), prev_ang_vel_w: wp.array(dtype=wp.vec3f), ): + """Reset the PVA sensor data. + + Args: + env_mask: Mask of environments to reset. + out_pos_w: Output position in the world frame. + out_quat_w: Output orientation in the world frame. + out_lin_vel_b: Output linear velocity in the body frame. + out_ang_vel_b: Output angular velocity in the body frame. + out_lin_acc_b: Output linear acceleration in the body frame. + out_ang_acc_b: Output angular acceleration in the body frame. + out_projected_gravity_b: Output projected gravity in the body frame. + prev_lin_vel_w: Previous linear velocity in the world frame. + prev_ang_vel_w: Previous angular velocity in the world frame. + """ idx = wp.tid() if not env_mask[idx]: return diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py index 23f840cd4e12..00b5a65883ae 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py @@ -202,10 +202,10 @@ def _update_buffers_impl(self, env_mask: wp.array | None = None): self._offset_pos_b, self._offset_quat_b, self.GRAVITY_VEC_W, - self._prev_lin_vel_w, - self._prev_ang_vel_w, 1.0 / self._dt, self._timestamp, + self._prev_lin_vel_w, + self._prev_ang_vel_w, self._data._pos_w, self._data._quat_w, self._data._lin_vel_b,