[OVPHYSX] FrameTransformer sensor

source/isaaclab/changelog.d/antoiner-feat-ovphysx-frametransformer.rst

@@ -0,0 +1,5 @@
+Added
+^^^^^
+
+* Added OVPhysX backend dispatch typing for
+  :class:`~isaaclab.sensors.FrameTransformer`.

source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py

@@ -14,6 +14,7 @@
 
 if TYPE_CHECKING:
     from isaaclab_newton.sensors.frame_transformer import FrameTransformer as NewtonFrameTransformer
+    from isaaclab_ovphysx.sensors.frame_transformer import FrameTransformer as OvPhysxFrameTransformer
     from isaaclab_physx.sensors.frame_transformer import FrameTransformer as PhysXFrameTransformer
     from isaaclab_physx.sensors.frame_transformer import FrameTransformerData as PhysXFrameTransformerData
 
@@ -23,6 +24,8 @@ class FrameTransformer(FactoryBase, BaseFrameTransformer):
 
     data: BaseFrameTransformerData | PhysXFrameTransformerData
 
-    def __new__(cls, *args, **kwargs) -> BaseFrameTransformer | NewtonFrameTransformer | PhysXFrameTransformer:
+    def __new__(
+        cls, *args, **kwargs
+    ) -> BaseFrameTransformer | NewtonFrameTransformer | OvPhysxFrameTransformer | PhysXFrameTransformer:
         """Create a new instance of a frame transformer based on the backend."""
         return super().__new__(cls, *args, **kwargs)

source/isaaclab_ovphysx/changelog.d/antoiner-feat-ovphysx_frametransformer.minor.rst

@@ -0,0 +1,6 @@
+Added
+^^^^^
+
+* Added :class:`~isaaclab_ovphysx.sensors.FrameTransformer` and
+  :class:`~isaaclab_ovphysx.sensors.FrameTransformerData` for OVPhysX
+  frame transform sensing.

source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/__init__.pyi

@@ -7,6 +7,8 @@ __all__ = [
     "ContactSensor",
     "ContactSensorCfg",
     "ContactSensorData",
+    "FrameTransformer",
+    "FrameTransformerData",
     "Imu",
     "ImuData",
     "JointWrenchSensor",
@@ -16,6 +18,7 @@ __all__ = [
 ]
 
 from .contact_sensor import ContactSensor, ContactSensorCfg, ContactSensorData
+from .frame_transformer import FrameTransformer, FrameTransformerData
 from .imu import Imu, ImuData
 from .joint_wrench import JointWrenchSensor, JointWrenchSensorData
 from .pva import Pva, PvaData

source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/frame_transformer/__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 frame transformer sensor."""
+
+from isaaclab.utils.module import lazy_export
+
+lazy_export()

source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/frame_transformer/__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__ = [
+    "FrameTransformer",
+    "FrameTransformerData",
+]
+
+from .frame_transformer import FrameTransformer
+from .frame_transformer_data import FrameTransformerData

source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/frame_transformer/frame_transformer_data.py

@@ -0,0 +1,187 @@
+# 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.frame_transformer import BaseFrameTransformerData
+from isaaclab.utils.warp import ProxyArray
+
+from isaaclab_ovphysx.sensors.kernels import concat_pos_and_quat_to_pose_1d_kernel, concat_pos_and_quat_to_pose_kernel
+
+
+class FrameTransformerData(BaseFrameTransformerData):
+    """Data container for the OVPhysX frame transformer sensor."""
+
+    @property
+    def target_frame_names(self) -> list[str]:
+        """Target frame names (order matches data ordering)."""
+        return self._target_frame_names
+
+    @property
+    def target_pose_source(self) -> ProxyArray:
+        """Pose of target frame(s) relative to source frame [m, unitless].
+
+        Shape is (num_instances, num_target_frames), dtype = wp.transformf. In torch this resolves to
+        (num_instances, num_target_frames, 7). The pose is provided in (x, y, z, qx, qy, qz, qw) format.
+        """
+        wp.launch(
+            concat_pos_and_quat_to_pose_kernel,
+            dim=(self._num_envs, self._num_target_frames),
+            inputs=[self._target_pos_source, self._target_quat_source],
+            outputs=[self._target_pose_source],
+            device=self._device,
+        )
+        if self._target_pose_source_ta is None:
+            self._target_pose_source_ta = ProxyArray(self._target_pose_source)
+        return self._target_pose_source_ta
+
+    @property
+    def target_pos_source(self) -> ProxyArray:
+        """Position of target frame(s) relative to source frame [m].
+
+        Shape is (num_instances, num_target_frames), dtype = wp.vec3f. In torch this resolves to
+        (num_instances, num_target_frames, 3).
+        """
+        if self._target_pos_source_ta is None:
+            self._target_pos_source_ta = ProxyArray(self._target_pos_source)
+        return self._target_pos_source_ta
+
+    @property
+    def target_quat_source(self) -> ProxyArray:
+        """Orientation of target frame(s) relative to source frame [unitless].
+
+        Shape is (num_instances, num_target_frames), dtype = wp.quatf. In torch this resolves to
+        (num_instances, num_target_frames, 4). The orientation is provided in (x, y, z, w) format.
+        """
+        if self._target_quat_source_ta is None:
+            self._target_quat_source_ta = ProxyArray(self._target_quat_source)
+        return self._target_quat_source_ta
+
+    @property
+    def target_pose_w(self) -> ProxyArray:
+        """Pose of target frame(s) after offset in world frame [m, unitless].
+
+        Shape is (num_instances, num_target_frames), dtype = wp.transformf. In torch this resolves to
+        (num_instances, num_target_frames, 7). The pose is provided in (x, y, z, qx, qy, qz, qw) format.
+        """
+        wp.launch(
+            concat_pos_and_quat_to_pose_kernel,
+            dim=(self._num_envs, self._num_target_frames),
+            inputs=[self._target_pos_w, self._target_quat_w],
+            outputs=[self._target_pose_w],
+            device=self._device,
+        )
+        if self._target_pose_w_ta is None:
+            self._target_pose_w_ta = ProxyArray(self._target_pose_w)
+        return self._target_pose_w_ta
+
+    @property
+    def target_pos_w(self) -> ProxyArray:
+        """Position of target frame(s) after offset in world frame [m].
+
+        Shape is (num_instances, num_target_frames), dtype = wp.vec3f. In torch this resolves to
+        (num_instances, num_target_frames, 3).
+        """
+        if self._target_pos_w_ta is None:
+            self._target_pos_w_ta = ProxyArray(self._target_pos_w)
+        return self._target_pos_w_ta
+
+    @property
+    def target_quat_w(self) -> ProxyArray:
+        """Orientation of target frame(s) after offset in world frame [unitless].
+
+        Shape is (num_instances, num_target_frames), dtype = wp.quatf. In torch this resolves to
+        (num_instances, num_target_frames, 4). The orientation is provided in (x, y, z, w) format.
+        """
+        if self._target_quat_w_ta is None:
+            self._target_quat_w_ta = ProxyArray(self._target_quat_w)
+        return self._target_quat_w_ta
+
+    @property
+    def source_pose_w(self) -> ProxyArray:
+        """Pose of source frame after offset 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._source_pos_w, self._source_quat_w],
+            outputs=[self._source_pose_w],
+            device=self._device,
+        )
+        if self._source_pose_w_ta is None:
+            self._source_pose_w_ta = ProxyArray(self._source_pose_w)
+        return self._source_pose_w_ta
+
+    @property
+    def source_pos_w(self) -> ProxyArray:
+        """Position of source frame after offset in world frame [m].
+
+        Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3).
+        """
+        if self._source_pos_w_ta is None:
+            self._source_pos_w_ta = ProxyArray(self._source_pos_w)
+        return self._source_pos_w_ta
+
+    @property
+    def source_quat_w(self) -> ProxyArray:
+        """Orientation of source frame after offset in world frame [unitless].
+
+        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._source_quat_w_ta is None:
+            self._source_quat_w_ta = ProxyArray(self._source_quat_w)
+        return self._source_quat_w_ta
+
+    def create_buffers(
+        self,
+        num_envs: int,
+        num_target_frames: int,
+        target_frame_names: list[str],
+        device: str,
+    ) -> None:
+        """Create internal buffers for sensor data.
+
+        Args:
+            num_envs: Number of environments.
+            num_target_frames: Number of target frames.
+            target_frame_names: Names of target frames.
+            device: Device for tensor storage.
+        """
+        self._num_envs = num_envs
+        self._device = device
+        self._num_target_frames = num_target_frames
+        self._target_frame_names = target_frame_names
+        self._source_pose_w = wp.zeros(num_envs, dtype=wp.transformf, device=device)
+        self._source_pos_w = wp.zeros(num_envs, dtype=wp.vec3f, device=device)
+        self._source_quat_w = wp.zeros(num_envs, dtype=wp.quatf, device=device)
+        self._target_pose_w = wp.zeros((num_envs, num_target_frames), dtype=wp.transformf, device=device)
+        self._target_pos_w = wp.zeros((num_envs, num_target_frames), dtype=wp.vec3f, device=device)
+        self._target_quat_w = wp.zeros((num_envs, num_target_frames), dtype=wp.quatf, device=device)
+        self._target_pose_source = wp.zeros((num_envs, num_target_frames), dtype=wp.transformf, device=device)
+        self._target_pos_source = wp.zeros((num_envs, num_target_frames), dtype=wp.vec3f, device=device)
+        self._target_quat_source = wp.zeros((num_envs, num_target_frames), dtype=wp.quatf, device=device)
+
+        # Initialize quaternions to identity (w=1). wp.zeros gives (0,0,0,0) not (0,0,0,1).
+
+        wp.to_torch(self._source_quat_w)[:, 3] = 1.0
+        wp.to_torch(self._target_quat_w)[:, :, 3] = 1.0
+        wp.to_torch(self._target_quat_source)[:, :, 3] = 1.0
+
+        # -- Pinned ProxyArray cache (one per read property, lazily created on first access)
+        self._target_pose_source_ta: ProxyArray | None = None
+        self._target_pos_source_ta: ProxyArray | None = None
+        self._target_quat_source_ta: ProxyArray | None = None
+        self._target_pose_w_ta: ProxyArray | None = None
+        self._target_pos_w_ta: ProxyArray | None = None
+        self._target_quat_w_ta: ProxyArray | None = None
+        self._source_pose_w_ta: ProxyArray | None = None
+        self._source_pos_w_ta: ProxyArray | None = None
+        self._source_quat_w_ta: ProxyArray | None = None

source/isaaclab_ovphysx/isaaclab_ovphysx/sensors/frame_transformer/kernels.py

@@ -0,0 +1,117 @@
+# 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
+
+"""Warp kernels for the OVPhysX frame transformer sensor."""
+
+import warp as wp
+
+# ---- Frame transformer update kernel ----
+
+
+@wp.kernel
+def frame_transformer_update_kernel(
+    env_mask: wp.array(dtype=wp.bool),
+    raw_transforms: wp.array(dtype=wp.transformf),
+    source_raw_indices: wp.array(dtype=wp.int32),
+    target_raw_indices: wp.array2d(dtype=wp.int32),
+    source_offset_pos: wp.array(dtype=wp.vec3f),
+    source_offset_quat: wp.array(dtype=wp.quatf),
+    target_offset_pos: wp.array(dtype=wp.vec3f),
+    target_offset_quat: wp.array(dtype=wp.quatf),
+    source_pos_w: wp.array(dtype=wp.vec3f),
+    source_quat_w: wp.array(dtype=wp.quatf),
+    target_pos_w: wp.array2d(dtype=wp.vec3f),
+    target_quat_w: wp.array2d(dtype=wp.quatf),
+    target_pos_source: wp.array2d(dtype=wp.vec3f),
+    target_quat_source: wp.array2d(dtype=wp.quatf),
+):
+    """Update frame transformer sensor data from raw PhysX transforms.
+
+    This kernel processes raw transforms from PhysX and computes:
+    1. Source frame pose in world frame (with optional offset)
+    2. Target frame poses in world frame (with optional offsets)
+    3. Target frame poses relative to source frame
+
+    Args:
+        raw_transforms: Raw transforms from PhysX view. Shape is (N*M,) where N is num_envs and M is num_bodies.
+        source_raw_indices: Indices into raw_transforms for source frame per environment. Shape is (N,).
+        target_raw_indices: Indices into raw_transforms for target frames per (env, frame). Shape is (N, M) where M is
+            num_target_frames.
+        source_offset_pos: Optional position offset for source frame. Shape is (N, 3).
+        source_offset_quat: Optional quaternion offset for source frame. Shape is (N, 4).
+        target_offset_pos: Optional position offsets for target frames. Shape is (M, 3).
+        target_offset_quat: Optional quaternion offsets for target frames. Shape is (M, 4).
+        source_pos_w: Output source position in world frame. Shape is (N, 3).
+        source_quat_w: Output source quaternion in world frame. Shape is (N, 4).
+        target_pos_w: Output target positions in world frame. Shape is (N, M, 3).
+        target_quat_w: Output target quaternions in world frame. Shape is (N, M, 4).
+        target_pos_source: Output target positions relative to source frame. Shape is (N, M, 3).
+        target_quat_source: Output target quaternions relative to source frame. Shape is (N, M, 4).
+    """
+    env_id, frame_id = wp.tid()
+
+    if not env_mask[env_id]:
+        return
+
+    # Get source frame transform
+    source_idx = source_raw_indices[env_id]
+    source_tf = raw_transforms[source_idx]
+
+    # Apply source frame offset
+    source_offset_tf = wp.transform(source_offset_pos[env_id], source_offset_quat[env_id])
+    source_tf_offset = wp.transform_multiply(source_tf, source_offset_tf)
+    source_pos_w[env_id] = wp.transform_get_translation(source_tf_offset)
+    source_quat_w[env_id] = wp.transform_get_rotation(source_tf_offset)
+
+    # Get target frame transform
+    target_idx = target_raw_indices[env_id, frame_id]
+    target_tf = raw_transforms[target_idx]
+
+    # Apply target offset if needed
+    target_offset_tf = wp.transform(target_offset_pos[frame_id], target_offset_quat[frame_id])
+    target_tf_offset = wp.transform_multiply(target_tf, target_offset_tf)
+    target_pos_w[env_id, frame_id] = wp.transform_get_translation(target_tf_offset)
+    target_quat_w[env_id, frame_id] = wp.transform_get_rotation(target_tf_offset)
+
+    # Compute target frame relative to source frame
+    source_tf_inv = wp.transform_inverse(source_tf_offset)
+    target_relative_tf = wp.transform_multiply(source_tf_inv, target_tf_offset)
+    target_pos_source[env_id, frame_id] = wp.transform_get_translation(target_relative_tf)
+    target_quat_source[env_id, frame_id] = wp.transform_get_rotation(target_relative_tf)
+
+
+# ---- Gather body pose kernel ----
+
+
+@wp.kernel
+def gather_body_pose_kernel(
+    env_mask: wp.array(dtype=wp.bool),
+    pose_buffer: wp.array(dtype=wp.transformf),
+    dst_flat_indices: wp.array(dtype=wp.int32),
+    raw_transforms: wp.array(dtype=wp.transformf),
+):
+    """Copy a single body's per-env pose into the flat raw transforms buffer.
+
+    For each env in the launch, copies ``pose_buffer[env]`` into
+    ``raw_transforms[dst_flat_indices[env]]``. Skips envs whose ``env_mask`` is False.
+
+    The pose buffer is a view (``wp.array.view(wp.transformf)``) over a
+    ``(num_envs, 7)`` ``float32`` array populated by
+    ``binding.read(...)`` for a single ``RIGID_BODY_POSE`` tensor binding,
+    so it has shape ``(num_envs,)``. One launch per tracked body fills the
+    body's slot column in the flat ``raw_transforms`` buffer.
+
+    Args:
+        env_mask: Active environment mask, shape ``(num_envs,)``.
+        pose_buffer: Per-env world pose [m, dimensionless], shape ``(num_envs,)``,
+            dtype ``wp.transformf`` in ``(px, py, pz, qx, qy, qz, qw)`` format.
+        dst_flat_indices: Destination slot in ``raw_transforms`` per env, shape ``(num_envs,)``.
+        raw_transforms: Destination flat pose buffer [m, dimensionless], shape
+            ``(num_envs * num_unique_bodies,)``, dtype ``wp.transformf``.
+    """
+    env_id = wp.tid()
+    if not env_mask[env_id]:
+        return
+    raw_transforms[dst_flat_indices[env_id]] = pose_buffer[env_id]