diff --git a/docs/source/api/lab/isaaclab.sensors.rst b/docs/source/api/lab/isaaclab.sensors.rst index f95c0cbbf365..2935e99bd0a8 100644 --- a/docs/source/api/lab/isaaclab.sensors.rst +++ b/docs/source/api/lab/isaaclab.sensors.rst @@ -46,6 +46,29 @@ Sensor Base ----------- +.. rubric:: Extending batch updates + +Sensors opt into eager batch updates by overriding +:attr:`SensorBase.supports_batch_update`, which defaults to ``False``. +Eager scene updates advance sensors in order, then batch the remaining buffer refreshes for +sensors that opted in. Lazy data access remains per sensor. + +For standalone use, :meth:`SensorBase.update_batch` performs the complete eager update for a +sequence of batch-capable sensors. It calls each sensor's ``update()`` once in the supplied order, +then refreshes pending buffers together. Each call advances sensor clocks by ``dt``, so use it +in place of separate ``sensor.update(dt)`` calls. An unsupported sensor raises ``ValueError`` +before any input is updated; update unsupported sensors individually. + +An implementation can override the static ``_update_buffers_batch_impl(sensors)`` hook to perform +shared work. Sensors inheriting the same hook are grouped together, including instances of +different subclasses. The hook must respect each sensor's outdated-environment mask and fill its +buffers; ``SensorBase`` handles the update timestamps after the hook succeeds. The default hook +calls each sensor's individual buffer implementation. + +Camera subclasses that override the individual buffer-update hooks retain individual updates by +default. They must explicitly opt into batching and ensure their batch implementation preserves +the custom behavior. + .. autoclass:: SensorBase :members: diff --git a/docs/source/concepts/renderers.rst b/docs/source/concepts/renderers.rst index 1568f259e34e..f4f55dd45934 100644 --- a/docs/source/concepts/renderers.rst +++ b/docs/source/concepts/renderers.rst @@ -428,6 +428,36 @@ For the RTX renderer (requires Isaac Sim): For RTX renderer settings, see :doc:`/source/how-to/configure_rendering`. +.. _renderer-camera-batching: + +Batching camera renders +----------------------- + +:meth:`~isaaclab.renderers.BaseRenderer.render_batch` accepts a sequence of render-data objects +owned by the same renderer. Prepare the camera poses, intrinsics, and shared scene state before +rendering, then read each camera's output. An empty sequence performs no rendering. + +.. code-block:: python + + renderer.render_batch([first_render_data, second_render_data]) + renderer.read_output(first_render_data, first_camera_data) + renderer.read_output(second_render_data, second_camera_data) + +:meth:`~isaaclab.renderers.BaseRenderer.render` continues to accept a single render-data object. +The default ``render_batch()`` implementation calls ``render()`` for each entry, so existing +custom renderers and single-camera callers need no changes. OVRTX overrides ``render_batch()`` +to submit the requested camera products in one native renderer step. + +With eager sensor updates (``scene.cfg.lazy_sensor_update=False``), ``scene.update()`` advances +sensor clocks in scene order and collects batch-capable sensors. After the loop, the camera +batch implementation prepares the remaining due captures for submission. +:meth:`~isaaclab.renderers.RenderContext.render_into_cameras` groups these requests by renderer +instance, renders each group, and reads its outputs. The context does not retain a pending-camera +queue. Each camera retains its own update period and reset state. + +With lazy sensor updates, reading a camera's ``data`` refreshes only that camera's sensor buffers +and capture timestamps. It does not refresh peer camera sensors sharing the renderer. + Core concepts ------------- diff --git a/docs/source/concepts/sensors/index.rst b/docs/source/concepts/sensors/index.rst index fe48c7b69810..5a1bc3dd81e1 100644 --- a/docs/source/concepts/sensors/index.rst +++ b/docs/source/concepts/sensors/index.rst @@ -22,6 +22,12 @@ sensor derives from :class:`~isaaclab.sensors.SensorBase` and follows the same l debug visualization is requested. * :meth:`~isaaclab.sensors.SensorBase.reset` clears per-environment timestamps and internal state. +With eager sensor updates (``scene.cfg.lazy_sensor_update=False``), ``scene.update(dt)`` refreshes +sensor data before returning. It advances sensor clocks in scene order, updating ordinary sensors +individually and collecting sensors that support batch updates. After the loop, it refreshes the +collected sensors' pending buffers together. Each sensor retains its own update period and reset +state. Camera sensors use this mechanism to :ref:`batch captures by renderer `. + Sensor data is exposed through :class:`~isaaclab.utils.warp.ProxyArray` buffers, including camera outputs. Use the ``torch`` property for a cached zero-copy Torch view or ``warp`` for the underlying Warp array. diff --git a/scripts/benchmarks/nsys_trace.json b/scripts/benchmarks/nsys_trace.json index e3a5b8a16675..fa8523455cb8 100644 --- a/scripts/benchmarks/nsys_trace.json +++ b/scripts/benchmarks/nsys_trace.json @@ -89,6 +89,7 @@ "color": "0xFFC107", "module": "isaaclab.renderers.render_context", "functions": [ + {"function": "RenderContext.render_into_cameras", "color": "0xFFC107"}, {"function": "RenderContext.render_into_camera", "color": "0xFFC107"}, {"function": "RenderContext.update_scene_state", "color": "0xFFD54F"}, {"function": "RenderContext.ensure_initialize", "color": "0xFFE082"}, @@ -266,6 +267,7 @@ "OVRTXRenderer.update_geometries", "OVRTXRenderer.update_camera", "OVRTXRenderer.render", + "OVRTXRenderer.render_batch", "OVRTXRenderer.read_output", {"module": "isaaclab_ov.renderers.ovrtx_usd", "function": "create_scene_partition_attributes"}, {"module": "isaaclab_ov.renderers.ovrtx_usd", "function": "export_stage_to_string"} diff --git a/source/isaaclab/changelog.d/fix-ovphysx-joint-wrench-frame.skip b/source/isaaclab/changelog.d/fix-ovphysx-joint-wrench-frame.skip new file mode 100644 index 000000000000..fe56d14e2e27 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-ovphysx-joint-wrench-frame.skip @@ -0,0 +1 @@ +Moved the shared joint-wrench physics test into the test tree and checked registration by every backend. diff --git a/source/isaaclab/changelog.d/fix-task-space-body-offset-jacobian.rst b/source/isaaclab/changelog.d/fix-task-space-body-offset-jacobian.rst new file mode 100644 index 000000000000..6e0f862f317e --- /dev/null +++ b/source/isaaclab/changelog.d/fix-task-space-body-offset-jacobian.rst @@ -0,0 +1,8 @@ +Fixed +^^^^^ + +* Fixed the body-offset Jacobian correction in :class:`~isaaclab.envs.mdp.actions.DifferentialInverseKinematicsAction` + and :class:`~isaaclab.envs.mdp.actions.OperationalSpaceControllerAction`. The offset is now rotated into the root + frame by the body orientation before shifting the translational rows, and the angular rows are no longer rotated by + the offset rotation, matching the offset frame's pose and velocity. Tasks that set ``body_offset`` (for example, the + Franka IK tasks) now receive the Jacobian of the offset frame instead of an approximation. diff --git a/source/isaaclab/changelog.d/render-batch.minor.rst b/source/isaaclab/changelog.d/render-batch.minor.rst new file mode 100644 index 000000000000..51df5f5c29b4 --- /dev/null +++ b/source/isaaclab/changelog.d/render-batch.minor.rst @@ -0,0 +1,10 @@ +Added +^^^^^ + +* Added ``BaseRenderer.render_batch()`` with a default loop over the existing single-camera + ``render()`` interface, allowing renderers to optimize multiple camera captures. +* Added ``SensorBase.supports_batch_update`` to opt sensors into eager scene batching and + ``SensorBase.update_batch()`` for standalone updates of batch-capable sensors. +* Added ``RenderContext.render_into_cameras()`` to group prepared captures by renderer and + batch due cameras during eager scene updates. Preserved per-camera lazy reads, update + periods, and reset state. diff --git a/source/isaaclab/isaaclab/benchmark/stepping.py b/source/isaaclab/isaaclab/benchmark/stepping.py index 34f3ead2dc52..d72a22439e01 100644 --- a/source/isaaclab/isaaclab/benchmark/stepping.py +++ b/source/isaaclab/isaaclab/benchmark/stepping.py @@ -65,24 +65,24 @@ def profile_renderers( originals = [] try: for _, renderer in render_context._renderer_entries: - render = renderer.render - original = vars(renderer).get("render", missing) + render = renderer.render_batch + original = vars(renderer).get("render_batch", missing) @wraps(render) def timed_render(render_data: Any, _render=render) -> None: with wp.ScopedTimer(RENDER_PROFILE_SCOPE, dict=scope_timings, print=False, synchronize=True): return _render(render_data) - renderer.render = timed_render + renderer.render_batch = timed_render originals.append((renderer, original)) yield timings finally: for renderer, original in reversed(originals): if original is missing: - del renderer.render + del renderer.render_batch else: - renderer.render = original + renderer.render_batch = original @contextmanager diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index e9fd9da411a9..b04728972203 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -254,18 +254,14 @@ def _compute_frame_jacobian(self): self._jacobian_b[:] = self.jacobian_b # account for the offset if self.cfg.body_offset is not None: - # Modify the jacobian to account for the offset - # -- translational part - # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee - # = (v_J_ee + w_J_ee x r_link_ee ) * q - # = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q - self._jacobian_b[:, 0:3, :] += torch.bmm( - -math_utils.skew_symmetric_matrix(self._offset_pos), self._jacobian_b[:, 3:, :] + # Express the lever arm in root axes; a rigid offset leaves angular velocity unchanged. + body_quat_b = math_utils.quat_mul( + math_utils.quat_inv(self._asset.data.root_quat_w.torch), + self._asset.data.body_quat_w.torch[:, self._body_idx], ) - # -- rotational part - # w_link = R_link_ee @ w_ee - self._jacobian_b[:, 3:, :] = torch.bmm( - math_utils.matrix_from_quat(self._offset_rot), self._jacobian_b[:, 3:, :] + offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) + self._jacobian_b[:, 0:3, :] += torch.bmm( + -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:, :] ) return self._jacobian_b @@ -673,19 +669,15 @@ def _compute_ee_jacobian(self): # account for the offset if self.cfg.body_offset is not None: - # Modify the jacobian to account for the offset - # -- translational part - # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee - # = (v_J_ee + w_J_ee x r_link_ee ) * q - # = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q + # Express the lever arm in root axes; a rigid offset leaves angular velocity unchanged. + body_quat_b = math_utils.quat_mul( + math_utils.quat_inv(self._asset.data.root_quat_w.torch), + self._asset.data.body_quat_w.torch[:, self._ee_body_idx], + ) + offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) self._jacobian_b[:, 0:3, :] += torch.bmm( - -math_utils.skew_symmetric_matrix(self._offset_pos), self._jacobian_b[:, 3:, :] - ) # type: ignore - # -- rotational part - # w_link = R_link_ee @ w_ee - self._jacobian_b[:, 3:, :] = torch.bmm( - math_utils.matrix_from_quat(self._offset_rot), self._jacobian_b[:, 3:, :] - ) # type: ignore + -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:, :] + ) def _compute_ee_pose(self): """Computes the pose of the ee frame in root frame.""" diff --git a/source/isaaclab/isaaclab/renderers/base_renderer.py b/source/isaaclab/isaaclab/renderers/base_renderer.py index 2f81f5538763..ba79dc6d81e8 100644 --- a/source/isaaclab/isaaclab/renderers/base_renderer.py +++ b/source/isaaclab/isaaclab/renderers/base_renderer.py @@ -15,7 +15,7 @@ from .output_contract import RenderBufferKind, RenderBufferSpec if TYPE_CHECKING: - from collections.abc import Callable + from collections.abc import Callable, Sequence import torch import warp as wp @@ -183,6 +183,20 @@ def render(self, render_data: Any) -> None: """ pass + def render_batch(self, render_data: Sequence[Any]) -> None: + """Render a collection of cameras into their bound output buffers. + + All camera poses and shared scene state must be prepared before calling this method. + An empty sequence is a no-op. Each object must belong to this renderer and appear once. + The default implementation calls :meth:`render` for each camera; subclasses may override + this method to submit all cameras together. + + Args: + render_data: Renderer-specific objects from :meth:`create_render_data`. + """ + for data in render_data: + self.render(data) + @abstractmethod def read_output(self, render_data: Any, camera_data: CameraData) -> None: """Read rendered outputs from the renderer into the camera data container. diff --git a/source/isaaclab/isaaclab/renderers/render_context.py b/source/isaaclab/isaaclab/renderers/render_context.py index bba6f7a76bf7..9d3e7ac55310 100644 --- a/source/isaaclab/isaaclab/renderers/render_context.py +++ b/source/isaaclab/isaaclab/renderers/render_context.py @@ -9,6 +9,7 @@ import logging import warnings +from collections.abc import Sequence from typing import TYPE_CHECKING, Any import torch @@ -334,10 +335,37 @@ def render_into_camera( camera_data: CameraData, physics_step_count: int, ) -> None: - """Sync scene state, render, and read outputs into ``camera_data``.""" + """Sync scene state and capture one camera through :meth:`render_into_cameras`.""" + self.render_into_cameras([(renderer, render_data, camera_data)], physics_step_count) + + def render_into_cameras( + self, + requests: Sequence[tuple[BaseRenderer, Any, CameraData]], + physics_step_count: int, + ) -> None: + """Render prepared cameras in batches grouped by renderer instance. + + Camera poses must be updated before this call. Requests are used only for this + submission; the context does not retain cameras or manage sensor timing. + + Args: + requests: Tuples of renderer, renderer-specific render data, and output camera data. + An empty sequence performs no work. + physics_step_count: Current physics step for shared scene synchronization. + """ + if not requests: + return + self.update_scene_state(physics_step_count) - renderer.render(render_data) - renderer.read_output(render_data, camera_data) + + groups: dict[int, tuple[BaseRenderer, list[tuple[Any, CameraData]]]] = {} + for renderer, render_data, camera_data in requests: + groups.setdefault(id(renderer), (renderer, []))[1].append((render_data, camera_data)) + + for renderer, cameras in groups.values(): + renderer.render_batch([render_data for render_data, _ in cameras]) + for render_data, camera_data in cameras: + renderer.read_output(render_data, camera_data) def reset_stage_prepare_flag(self) -> None: """Allow :meth:`ensure_prepare_stage` to run ``prepare_stage`` again (e.g. a new USD stage).""" diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index ca5d61ccd5ae..0ad7548cda61 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -516,8 +516,19 @@ def update(self, dt: float) -> None: for surface_gripper in self._surface_grippers.values(): surface_gripper.update(dt) # -- sensors + force_recompute = not self.cfg.lazy_sensor_update + batched_sensors: list[SensorBase] = [] for sensor in self._sensors.values(): - sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update) + if force_recompute and sensor.supports_batch_update: + # Defer buffer refresh until all sensors have advanced. + sensor.update(dt, force_recompute=False) + batched_sensors.append(sensor) + else: + sensor.update(dt, force_recompute=force_recompute) + + # Process batched sensors only during eager updates. + if batched_sensors: + SensorBase._process_batch(batched_sensors) """ Operations: Scene State. diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 1b6d2b801f11..bf7eb40bdcf3 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -8,7 +8,7 @@ import logging import sys from collections.abc import Sequence -from typing import TYPE_CHECKING, Any, Literal +from typing import TYPE_CHECKING, Any, Literal, cast import numpy as np import torch @@ -329,6 +329,18 @@ def __str__(self) -> str: def num_instances(self) -> int: return self._view.count + @property + def supports_batch_update(self) -> bool: + """Whether captures can use the shared camera batch implementation. + + Custom scalar capture hooks retain their individual update path. Subclasses may opt + in explicitly when they also provide a compatible ``_update_buffers_batch_impl``. + """ + return ( + type(self)._update_buffers_impl is Camera._update_buffers_impl + and type(self)._update_outdated_buffers is SensorBase._update_outdated_buffers + ) + @property def data(self) -> CameraData: # update sensors if needed @@ -703,15 +715,18 @@ def _initialize_impl(self): # Create internal buffers (includes intrinsic matrix and pose init) self._create_buffers() - def _update_buffers_impl(self, env_mask: wp.array): - if not self._env_mask_has_any(env_mask): - return - # Increment frame count + def _prepare_camera(self, env_mask: wp.array) -> None: + """Advance capture frames and refresh requested poses before rendering.""" if self.cfg.update_latest_camera_pose: self._update_poses(env_mask=env_mask, frame_op=1) else: self._update_camera_state(env_mask=env_mask, frame_op=1) + def _update_buffers_impl(self, env_mask: wp.array): + if not self._env_mask_has_any(env_mask): + return + self._prepare_camera(env_mask) + sim_ctx = sim_utils.SimulationContext.instance() renderer = self._renderer assert renderer is not None @@ -726,6 +741,30 @@ def _update_buffers_impl(self, env_mask: wp.array): renderer.render(self._render_data) renderer.read_output(self._render_data, self._data) + @staticmethod + def _update_buffers_batch_impl(sensors: Sequence[SensorBase]) -> None: + """Prepare due cameras and render them together through their shared context.""" + cameras = cast(Sequence[Camera], sensors) + sim_ctx = sim_utils.SimulationContext.instance() + if sim_ctx is None: + for camera in cameras: + camera._update_buffers_impl(camera._is_outdated) + return + + ready = [] + for camera in cameras: + if not camera._env_mask_has_any(camera._is_outdated): + continue + camera._prepare_camera(camera._is_outdated) + ready.append(camera) + if not ready: + return + + sim_ctx.render_context.render_into_cameras( + [(camera._renderer, camera._render_data, camera._data) for camera in ready], + sim_ctx.get_physics_step_count(), + ) + """ Private Helpers """ diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 274b7089309a..c405e78a2840 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -16,7 +16,7 @@ import sys import weakref from abc import ABC, abstractmethod -from collections.abc import Sequence +from collections.abc import Callable, Sequence from typing import TYPE_CHECKING, Any import warp as wp @@ -39,10 +39,9 @@ class SensorBase(ABC): """The base class for implementing a sensor. - The implementation is based on lazy evaluation. The sensor data is only updated when the user - tries accessing the data through the :attr:`data` property or sets ``force_compute=True`` in - the :meth:`update` method. This is done to avoid unnecessary computation when the sensor data - is not used. + The implementation is based on lazy evaluation. Sensor buffers are refreshed through the + :attr:`data` property, by setting ``force_recompute=True`` in :meth:`update`, or by calling + :meth:`update_batch`. This avoids unnecessary computation when sensor data is not used. The sensor is updated at the specified update period. If the update period is zero, then the sensor is updated at every simulation step. @@ -106,6 +105,15 @@ def device(self) -> str: """Memory device for computation.""" return self._device + @property + def supports_batch_update(self) -> bool: + """Whether eager buffer refreshes can share work through :meth:`update_batch`. + + Defaults to False. Sensors may opt in and implement ``_update_buffers_batch_impl`` + to share work with sensors that use the same batch implementation. + """ + return False + @property @abstractmethod def data(self) -> Any: @@ -211,6 +219,34 @@ def update(self, dt: float, force_recompute: bool = False): if force_recompute or self._is_visualizing: self._update_outdated_buffers(force_recompute=force_recompute) + @staticmethod + def update_batch(sensors: Sequence[SensorBase], dt: float) -> None: + """Advance batch-capable sensors and eagerly refresh their data in compatible groups. + + All sensors must report :attr:`supports_batch_update` as True. Calls each sensor's + :meth:`update` once in input order with ``force_recompute=False``, then processes + pending buffers through their shared ``_update_buffers_batch_impl`` static methods. + Use this method instead of calling :meth:`update` separately for the same time step. + + Uninitialized sensors and sensors already refreshed during the update loop are excluded + from batch processing. Each batch is marked updated only after its implementation + returns successfully. + + Args: + sensors: Batch-capable sensors to update, each appearing once. An empty sequence + performs no work. + dt: Time elapsed since the previous sensor update [s]. + + Raises: + ValueError: If any sensor does not support batch updates. No sensors are advanced + in this case. + """ + if any(not sensor.supports_batch_update for sensor in sensors): + raise ValueError("Batch updates require sensors with supports_batch_update=True.") + for sensor in sensors: + sensor.update(dt, force_recompute=False) + SensorBase._process_batch(sensors) + """ Implementation specific. """ @@ -276,6 +312,21 @@ def _update_buffers_impl(self, env_mask: wp.array): """ raise NotImplementedError + @staticmethod + def _update_buffers_batch_impl(sensors: Sequence[SensorBase]) -> None: + """Fill buffers for initialized sensors that share this batch implementation. + + Each sensor's ``_is_outdated`` mask selects its due environments and may be empty. + Implementations must fill the requested buffers before returning and leave timestamp + and generation bookkeeping to the caller. The default implementation + calls each sensor's ``_update_buffers_impl`` individually. + + Args: + sensors: Sensors whose buffers need checking, all using this static method. + """ + for sensor in sensors: + sensor._update_buffers_impl(sensor._is_outdated) + def _set_debug_vis_impl(self, debug_vis: bool): """Set debug visualization into visualization objects. @@ -398,6 +449,10 @@ def _update_outdated_buffers(self, force_recompute: bool = False) -> None: if not force_recompute and self._data_generation == self._data_generation_last_update: return self._update_buffers_impl(self._is_outdated) + self._mark_buffers_updated() + + def _mark_buffers_updated(self) -> None: + """Commit capture timestamps after the sensor's output buffers have been filled.""" # update timestamps and clear outdated flags wp.launch( update_outdated_envs_kernel, @@ -407,6 +462,22 @@ def _update_outdated_buffers(self, force_recompute: bool = False) -> None: ) self._data_generation_last_update = self._data_generation + @staticmethod + def _process_batch(sensors: Sequence[SensorBase]) -> None: + """Refresh pending buffers for batch-capable sensors whose timing has already advanced.""" + # A later sensor's update may already have refreshed an earlier sensor's data. + groups: dict[Callable[[Sequence[SensorBase]], None], list[SensorBase]] = {} + for sensor in sensors: + if not sensor.is_initialized or sensor._data_generation == sensor._data_generation_last_update: + continue + batch_impl = type(sensor)._update_buffers_batch_impl + groups.setdefault(batch_impl, []).append(sensor) + + for batch_impl, group in groups.items(): + batch_impl(group) + for sensor in group: + sensor._mark_buffers_updated() + def _resolve_indices_and_mask( self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None ) -> wp.array: diff --git a/source/isaaclab/isaaclab/test/utils/joint_wrench.py b/source/isaaclab/isaaclab/test/utils/joint_wrench.py deleted file mode 100644 index 7dc7c5526c4f..000000000000 --- a/source/isaaclab/isaaclab/test/utils/joint_wrench.py +++ /dev/null @@ -1,76 +0,0 @@ -# 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 - -"""Shared physical contract for joint-wrench sensor backends.""" - -from pathlib import Path - -import torch - -from pxr import Gf, Usd, UsdGeom, UsdPhysics - -import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets import ArticulationCfg -from isaaclab.physics import PhysicsCfg -from isaaclab.scene import InteractiveScene, InteractiveSceneCfg -from isaaclab.sensors import JointWrenchSensorCfg -from isaaclab.sim import SimulationCfg -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, retrieve_file_path - - -def check_joint_wrench_frame(physics: PhysicsCfg, tmp_path: Path) -> None: - """Compare a loaded joint's wrench with gravity equilibrium using either physics backend.""" - source = retrieve_file_path(f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd") - usd_path = str(tmp_path / "joint_wrench.usda") - stage = Usd.Stage.CreateNew(usd_path) - root = stage.DefinePrim("/Articulation", "Xform") - stage.SetDefaultPrim(root) - root.GetReferences().AddReference(source) - joint = next(UsdPhysics.Joint(prim) for prim in stage.Traverse() if prim.IsA(UsdPhysics.RevoluteJoint)) - UsdPhysics.RevoluteJoint(joint).GetAxisAttr().Set("Z") - joint.GetLocalPos1Attr().Set(Gf.Vec3f(0.25, -0.15, 0.1)) - joint.GetLocalRot1Attr().Set(Gf.Quatf(2.0**-0.5, Gf.Vec3f(2.0**-0.5, 0.0, 0.0))) - # Unit scale makes the authored joint offset a metric offset. Align the joint frames initially. - arm_prim = stage.GetPrimAtPath(joint.GetBody1Rel().GetTargets()[0]) - pose = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(1.0, 0.0, 0.0), -90.0)) - pose.SetTranslateOnly(Gf.Vec3d(-0.25, -0.1, -0.15)) - UsdGeom.Xformable(arm_prim).MakeMatrixXform().Set(pose) - mass = UsdPhysics.MassAPI.Apply(arm_prim) - mass.CreateMassAttr(2.0) - mass.CreateCenterOfMassAttr(Gf.Vec3f(0.0)) - for prim in stage.Traverse(): - if prim.HasAPI(UsdPhysics.CollisionAPI): - UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Set(False) - stage.GetRootLayer().Save() - - with sim_utils.build_simulation_context(sim_cfg=SimulationCfg(dt=1.0 / 200.0, physics=physics)) as sim: - sim._app_control_on_stop_handle = None - cfg = InteractiveSceneCfg(num_envs=1, env_spacing=2.0) - cfg.robot = ArticulationCfg( - prim_path="{ENV_REGEX_NS}/Robot", - spawn=sim_utils.UsdFileCfg(usd_path=usd_path), - actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0)}, - init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0), rot=(0.0, 0.0, 2.0**-0.5, 2.0**-0.5)), - ) - cfg.wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") - scene = InteractiveScene(cfg) - sim.reset() - for _ in range(20): - sim.step() - scene.update(sim.get_physics_dt()) - - robot, sensor = scene["robot"], scene["wrench"] - arm = robot.body_names.index("Arm") - sensor_arm = sensor.find_bodies("Arm")[0][0] - assert robot.data.body_com_vel_w.torch[:, arm].norm() < 1e-3 - # The joint frame is rotated 90 degrees about world Z, so gravity still points along its -Z. - # Its 2 kg load is offset by (-0.25, -0.1, -0.15) m in joint coordinates. - # Reaction force is (0, 0, mg), and r x F gives torque (-0.1 mg, 0.25 mg, 0). - weight = -2.0 * sim.cfg.gravity[2] - expected_force = torch.tensor([[0.0, 0.0, weight]], device=sim.device) - expected_torque = torch.tensor([[-0.1 * weight, 0.25 * weight, 0.0]], device=sim.device) - torch.testing.assert_close(sensor.data.force.torch[:, sensor_arm], expected_force, atol=1e-2, rtol=1e-3) - torch.testing.assert_close(sensor.data.torque.torch[:, sensor_arm], expected_torque, atol=1e-2, rtol=1e-3) diff --git a/source/isaaclab/test/benchmark/test_stepping.py b/source/isaaclab/test/benchmark/test_stepping.py index 1bcacd3bac89..364d361140c8 100644 --- a/source/isaaclab/test/benchmark/test_stepping.py +++ b/source/isaaclab/test/benchmark/test_stepping.py @@ -97,11 +97,11 @@ def test_profile_renderers_wraps_each_renderer(monkeypatch, capsys): second_render = Mock(side_effect=ValueError("render failed")) class Renderer: - def render(self, render_data): + def render_batch(self, render_data): second_render(render_data) - first, second = SimpleNamespace(render=Mock()), Renderer() - originals = [first.render, second.render] + first, second = SimpleNamespace(render_batch=Mock()), Renderer() + originals = [first.render_batch, second.render_batch] context = SimpleNamespace(_renderer_entries=[(None, first), (None, second)]) synchronize = Mock() monkeypatch.setattr(wp, "synchronize", synchronize) @@ -110,28 +110,28 @@ def render(self, render_data): with pytest.raises(ValueError, match="render failed"): with profile_renderers(context, timings=timings) as collected: assert collected is timings - first.render("first") - second.render("second") + first.render_batch(["first"]) + second.render_batch(["second"]) - originals[0].assert_called_once_with("first") - second_render.assert_called_once_with("second") + originals[0].assert_called_once_with(["first"]) + second_render.assert_called_once_with(["second"]) assert synchronize.call_count == 4 assert len(timings) == 2 assert all(scope == RENDER_PROFILE_SCOPE and elapsed >= 0.0 for scope, elapsed in timings) assert RENDER_PROFILE_SCOPE not in capsys.readouterr().out - assert [first.render, second.render] == originals - assert "render" not in vars(second) + assert [first.render_batch, second.render_batch] == originals + assert "render_batch" not in vars(second) second_render.side_effect = None - first.render("unprofiled") + first.render_batch(["unprofiled"]) for enabled in (True, False): with profile_renderers(context, active=enabled) as later_timings: - first.render("first") - second.render("second") + first.render_batch(["first"]) + second.render_batch(["second"]) assert len(later_timings) == 2 * int(enabled) assert len(timings) == 2 - assert [first.render, second.render] == originals - assert "render" not in vars(second) + assert [first.render_batch, second.render_batch] == originals + assert "render_batch" not in vars(second) assert synchronize.call_count == 8 diff --git a/source/isaaclab/test/envs/test_diffik_jacobian_aliasing.py b/source/isaaclab/test/envs/test_diffik_jacobian_aliasing.py index 86e287cb9522..c92794fe8b79 100644 --- a/source/isaaclab/test/envs/test_diffik_jacobian_aliasing.py +++ b/source/isaaclab/test/envs/test_diffik_jacobian_aliasing.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Regression tests for DiffIK Jacobian aliasing (NVBug 6043099). +"""Regression tests for task-space body offsets and DiffIK Jacobian aliasing (NVBug 6043099). ``DifferentialInverseKinematicsAction._compute_frame_jacobian`` historically aliased the parent Jacobian and applied the body-offset correction in place. @@ -13,20 +13,25 @@ idempotent regardless of whether ``jacobian_b`` returns a view or a copy. """ +import math from types import SimpleNamespace import pytest import torch -from isaaclab.envs.mdp.actions.task_space_actions import DifferentialInverseKinematicsAction +from isaaclab.envs.mdp.actions.task_space_actions import ( + DifferentialInverseKinematicsAction, + OperationalSpaceControllerAction, +) from isaaclab.utils import math as math_utils pytestmark = pytest.mark.unit class _Stub: - """Minimal stand-in for ``DifferentialInverseKinematicsAction`` that exposes only - what ``_compute_frame_jacobian`` reads. ``jacobian_b`` returns the backing buffer + """Minimal stand-in for the task-space actions' Jacobian methods. + + ``jacobian_b`` returns the backing buffer **without copying**, mirroring the worst case where the data layer hands out a view onto engine memory. The owned ``_jacobian_b`` buffer is what the fixed method must write into. @@ -38,6 +43,16 @@ def __init__(self, num_envs: int, num_joints: int, body_offset_pos, body_offset_ self._offset_rot = torch.tensor(body_offset_rot, dtype=torch.float32).repeat(num_envs, 1) self._jacobian_b = torch.zeros(num_envs, 6, num_joints) self._backing_buffer = backing_buffer + # Non-trivial root and body orientations, so the offset must be rotated into the root frame. + self._body_idx = self._ee_body_idx = 0 + root_quat_w = math_utils.quat_from_euler_xyz(torch.tensor(0.3), torch.tensor(-0.2), torch.tensor(0.5)) + body_quat_w = math_utils.quat_from_euler_xyz(torch.tensor(-1.1), torch.tensor(0.4), torch.tensor(2.0)) + self._asset = SimpleNamespace( + data=SimpleNamespace( + root_quat_w=SimpleNamespace(torch=root_quat_w.repeat(num_envs, 1)), + body_quat_w=SimpleNamespace(torch=body_quat_w.repeat(num_envs, 1, 1)), + ) + ) @property def jacobian_b(self): @@ -91,17 +106,42 @@ def test_compute_frame_jacobian_applies_offset_once(): stub = _make_stub(num_envs, num_joints, offset_pos, offset_rot, backing) - # Reference: out-of-place computation, no aliasing. - skew = math_utils.skew_symmetric_matrix(stub._offset_pos) - rot = math_utils.matrix_from_quat(stub._offset_rot) - ref_trans = backing[:, 0:3, :] + torch.bmm(-skew, backing[:, 3:, :]) - ref_rot = torch.bmm(rot, backing[:, 3:, :]) - reference = torch.cat([ref_trans, ref_rot], dim=1) + # Reference: out-of-place computation, no aliasing. The offset is rotated into the root frame + # and the angular rows are unchanged, since the offset frame is rigidly attached to the body. + root_rot = math_utils.matrix_from_quat(stub._asset.data.root_quat_w.torch) + body_rot = math_utils.matrix_from_quat(stub._asset.data.body_quat_w.torch[:, 0]) + offset_b = torch.bmm(root_rot.mT @ body_rot, stub._offset_pos.unsqueeze(-1)).squeeze(-1) + ref_trans = backing[:, 0:3, :] + torch.bmm(-math_utils.skew_symmetric_matrix(offset_b), backing[:, 3:, :]) + reference = torch.cat([ref_trans, backing[:, 3:, :]], dim=1) actual = DifferentialInverseKinematicsAction._compute_frame_jacobian(stub) torch.testing.assert_close(actual, reference) +@pytest.mark.parametrize( + "compute", + [ + DifferentialInverseKinematicsAction._compute_frame_jacobian, + OperationalSpaceControllerAction._compute_ee_jacobian, + ], + ids=["diff_ik", "osc"], +) +def test_body_offset_jacobian_uses_offset_in_root_frame(compute): + """The offset uses root axes, and a rigid offset rotation leaves angular rows unchanged.""" + # One revolute joint about root z, with the body rotated 90 degrees about z. + backing = torch.tensor([[[0.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) + # A 90 degree rotation about x for the offset frame must not change the angular rows. + offset_rot = [math.sin(math.pi / 4.0), 0.0, 0.0, math.cos(math.pi / 4.0)] + stub = _make_stub(1, 1, [1.0, 0.0, 0.0], offset_rot, backing) + stub._asset.data.root_quat_w.torch[:] = torch.tensor([0.0, 0.0, 0.0, 1.0]) + stub._asset.data.body_quat_w.torch[:] = torch.tensor([0.0, 0.0, math.sin(math.pi / 4.0), math.cos(math.pi / 4.0)]) + compute(stub) + + # R_body_b @ (1, 0, 0) = (0, 1, 0), and z x (0, 1, 0) = (-1, 0, 0). + expected = torch.tensor([[[-1.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) + torch.testing.assert_close(stub._jacobian_b, expected) + + def test_compute_frame_jacobian_returns_owned_buffer(): """The returned tensor must be the owned buffer, not the data-layer source. diff --git a/source/isaaclab/test/renderers/test_simulation_render_context.py b/source/isaaclab/test/renderers/test_simulation_render_context.py index 3ada5ae132d9..678a2cb557ed 100644 --- a/source/isaaclab/test/renderers/test_simulation_render_context.py +++ b/source/isaaclab/test/renderers/test_simulation_render_context.py @@ -7,18 +7,24 @@ from __future__ import annotations +import contextlib from types import SimpleNamespace from unittest.mock import Mock, call +import numpy as np import pytest import torch +import warp as wp from isaaclab.benchmark.stepping import RENDER_PROFILE_SCOPE, profile_renderers from isaaclab.renderers.base_renderer import BaseRenderer from isaaclab.renderers.render_context import RenderContext from isaaclab.renderers.renderer_cfg import RendererCfg +from isaaclab.scene import InteractiveScene +from isaaclab.sensors import Camera, SensorBase from isaaclab.sensors.camera.camera_data import CameraData from isaaclab.sim import BackendCfg, SimulationContext +from isaaclab.utils.warp import ProxyArray pytest.importorskip("isaaclab_physx") pytest.importorskip("isaaclab_newton") @@ -187,10 +193,10 @@ def test_render_into_camera_call_order_and_profile_output(sim, capsys, profile): assert renderer.mock_calls == [ call.update_transforms(), call.update_geometries(), - call.render(data), + call.render_batch([data]), call.read_output(data, camera), call.update_transforms(), - call.render(data), + call.render_batch([data]), call.read_output(data, camera), ] assert len(timings) == (2 if profile else 0) @@ -198,6 +204,41 @@ def test_render_into_camera_call_order_and_profile_output(sim, capsys, profile): assert RENDER_PROFILE_SCOPE not in capsys.readouterr().out +def test_default_render_batch_preserves_single_camera_render_contract(): + """Existing render implementations handle batches in order, including empty batches.""" + renderer = SimpleNamespace(render=Mock()) + render_data = (object(), object()) + + BaseRenderer.render_batch(renderer, ()) + renderer.render.assert_not_called() + BaseRenderer.render_batch(renderer, render_data) + assert renderer.render.call_args_list == [call(data) for data in render_data] + + +def test_render_into_cameras_groups_renderers_and_reads_each_output(sim): + first = sim.get_or_create_backend(RendererCfg(class_type=_renderer)) + second = sim.get_or_create_backend(RendererCfg(class_type=_renderer, renderer_type="second")) + data = [object() for _ in range(3)] + cameras = [CameraData() for _ in data] + + sim.render_context.render_into_cameras([], physics_step_count=1) + first.render_batch.assert_not_called() + second.render_batch.assert_not_called() + sim.render_context.render_into_cameras( + [(first, data[0], cameras[0]), (second, data[1], cameras[1]), (first, data[2], cameras[2])], + physics_step_count=1, + ) + + first.render_batch.assert_called_once_with([data[0], data[2]]) + second.render_batch.assert_called_once_with([data[1]]) + assert first.read_output.call_args_list == [call(data[0], cameras[0]), call(data[2], cameras[2])] + second.read_output.assert_called_once_with(data[1], cameras[1]) + for renderer in (first, second): + renderer.render.assert_not_called() + renderer.update_transforms.assert_called_once_with() + renderer.update_geometries.assert_called_once_with() + + def test_legacy_render_profile_scope_warns_and_preserves_import(): """The old scope import remains available during its deprecation period.""" with pytest.warns(DeprecationWarning, match="isaaclab.benchmark.stepping.RENDER_PROFILE_SCOPE"): @@ -236,3 +277,339 @@ def test_context_close_only_releases_writers_and_resets_bookkeeping(sim, fail_wr context.update_scene_state(1) assert renderer.initialize.call_count == renderer.prepare_stage.call_count == 2 assert renderer.update_transforms.call_count == renderer.update_geometries.call_count == 2 + + +class _CpuCamera(Camera): + """Exercise capture timing with CPU buffers and an in-memory pose source.""" + + def __init__(self, renderer, name, update_period=0.0): + self.cfg = SimpleNamespace(update_period=update_period, update_latest_camera_pose=True) + self._device = "cpu" + self._num_envs = 2 + self._is_initialized = True + self._is_visualizing = False + self._renderer = renderer + self._render_data = SimpleNamespace(name=name, pose=None) + self._data = CameraData() + self._data.create_buffers(2, "cpu") + self._data.info = {} + self._frame = ProxyArray(wp.zeros(2, dtype=wp.int64, device="cpu")) + self._ALL_INDICES = wp.array([0, 1], dtype=wp.int32, device="cpu") + self._ALL_ENV_MASK = wp.ones(2, dtype=wp.bool, device="cpu") + self._is_outdated = wp.ones(2, dtype=wp.bool, device="cpu") + self._timestamp = wp.zeros(2, device="cpu") + self._timestamp_last_update = wp.zeros(2, device="cpu") + self._data_generation = 0 + self._data_generation_last_update = -1 + self.pose = 0.0 + self._view = SimpleNamespace(count=2, xform_world_space_writer=self._pose_writer) + self.update(0.0) + + def __del__(self): + pass + + @contextlib.contextmanager + def _pose_writer(self): + def set_poses(positions, orientations, indices): + self.pose = float(positions.numpy()[0, 0]) + + yield SimpleNamespace(set_poses=set_poses) + + def _update_poses(self, env_ids=None, env_mask=None, frame_op=0): + self._render_data.pose = self.pose + self._update_camera_state(env_ids=env_ids, env_mask=env_mask, frame_op=frame_op) + + +class _CpuSensor(SensorBase): + """Exercise generic sensor updates without a rendering backend.""" + + def __init__(self, name, batches=None): + self.cfg = SimpleNamespace(update_period=0.0) + self._device = "cpu" + self._num_envs = 2 + self._is_initialized = True + self._is_visualizing = False + self._is_outdated = wp.ones(2, dtype=wp.bool, device="cpu") + self._timestamp = wp.zeros(2, device="cpu") + self._timestamp_last_update = wp.zeros(2, device="cpu") + self._data_generation = 0 + self._data_generation_last_update = -1 + self._data = np.zeros(2, dtype=int) + self.name = name + self.batches = batches + self.captures = [] + + def __del__(self): + pass + + @property + def data(self): + self._update_outdated_buffers() + return self._data + + def _initialize_impl(self): + pass + + def _update_buffers_impl(self, env_mask): + mask = env_mask.numpy() + self.captures.append(mask) + self._data[mask] += 1 + + +class _BatchSensor(_CpuSensor): + @property + def supports_batch_update(self): + return True + + @staticmethod + def _update_buffers_batch_impl(sensors): + sensors[0].batches.append([sensor.name for sensor in sensors]) + for sensor in sensors: + sensor._update_buffers_impl(sensor._is_outdated) + + +@pytest.fixture +def camera_batch_context(sim, monkeypatch): + ctx = sim.render_context + sim._physics_step_count = 1 + monkeypatch.setattr(SimulationContext, "_instance", sim) + renderer = sim.get_or_create_backend(NewtonWarpRendererCfg(class_type=_renderer)) + batches = [] + renderer.render.side_effect = lambda rd: batches.append([(rd.name, rd.pose)]) + renderer.render_batch = Mock(side_effect=lambda requests: batches.append([(rd.name, rd.pose) for rd in requests])) + renderer.read_output.side_effect = lambda rd, data: data.info.update(pose=rd.pose) + return ctx, renderer, batches + + +def _camera_scene(cameras, lazy=False, **sensors): + return SimpleNamespace( + sim=SimulationContext.instance(), + cfg=SimpleNamespace(lazy_sensor_update=lazy), + _sensors={**{camera._render_data.name: camera for camera in cameras}, **sensors}, + **{ + name: {} + for name in ( + "_articulations", + "_cable_objects", + "_deformable_objects", + "_rigid_objects", + "_rigid_object_collections", + "_surface_grippers", + ) + }, + ) + + +def test_camera_eager_updates_render_shared_batch_with_current_poses(camera_batch_context): + """Eager scene updates batch initialized cameras and still update other sensor types.""" + _, renderer, batches = camera_batch_context + cameras = [_CpuCamera(renderer, "wide"), _CpuCamera(renderer, "tele")] + cameras[0].pose, cameras[1].pose = 1.0, 2.0 + uninitialized = _CpuCamera(renderer, "uninitialized") + uninitialized._is_initialized = False + other_sensor = Mock(supports_batch_update=False) + scene = _camera_scene([*cameras, uninitialized], other=other_sensor) + + InteractiveScene.update(scene, 0.01) + assert batches == [[("wide", 1.0), ("tele", 2.0)]] + renderer.render.assert_not_called() + other_sensor.update.assert_called_once_with(0.01, force_recompute=True) + assert cameras[0].data.info["pose"] == 1.0 + assert cameras[1].data.info["pose"] == 2.0 + assert batches == [[("wide", 1.0), ("tele", 2.0)]] + for camera in cameras: + np.testing.assert_array_equal(camera.frame.warp.numpy(), [1, 1]) + np.testing.assert_allclose(camera._timestamp_last_update.numpy(), [0.01, 0.01]) + np.testing.assert_array_equal(uninitialized.frame.warp.numpy(), [0, 0]) + + +def test_camera_lazy_reads_leave_peer_cameras_outdated(camera_batch_context): + """Reading a lazy camera captures only that camera, even when a peer shares its renderer.""" + _, renderer, batches = camera_batch_context + cameras = [_CpuCamera(renderer, "wide"), _CpuCamera(renderer, "tele")] + cameras[0].pose, cameras[1].pose = 1.0, 2.0 + other_sensor = Mock(supports_batch_update=False) + scene = _camera_scene(cameras, lazy=True, other=other_sensor) + + InteractiveScene.update(scene, 0.01) + assert not batches + other_sensor.update.assert_called_once_with(0.01, force_recompute=False) + assert cameras[0].data.info["pose"] == 1.0 + assert batches == [[("wide", 1.0)]] + np.testing.assert_array_equal(cameras[1].frame.warp.numpy(), [0, 0]) + np.testing.assert_array_equal(cameras[1]._timestamp_last_update.numpy(), [0.0, 0.0]) + + assert cameras[1].data.info["pose"] == 2.0 + for camera in cameras: + assert camera.data.info["pose"] == camera.pose + np.testing.assert_array_equal(camera.frame.warp.numpy(), [1, 1]) + np.testing.assert_allclose(camera._timestamp_last_update.numpy(), [0.01, 0.01]) + assert batches == [[("wide", 1.0)], [("tele", 2.0)]] + renderer.render.assert_not_called() + + +def test_eager_scene_batches_multiple_sensor_families(camera_batch_context): + """Sensor families batch together unless an earlier update already refreshed their data.""" + _, renderer, camera_batches = camera_batch_context + sensor_batches = [] + + class InheritedSensor(_BatchSensor): + pass + + class OtherSensor(_BatchSensor): + @staticmethod + def _update_buffers_batch_impl(sensors): + _BatchSensor._update_buffers_batch_impl(sensors) + + class ObserverSensor(_CpuSensor): + def _update_buffers_impl(self, env_mask): + np.testing.assert_array_equal(observed.data, [1, 1]) + super()._update_buffers_impl(env_mask) + + observed = _BatchSensor("observed", sensor_batches) + observer = ObserverSensor("observer") + first = _BatchSensor("first", sensor_batches) + second = InheritedSensor("second", sensor_batches) + other = OtherSensor("other", sensor_batches) + cameras = [_CpuCamera(renderer, "wide"), _CpuCamera(renderer, "tele")] + scene = _camera_scene(cameras, observed=observed, observer=observer, first=first, other=other, second=second) + + InteractiveScene.update(scene, 0.25) + + assert camera_batches == [[("wide", 0.0), ("tele", 0.0)]] + assert sensor_batches == [["first", "second"], ["other"]] + for sensor in (observed, observer, first, second, other): + np.testing.assert_array_equal(sensor.data, [1, 1]) + np.testing.assert_allclose(sensor._timestamp.numpy(), [0.25, 0.25]) + np.testing.assert_allclose(sensor._timestamp_last_update.numpy(), [0.25, 0.25]) + assert len(sensor.captures) == 1 + + +def test_sensor_batch_advances_time_once_and_preserves_update_hooks(): + """Batch updates advance sensors in order and preserve visualization refreshes.""" + updates = [] + + class DefaultBatchSensor(_CpuSensor): + @property + def supports_batch_update(self): + return True + + def update(self, dt, force_recompute=False): + updates.append((self.name, dt, force_recompute)) + super().update(dt, force_recompute=force_recompute) + + batched = DefaultBatchSensor("batched") + visualized = DefaultBatchSensor("visualized") + visualized._is_visualizing = True + uninitialized = DefaultBatchSensor("uninitialized") + uninitialized._is_initialized = False + sensors = [batched, visualized, uninitialized] + + SensorBase.update_batch([], 0.25) + SensorBase.update_batch(sensors, 0.25) + + assert updates == [ + ("batched", 0.25, False), + ("visualized", 0.25, False), + ("uninitialized", 0.25, False), + ] + for sensor in (batched, visualized): + np.testing.assert_array_equal(sensor.data, [1, 1]) + np.testing.assert_allclose(sensor._timestamp.numpy(), [0.25, 0.25]) + np.testing.assert_allclose(sensor._timestamp_last_update.numpy(), [0.25, 0.25]) + assert len(sensor.captures) == 1 + assert not uninitialized.captures + np.testing.assert_array_equal(uninitialized._timestamp_last_update.numpy(), [0.0, 0.0]) + + +def test_sensor_batch_rejects_unsupported_input_before_updating(): + """Invalid batches leave all sensors untouched, including preceding supported sensors.""" + batches = [] + sensors = [_BatchSensor("supported", batches), _CpuSensor("unsupported")] + + with pytest.raises(ValueError, match="supports_batch_update"): + SensorBase.update_batch(sensors, 0.25) + + assert not batches + for sensor in sensors: + assert not sensor.captures + np.testing.assert_array_equal(sensor._timestamp.numpy(), [0.0, 0.0]) + + +@pytest.mark.parametrize("lazy", [False, True]) +@pytest.mark.parametrize("hook", ["_update_buffers_impl", "_update_outdated_buffers"]) +def test_camera_updates_preserve_custom_buffer_hooks(camera_batch_context, monkeypatch, lazy, hook): + """Custom camera capture hooks run in both eager and lazy scenes.""" + _, renderer, _ = camera_batch_context + custom_updates = Mock() + + class CustomCamera(_CpuCamera): + pass + + def custom_update(self, *args, **kwargs): + custom_updates() + return getattr(Camera, hook)(self, *args, **kwargs) + + monkeypatch.setattr(CustomCamera, hook, custom_update) + custom = CustomCamera(renderer, "custom") + peer = _CpuCamera(renderer, "peer") + InteractiveScene.update(_camera_scene([custom, peer], lazy=lazy), 0.01) + assert custom_updates.call_count == (0 if lazy else 1) + + assert custom.data.info["pose"] == 0.0 + renderer.render.assert_not_called() + if lazy: + custom_updates.assert_called_once_with() + renderer.render_batch.assert_called_once_with([custom._render_data]) + else: + assert renderer.render_batch.call_args_list == [call([custom._render_data]), call([peer._render_data])] + + +def test_camera_batch_respects_period_partial_reset_and_updated_pose(camera_batch_context): + """A fresh peer stays cached while due or reset cameras capture their current poses.""" + _, renderer, batches = camera_batch_context + fast = _CpuCamera(renderer, "fast") + slow = _CpuCamera(renderer, "slow", update_period=1.0) + scene = _camera_scene([fast, slow]) + + InteractiveScene.update(scene, 0.1) + InteractiveScene.update(scene, 0.2) + assert batches == [[("fast", 0.0), ("slow", 0.0)], [("fast", 0.0)]] + np.testing.assert_array_equal(slow.frame.warp.numpy(), [1, 1]) + np.testing.assert_allclose(slow._timestamp_last_update.numpy(), [0.1, 0.1]) + + slow.pose = 3.0 + slow.reset(env_mask=wp.array([False, True], dtype=wp.bool, device="cpu")) + InteractiveScene.update(scene, 0.1) + assert batches[-1] == [("fast", 0.0), ("slow", 3.0)] + assert slow.data.info["pose"] == 3.0 + np.testing.assert_array_equal(slow.frame.warp.numpy(), [1, 1]) + np.testing.assert_allclose(slow._timestamp.numpy(), [0.4, 0.1]) + np.testing.assert_allclose(slow._timestamp_last_update.numpy(), [0.1, 0.1]) + + InteractiveScene.update(scene, 0.9) + np.testing.assert_array_equal(slow.frame.warp.numpy(), [2, 1]) + np.testing.assert_allclose(slow._timestamp_last_update.numpy(), [1.3, 0.1]) + renderer.render.assert_not_called() + + +@pytest.mark.parametrize("failure", ["render_batch", "read_output"]) +def test_failed_eager_capture_can_retry_one_camera(camera_batch_context, failure): + """Failed captures leave timestamps outdated, and lazy retries do not capture peer cameras.""" + _, renderer, batches = camera_batch_context + cameras = [_CpuCamera(renderer, "wide"), _CpuCamera(renderer, "tele")] + scene = _camera_scene(cameras) + original = getattr(renderer, failure) + + setattr(renderer, failure, Mock(side_effect=RuntimeError("capture failed"))) + with pytest.raises(RuntimeError, match="capture failed"): + InteractiveScene.update(scene, 0.1) + for camera in cameras: + np.testing.assert_array_equal(camera._timestamp_last_update.numpy(), [0.0, 0.0]) + setattr(renderer, failure, original) + batches.clear() + + assert cameras[0].data.info["pose"] == 0.0 + assert batches == [[("wide", 0.0)]] + np.testing.assert_allclose(cameras[0]._timestamp_last_update.numpy(), [0.1, 0.1]) + np.testing.assert_array_equal(cameras[1]._timestamp_last_update.numpy(), [0.0, 0.0]) diff --git a/source/isaaclab/test/sensors/joint_wrench_contract.py b/source/isaaclab/test/sensors/joint_wrench_contract.py new file mode 100644 index 000000000000..cb82786cdcea --- /dev/null +++ b/source/isaaclab/test/sensors/joint_wrench_contract.py @@ -0,0 +1,77 @@ +# 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 + +"""Joint-wrench contract tests imported by every backend's sensor test module. + +Backend modules supply the ``sim`` fixture; this test owns the physical scene and oracle. +""" + +from pathlib import Path + +import pytest +import torch + +from pxr import Gf, Usd, UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import JointWrenchSensorCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, retrieve_file_path + + +@pytest.mark.integration +def test_joint_wrench_frame(sim, tmp_path: Path) -> None: + """A rotated, offset joint reports the analytic gravity reaction at its anchor.""" + source = retrieve_file_path(f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd") + usd_path = str(tmp_path / "joint_wrench.usda") + stage = Usd.Stage.CreateNew(usd_path) + root = stage.DefinePrim("/Articulation", "Xform") + stage.SetDefaultPrim(root) + root.GetReferences().AddReference(source) + joint = next(UsdPhysics.Joint(prim) for prim in stage.Traverse() if prim.IsA(UsdPhysics.RevoluteJoint)) + UsdPhysics.RevoluteJoint(joint).GetAxisAttr().Set("Z") + joint.GetLocalPos1Attr().Set(Gf.Vec3f(0.25, -0.15, 0.1)) + joint.GetLocalRot1Attr().Set(Gf.Quatf(2.0**-0.5, Gf.Vec3f(2.0**-0.5, 0.0, 0.0))) + # Unit scale makes the authored joint offset a metric offset. Align the joint frames initially. + arm_prim = stage.GetPrimAtPath(joint.GetBody1Rel().GetTargets()[0]) + pose = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(1.0, 0.0, 0.0), -90.0)) + pose.SetTranslateOnly(Gf.Vec3d(-0.25, -0.1, -0.15)) + UsdGeom.Xformable(arm_prim).MakeMatrixXform().Set(pose) + mass = UsdPhysics.MassAPI.Apply(arm_prim) + mass.CreateMassAttr(2.0) + mass.CreateCenterOfMassAttr(Gf.Vec3f(0.0)) + for prim in stage.Traverse(): + if prim.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Set(False) + stage.GetRootLayer().Save() + + cfg = InteractiveSceneCfg(num_envs=1, env_spacing=2.0) + cfg.robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg(usd_path=usd_path), + actuators={"joint": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0)}, + init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0), rot=(0.0, 0.0, 2.0**-0.5, 2.0**-0.5)), + ) + cfg.wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + scene = InteractiveScene(cfg) + sim.reset() + for _ in range(20): + sim.step() + scene.update(sim.get_physics_dt()) + + robot, sensor = scene["robot"], scene["wrench"] + arm = robot.body_names.index("Arm") + sensor_arm = sensor.find_bodies("Arm")[0][0] + assert robot.data.body_com_vel_w.torch[:, arm].norm() < 1e-3 + # The joint frame is rotated 90 degrees about world Z, so gravity still points along its -Z. + # Its 2 kg load is offset by (-0.25, -0.1, -0.15) m in joint coordinates. + # Reaction force is (0, 0, mg), and r x F gives torque (-0.1 mg, 0.25 mg, 0). + weight = -2.0 * sim.cfg.gravity[2] + expected_force = torch.tensor([[0.0, 0.0, weight]], device=sim.device) + expected_torque = torch.tensor([[-0.1 * weight, 0.25 * weight, 0.0]], device=sim.device) + torch.testing.assert_close(sensor.data.force.torch[:, sensor_arm], expected_force, atol=1e-2, rtol=1e-3) + torch.testing.assert_close(sensor.data.torque.torch[:, sensor_arm], expected_torque, atol=1e-2, rtol=1e-3) diff --git a/source/isaaclab/test/sensors/test_joint_wrench_contract.py b/source/isaaclab/test/sensors/test_joint_wrench_contract.py new file mode 100644 index 000000000000..7f0fdff5dab5 --- /dev/null +++ b/source/isaaclab/test/sensors/test_joint_wrench_contract.py @@ -0,0 +1,48 @@ +# 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 + +"""Ensure every joint-wrench backend collects the same physical contract tests.""" + +import ast + +import pytest + +pytestmark = pytest.mark.unit + + +def test_joint_wrench_contract_registration(source_checkout_root): + source = source_checkout_root / "source" + contract = source / "isaaclab/test/sensors/joint_wrench_contract.py" + tree = ast.parse(contract.read_text()) + tests = {node.name for node in tree.body if isinstance(node, ast.FunctionDef) and node.name.startswith("test_")} + assert tests + assert not (source / "isaaclab/isaaclab/test/utils/joint_wrench.py").exists() + + implementations = list(source.glob("isaaclab_*/isaaclab_*/sensors/joint_wrench/joint_wrench_sensor.py")) + backend_packages = {path.parents[2].name for path in implementations} + assert backend_packages >= {"isaaclab_newton", "isaaclab_physx", "isaaclab_ov"} + for node in ast.walk(tree): + if isinstance(node, ast.Import): + modules = [alias.name for alias in node.names] + elif isinstance(node, ast.ImportFrom): + modules = [node.module or ""] + else: + continue + assert not {module.split(".")[0] for module in modules} & (backend_packages | {"newton", "ovphysx", "isaacsim"}) + + for implementation in implementations: + suite = implementation.parents[3] / "test/sensors/test_joint_wrench_sensor.py" + backend_tree = ast.parse(suite.read_text()) + imported_tests = { + alias.name + for node in backend_tree.body + if isinstance(node, ast.ImportFrom) and node.module == "joint_wrench_contract" + for alias in node.names + if alias.asname is None + } + local_functions = {node.name for node in backend_tree.body if isinstance(node, ast.FunctionDef)} + assert tests <= imported_tests, f"{suite}: missing shared contract tests" + assert not tests & local_functions, f"{suite}: shadows shared contract tests" + assert "test_non_identity_joint_frame_transform" not in local_functions diff --git a/source/isaaclab_newton/changelog.d/shared-joint-wrench-contract.skip b/source/isaaclab_newton/changelog.d/shared-joint-wrench-contract.skip new file mode 100644 index 000000000000..a5378c233af7 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/shared-joint-wrench-contract.skip @@ -0,0 +1 @@ +Collected the shared joint-wrench frame test directly with the existing Newton simulation fixture. diff --git a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py index 9353b1f0831d..dd54dbb9ea98 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -10,6 +10,7 @@ from unittest.mock import Mock sys.path.insert(0, str(Path(__file__).resolve().parents[1])) +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sensors")) import newton import numpy as np @@ -18,6 +19,7 @@ import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_physx.sim.schemas import PhysxJointCfg +from joint_wrench_contract import test_joint_wrench_frame # noqa: F401 from pxr import Usd, UsdPhysics @@ -28,7 +30,6 @@ from isaaclab.sensors.joint_wrench import JointWrenchSensor, JointWrenchSensorCfg from isaaclab.sim import SimulationCfg from isaaclab.terrains import TerrainImporterCfg -from isaaclab.test.utils.joint_wrench import check_joint_wrench_frame from isaaclab.utils import configclass from isaaclab.utils import math as math_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path @@ -348,11 +349,6 @@ def test_force_and_torque_components_at_rest(sim): torch.testing.assert_close(torque, expected_torque, atol=1e-2, rtol=1e-3) -def test_non_identity_joint_frame_transform(tmp_path): - """Newton must satisfy the same physical joint-frame contract as PhysX.""" - check_joint_wrench_frame(NewtonCfg(solver_cfg=MJWarpSolverCfg(), num_substeps=1), tmp_path) - - def test_wrench_with_external_force_and_torque(sim): """Full analytical wrench validation with external force and torque applied. diff --git a/source/isaaclab_ov/changelog.d/fix-joint-wrench-frame.rst b/source/isaaclab_ov/changelog.d/fix-joint-wrench-frame.rst new file mode 100644 index 000000000000..7a1119c2e80e --- /dev/null +++ b/source/isaaclab_ov/changelog.d/fix-joint-wrench-frame.rst @@ -0,0 +1,7 @@ +Fixed +^^^^^ + +* Fixed OVPhysX joint-wrench sensors applying an extra frame transformation to readings that are already + expressed in the child-side joint frame at the joint anchor, and removed the redundant USD frame buffers. + Force and torque values changed for joints with non-identity child frames; the documented frame + convention is unchanged. diff --git a/source/isaaclab_ov/changelog.d/ovrtx-render-batch.minor.rst b/source/isaaclab_ov/changelog.d/ovrtx-render-batch.minor.rst new file mode 100644 index 000000000000..b7bbdd0e30e7 --- /dev/null +++ b/source/isaaclab_ov/changelog.d/ovrtx-render-batch.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added an OVRTX ``render_batch()`` implementation that submitted all requested camera + products in one native renderer step while preserving the single-camera ``render()`` interface. diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py index 2fa6b26c3b98..398b06002bc4 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py @@ -106,7 +106,7 @@ if TYPE_CHECKING: from isaaclab_ppisp import PpispPipeline - from ovrtx import AttributeBinding + from ovrtx import AttributeBinding, RenderProductSetOutputs from isaaclab.renderers.base_renderer import VisualMaterialBatch from isaaclab.sensors.camera.camera_data import CameraData @@ -1607,8 +1607,8 @@ def _process_render_frame(self, render_data: OVRTXCameraRenderData, frame, outpu with self._map_render_var_to_dlpack(motion_var) as tiled_motion_vectors_data: self._launch_extract_all_tiles(render_data, tiled_motion_vectors_data, output_buffers["motion_vectors"]) - def _render_legacy(self, render_data: OVRTXCameraRenderData) -> None: - """Render the scene into the provided RenderData.""" + def _render_legacy(self, render_data: Sequence[OVRTXCameraRenderData]) -> None: + """Render the requested camera products in one native submission.""" if not self._initialized_scene: raise RuntimeError("Scene not initialized. Call initialize() first.") if self.backend.renderer is None or len(self._render_product_paths) == 0: @@ -1618,7 +1618,7 @@ def _render_legacy(self, render_data: OVRTXCameraRenderData) -> None: if material_writer is not None: material_writer.publish() products = self.backend.renderer.step( - render_products={render_data.render_product_path}, + render_products={data.render_product_path for data in render_data}, delta_time=1.0 / 60.0, ) finally: @@ -1626,21 +1626,28 @@ def _render_legacy(self, render_data: OVRTXCameraRenderData) -> None: drain_errors = contextlib.nullcontext() if sys.exc_info()[0] is None else contextlib.suppress(Exception) with drain_errors: material_writer.drain() - product_path = render_data.render_product_path - if product_path in products and len(products[product_path].frames) > 0: + self._process_render_products(render_data, products) + + def _process_render_products( + self, render_data: Sequence[OVRTXCameraRenderData], products: RenderProductSetOutputs + ) -> None: + """Populate camera outputs only after every requested product returned a frame.""" + for data in render_data: + if data.render_product_path not in products or not products[data.render_product_path].frames: + raise RuntimeError(f"OVRTX returned no frame for render product {data.render_product_path!r}.") + for data in render_data: self._process_render_frame( - render_data, - products[product_path].frames[0], - render_data.warp_buffers, + data, + products[data.render_product_path].frames[0], + data.warp_buffers, ) - # Post-render PPISP: HDR scene-linear → LDR RGBA. Source/destination - # buffers are the same warp buffer map used by extraction. - if render_data.ppisp_pipeline is not None: - render_data.ppisp_pipeline.apply( - render_data.warp_buffers[str(RenderBufferKind.RGB_HDR)], - render_data.warp_buffers[str(RenderBufferKind.RGBA)], - ) + # Post-render PPISP uses each camera's own HDR source and RGBA destination. + if data.ppisp_pipeline is not None: + data.ppisp_pipeline.apply( + data.warp_buffers[str(RenderBufferKind.RGB_HDR)], + data.warp_buffers[str(RenderBufferKind.RGBA)], + ) def _close_legacy(self) -> None: """Release the renderer's tensor bindings. See :meth:`close`.""" @@ -1802,7 +1809,21 @@ def update_camera_intrinsics(self, render_data: OVRTXCameraRenderData, intrinsic operation.wait() def render(self, render_data: OVRTXCameraRenderData) -> None: - """Render the scene into the provided RenderData.""" + """Render one camera product into its bound output buffers.""" + self.render_batch((render_data,)) + + def render_batch(self, render_data: Sequence[OVRTXCameraRenderData]) -> None: + """Render all requested camera products in one native submission. + + Args: + render_data: Cameras whose poses and output buffers have been prepared. An empty + sequence performs no work. + + Raises: + RuntimeError: If the scene is uninitialized or a requested product returns no frame. + """ + if not render_data: + return if self._use_ovstage: self._render_ovstage(render_data) else: @@ -2425,7 +2446,7 @@ def _update_camera_ovstage( cuda_stream=self._warp_device.stream.cuda_stream, ).wait() - def _render_ovstage(self, render_data: OVRTXCameraRenderData) -> None: + def _render_ovstage(self, render_data: Sequence[OVRTXCameraRenderData]) -> None: if not self._initialized_scene: raise RuntimeError("Scene not initialized. Call initialize() first.") if self.backend.renderer is None or len(self._render_product_paths) == 0: @@ -2443,26 +2464,12 @@ def _render_ovstage(self, render_data: OVRTXCameraRenderData) -> None: with drain_errors: material_writer.drain() products = self.backend.renderer.step( - render_products={render_data.render_product_path}, + render_products={data.render_product_path for data in render_data}, delta_time=1.0 / 60.0, ordinal=self._current_ordinal, ) self._current_ordinal += 1 - product_path = render_data.render_product_path - if product_path in products and len(products[product_path].frames) > 0: - self._process_render_frame( - render_data, - products[product_path].frames[0], - render_data.warp_buffers, - ) - - # Post-render PPISP: HDR scene-linear → LDR RGBA. Source/destination - # buffers are the same warp buffer map used by extraction. - if render_data.ppisp_pipeline is not None: - render_data.ppisp_pipeline.apply( - render_data.warp_buffers[str(RenderBufferKind.RGB_HDR)], - render_data.warp_buffers[str(RenderBufferKind.RGBA)], - ) + self._process_render_products(render_data, products) def _close_ovstage(self) -> None: """Release the renderer's stage queries and path lists. See :meth:`close`.""" diff --git a/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/joint_wrench_sensor.py index d1d5825cf483..f5e1e62f0124 100644 --- a/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/joint_wrench_sensor.py @@ -12,10 +12,9 @@ from collections.abc import Sequence from typing import TYPE_CHECKING, Any -import numpy as np import warp as wp -from pxr import Usd, UsdPhysics +from pxr import UsdPhysics from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor from isaaclab.sim.utils.queries import find_first_matching_prim, get_all_matching_child_prims, path_expr_to_glob @@ -67,8 +66,6 @@ def __init__(self, cfg: JointWrenchSensorCfg): self._root_view: OvPhysxView | None = None self._wrench_binding: Any = None self._wrench_buf: wp.array | None = None - self._joint_pos_b: wp.array | None = None - self._joint_quat_b: wp.array | None = None self._num_bodies: int = 0 def __str__(self) -> str: @@ -158,8 +155,6 @@ def _initialize_impl(self) -> None: self._timestamp = wp.zeros(self._num_envs, dtype=wp.float32, device=self._device) self._timestamp_last_update = wp.zeros_like(self._timestamp) - self._create_joint_frame_buffers() - # Wrench storage as (N, L) spatial_vectorf, read each step via the view. The view # reinterprets this structured buffer off the binding's flat float32 shape and caches # that reinterpret per destination buffer, so no manual float32 alias is needed here. @@ -198,45 +193,6 @@ def _resolve_articulation_root_prim_path(self) -> str: root_prim_path_relative_to_prim_path = first_env_root_prim_path[len(first_env_matching_prim_path) :] return self.cfg.prim_path + root_prim_path_relative_to_prim_path - def _create_joint_frame_buffers(self) -> None: - """Create child-side joint frame transforms indexed by OVPhysX link order.""" - joint_pos_b = np.zeros((self._num_bodies, 3), dtype=np.float32) - joint_quat_b = np.zeros((self._num_bodies, 4), dtype=np.float32) - joint_quat_b[:, 3] = 1.0 - - first_env_matching_prim = find_first_matching_prim(self.cfg.prim_path) - if first_env_matching_prim is None: - raise RuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.") - link_name_to_index = {name: index for index, name in enumerate(self._data._body_names)} - - for prim in Usd.PrimRange(first_env_matching_prim): - joint = UsdPhysics.Joint(prim) - if not joint or joint.GetJointEnabledAttr().Get() is False: - continue - body1_targets = joint.GetBody1Rel().GetTargets() - if len(body1_targets) == 0: - continue - body_index = link_name_to_index.get(body1_targets[0].name) - if body_index is None: - continue - - local_pos1 = joint.GetLocalPos1Attr().Get() - if local_pos1 is not None: - joint_pos_b[body_index] = (float(local_pos1[0]), float(local_pos1[1]), float(local_pos1[2])) - - local_rot1 = joint.GetLocalRot1Attr().Get() - if local_rot1 is not None: - local_rot1_imag = local_rot1.GetImaginary() - joint_quat_b[body_index] = ( - float(local_rot1_imag[0]), - float(local_rot1_imag[1]), - float(local_rot1_imag[2]), - float(local_rot1.GetReal()), - ) - - self._joint_pos_b = wp.array(joint_pos_b, dtype=wp.vec3f, device=self._device) - self._joint_quat_b = wp.array(joint_quat_b, dtype=wp.quatf, device=self._device) - def _update_buffers_impl(self, env_mask: wp.array) -> None: """Read OVPhysX incoming joint wrenches and split them into force / torque buffers. @@ -248,8 +204,6 @@ def _update_buffers_impl(self, env_mask: wp.array) -> None: f"Joint wrench sensor '{self.cfg.prim_path}': not initialized." " Access sensor data only after sim.reset() has been called." ) - if self._joint_pos_b is None or self._joint_quat_b is None: - raise RuntimeError(f"Joint wrench sensor '{self.cfg.prim_path}': joint frame buffers are not initialized.") self._root_view.read_into(TT.LINK_INCOMING_JOINT_FORCE, self._wrench_buf) wp.launch( @@ -258,8 +212,6 @@ def _update_buffers_impl(self, env_mask: wp.array) -> None: inputs=[ env_mask, self._wrench_buf, - self._joint_pos_b, - self._joint_quat_b, self._timestamp, self._data._force, self._data._torque, @@ -281,8 +233,6 @@ def _invalidate_initialize_callback(self, event) -> None: self._wrench_binding = None self._physx_instance = None self._wrench_buf = None - self._joint_pos_b = None - self._joint_quat_b = None self._num_bodies = 0 self._data._force = None self._data._torque = None diff --git a/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/kernels.py b/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/kernels.py index 0badb4296135..0dc0791cc8eb 100644 --- a/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/kernels.py +++ b/source/isaaclab_ov/isaaclab_ov/sensors/joint_wrench/kernels.py @@ -11,20 +11,17 @@ def joint_wrench_split_kernel( # inputs env_mask: wp.array(dtype=wp.bool), incoming_joint_wrench: wp.array(dtype=wp.spatial_vectorf, ndim=2), - joint_pos_b: wp.array(dtype=wp.vec3f), - joint_quat_b: wp.array(dtype=wp.quatf), timestamp: wp.array(dtype=wp.float32), # outputs out_force: wp.array(dtype=wp.vec3f, ndim=2), out_torque: wp.array(dtype=wp.vec3f, ndim=2), ): - """Convert OVPhysX incoming joint spatial wrenches into the child-side joint frame. + """Split OVPhysX wrenches, already expressed at the child-side joint anchor in its frame. Args: env_mask: Boolean mask selecting which environments to update. - incoming_joint_wrench: Incoming joint spatial wrenches in child body frame ``(num_envs, num_bodies)``. - joint_pos_b: Child-side joint anchor positions in child body frame [m] ``(num_bodies,)``. - joint_quat_b: Child-side joint frame orientations in child body frame ``(num_bodies,)``. + incoming_joint_wrench: Incoming joint spatial wrenches in the child-side joint frame, referenced at the + joint anchor ``(num_envs, num_bodies)``. timestamp: Current sensor timestamp per environment [s] ``(num_envs,)``. out_force: Output force in child-side joint frame [N] ``(num_envs, num_bodies)``. out_torque: Output torque in child-side joint frame [N·m] ``(num_envs, num_bodies)``. @@ -39,15 +36,8 @@ def joint_wrench_split_kernel( return wrench = incoming_joint_wrench[env, body] - force_b = wp.spatial_top(wrench) - torque_b = wp.spatial_bottom(wrench) - - # OVPhysX wraps PhysX and reports the wrench in body1's frame, referenced at body1's origin. - # Shift torque to the child-side joint anchor and rotate both components - # into the child-side joint frame. - torque_joint_anchor_b = torque_b - wp.cross(joint_pos_b[body], force_b) - out_force[env, body] = wp.quat_rotate_inv(joint_quat_b[body], force_b) - out_torque[env, body] = wp.quat_rotate_inv(joint_quat_b[body], torque_joint_anchor_b) + out_force[env, body] = wp.spatial_top(wrench) + out_torque[env, body] = wp.spatial_bottom(wrench) @wp.kernel diff --git a/source/isaaclab_ov/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_ov/test/sensors/test_joint_wrench_sensor.py index 7285d7ee0505..232e731b5275 100644 --- a/source/isaaclab_ov/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_ov/test/sensors/test_joint_wrench_sensor.py @@ -22,20 +22,22 @@ from __future__ import annotations -import math +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sensors")) import pytest import torch import warp as wp -from pxr import Gf, UsdPhysics - # OVRTX-only CI jobs collect the consolidated isaaclab_ov test suite without # the optional ovphysx wheel. Skip the OVPhysX tests gracefully in that case. pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") from isaaclab_ov.physics import OvPhysxCfg # noqa: E402 from isaaclab_physx.sim.schemas import PhysxJointCfg # noqa: E402 +from joint_wrench_contract import test_joint_wrench_frame # noqa: E402, F401 import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.actuators import ImplicitActuatorCfg # noqa: E402 @@ -45,13 +47,14 @@ from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 from isaaclab.terrains import TerrainImporterCfg # noqa: E402 from isaaclab.utils import configclass # noqa: E402 -from isaaclab.utils import math as math_utils # noqa: E402 from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR # noqa: E402 from isaaclab_assets.robots.ant import ANT_CFG # noqa: E402 wp.init() +pytestmark = pytest.mark.device_split + # OVPhysX/Warp and the PyTorch reference use different float32 operation order on CUDA. The # relative gap grows with the wrench magnitude: the Ant scenes see contact wrenches on the order # of 1e8, where the two orderings differ by a few 1e-5 relative. @@ -195,6 +198,12 @@ def sim(device): yield sim_ctx +@pytest.fixture(params=["cuda:0", "cpu"]) +def device(request): + """Supply the device to imported contract tests as well as backend-local tests.""" + return request.param + + # --------------------------------------------------------------------------- # Raw-tensor helpers # --------------------------------------------------------------------------- @@ -223,55 +232,16 @@ def _ovphysx_incoming_joint_wrench(sensor: JointWrenchSensor) -> torch.Tensor: def _assert_sensor_matches_ovphysx_tensor(sensor: JointWrenchSensor) -> None: - """Compare sensor buffers to the raw OVPhysX tensor transformed into joint frames.""" + """The sensor exposes the OVPhysX tensor's existing child-joint-frame components.""" raw_wrench = _ovphysx_incoming_joint_wrench(sensor) - sensor_data = sensor.data - - expected_force, expected_torque = _ovphysx_incoming_joint_wrench_in_joint_frame(sensor, raw_wrench) torch.testing.assert_close( - sensor_data.force.torch, expected_force, rtol=_OVPHYSX_WRENCH_RTOL, atol=_OVPHYSX_WRENCH_ATOL + sensor.data.force.torch, raw_wrench[..., :3], rtol=_OVPHYSX_WRENCH_RTOL, atol=_OVPHYSX_WRENCH_ATOL ) torch.testing.assert_close( - sensor_data.torque.torch, expected_torque, rtol=_OVPHYSX_WRENCH_RTOL, atol=_OVPHYSX_WRENCH_ATOL + sensor.data.torque.torch, raw_wrench[..., 3:], rtol=_OVPHYSX_WRENCH_RTOL, atol=_OVPHYSX_WRENCH_ATOL ) -def _ovphysx_incoming_joint_wrench_in_joint_frame( - sensor: JointWrenchSensor, raw_wrench: torch.Tensor -) -> tuple[torch.Tensor, torch.Tensor]: - """Transform raw OVPhysX body-frame incoming joint wrenches into the configured convention.""" - force_b = raw_wrench[..., :3] - torque_b = raw_wrench[..., 3:] - joint_pos_b = wp.to_torch(sensor._joint_pos_b).unsqueeze(0) - joint_quat_b = wp.to_torch(sensor._joint_quat_b).unsqueeze(0) - torque_joint_anchor_b = torque_b - torch.cross(joint_pos_b.expand_as(force_b), force_b, dim=-1) - - flat_joint_quat_b = joint_quat_b.expand_as(raw_wrench[..., :4]).reshape(-1, 4) - expected_force = math_utils.quat_apply_inverse(flat_joint_quat_b, force_b.reshape(-1, 3)).reshape(force_b.shape) - expected_torque = math_utils.quat_apply_inverse(flat_joint_quat_b, torque_joint_anchor_b.reshape(-1, 3)).reshape( - torque_b.shape - ) - return expected_force, expected_torque - - -def _set_child_joint_frame(scene: InteractiveScene, child_body_name: str) -> None: - """Set a non-identity child-side joint frame for the requested body in env 0.""" - for prim in scene.stage.Traverse(): - if not prim.GetPath().pathString.startswith("/World/envs/env_0/Robot"): - continue - joint = UsdPhysics.Joint(prim) - if joint and any(target.name == child_body_name for target in joint.GetBody1Rel().GetTargets()): - joint.GetLocalPos1Attr().Set(Gf.Vec3f(0.25, -0.15, 0.1)) - joint.GetLocalRot1Attr().Set( - Gf.Quatf( - math.cos(math.pi / 4.0), - Gf.Vec3f(math.sin(math.pi / 4.0), 0.0, 0.0), - ) - ) - return - raise RuntimeError(f"Failed to find a USD joint with child body '{child_body_name}'.") - - # --------------------------------------------------------------------------- # Sensor data — pre-init contract # --------------------------------------------------------------------------- @@ -372,36 +342,6 @@ def test_force_and_torque_components_at_rest(sim, device): assert torch.any(raw_wrench[:, arm_idx, :] != 0.0) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_non_identity_joint_frame_transform(sim, device): - """OVPhysX raw body-frame wrench is converted to the child-side joint frame.""" - scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) - _set_child_joint_frame(scene, "Arm") - sim.reset() - - sensor: JointWrenchSensor = scene["wrench"] - robot: Articulation = scene["robot"] - arm_idx = robot.body_names.index("Arm") - - for _ in range(400): - sim.step() - scene.update(sim.get_physics_dt()) - - raw_wrench = _ovphysx_incoming_joint_wrench(sensor) - expected_force, expected_torque = _ovphysx_incoming_joint_wrench_in_joint_frame(sensor, raw_wrench) - torch.testing.assert_close( - sensor.data.force.torch, expected_force, rtol=_OVPHYSX_WRENCH_RTOL, atol=_OVPHYSX_WRENCH_ATOL - ) - torch.testing.assert_close( - sensor.data.torque.torch, expected_torque, rtol=_OVPHYSX_WRENCH_RTOL, atol=_OVPHYSX_WRENCH_ATOL - ) - - raw_force = raw_wrench[:, arm_idx, :3] - raw_torque = raw_wrench[:, arm_idx, 3:] - assert not torch.allclose(sensor.data.force.torch[:, arm_idx], raw_force) - assert not torch.allclose(sensor.data.torque.torch[:, arm_idx], raw_torque) - - @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) def test_wrench_with_external_force_and_torque(sim, device): """Full wrench validation with external force and torque applied.""" diff --git a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py index e3dce5bdd66b..12e1d319237c 100644 --- a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py +++ b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py @@ -164,6 +164,79 @@ def test_ovrtx_supported_output_types_key_set(): assert specs[RenderBufferKind.MOTION_VECTORS] == RenderBufferSpec(2, wp.float32) +@pytest.mark.parametrize("use_ovstage", [False, True]) +@pytest.mark.parametrize("missing_output", [None, "product", "frame"]) +@pytest.mark.parametrize("batch", [False, True]) +def test_ovrtx_render_submits_requested_products_and_routes_outputs(monkeypatch, use_ovstage, missing_output, batch): + """A submission fills each camera from its product and rejects incomplete results.""" + renderer = _make_ovrtx_renderer_without_backend() + renderer._use_ovstage = use_ovstage + renderer._initialized_scene = True + renderer._visual_material_writer_ref = None + renderer._current_ordinal = 7 + cameras = [_make_ovrtx_camera_render_data() for _ in range(2 if batch else 1)] + products = {} + processed = [] + postprocessed = [] + submissions = [] + published_ordinals = [] + for index, camera in enumerate(cameras): + camera.render_product_path = f"/Render/Camera{index}" + camera.warp_buffers = {str(RenderBufferKind.RGB_HDR): object(), str(RenderBufferKind.RGBA): object()} + camera.ppisp_pipeline = types.SimpleNamespace(apply=lambda *buffers: postprocessed.append(buffers)) + products[camera.render_product_path] = types.SimpleNamespace(frames=[object()]) + renderer._render_product_paths = [*products, "/Render/UnrequestedCamera"] + if missing_output == "product": + del products[cameras[-1].render_product_path] + elif missing_output == "frame": + products[cameras[-1].render_product_path].frames.clear() + + def step(**kwargs): + submissions.append(kwargs) + return products + + def advance_write_floor(*, ordinal): + published_ordinals.append(ordinal) + return types.SimpleNamespace(wait=lambda: None) + + renderer.backend.renderer = types.SimpleNamespace(step=step) + renderer.backend.stage = types.SimpleNamespace(advance_write_floor=advance_write_floor) + monkeypatch.setattr(renderer, "_process_render_frame", lambda *args: processed.append(args)) + + render = renderer.render_batch if batch else renderer.render + request = cameras if batch else cameras[0] + if missing_output is None: + render(request) + assert processed == [ + (camera, products[camera.render_product_path].frames[0], camera.warp_buffers) for camera in cameras + ] + assert postprocessed == [ + (camera.warp_buffers[str(RenderBufferKind.RGB_HDR)], camera.warp_buffers[str(RenderBufferKind.RGBA)]) + for camera in cameras + ] + else: + with pytest.raises(RuntimeError, match=cameras[-1].render_product_path): + render(request) + assert not processed + assert not postprocessed + + assert len(submissions) == 1 + assert submissions[0]["render_products"] == {camera.render_product_path for camera in cameras} + if use_ovstage: + assert submissions[0]["ordinal"] == 7 + assert published_ordinals == [7] + assert renderer._current_ordinal == 8 + else: + assert "ordinal" not in submissions[0] + assert not published_ordinals + + +def test_ovrtx_render_batch_empty_sequence_does_not_require_initialized_backend(): + """An empty render request has no backend work or initialization precondition.""" + renderer = OVRTXRenderer.__new__(OVRTXRenderer) + renderer.render_batch([]) + + @pytest.mark.integration @pytest.mark.rendering @pytest.mark.parametrize("use_ovstage", [False, True]) @@ -228,7 +301,6 @@ def camera_scope_exists(rd): return any(path.startswith(scope) for path in renderer.backend.renderer.query_prims()) def check_depth(rd, data, expected, label): - renderer.render(rd) depth = data.output["distance_to_image_plane"].torch for env_id in range(rd.num_envs): prefix = tmp_path / f"{label}_env{env_id}" @@ -271,6 +343,7 @@ def check_depth(rd, data, expected, label): renderer.set_outputs(rd, data.output) cameras.append((rd, data)) # Register the next camera after rendering has already started. + renderer.render(rd) check_depth(rd, data, 5.0 - index - 0.5, f"initial_cam{index}") if index == 1: normals = data.output["normals"].torch[:, height // 2, width // 2, :3] @@ -285,11 +358,13 @@ def check_depth(rd, data, expected, label): ) orientations = ProxyArray(wp.from_torch(quats, dtype=wp.quatf)) renderer.update_camera(cameras[1][0], positions, orientations, cameras[1][1].intrinsic_matrices) + renderer.render_batch([rd for rd, _ in cameras]) check_depth(*cameras[0], 4.5, "after_move_cam0") check_depth(*cameras[1], 5.5, "after_move_cam1") assert all(camera_scope_exists(rd) for rd, _ in cameras) renderer.cleanup(cameras[0][0]) assert not camera_scope_exists(cameras[0][0]) + renderer.render(cameras[1][0]) check_depth(*cameras[1], 5.5, "after_cleanup_cam1") renderer.cleanup(cameras[1][0]) renderer.cleanup(cameras[1][0]) diff --git a/source/isaaclab_ov/test/test_ovrtx_visual_material_sync.py b/source/isaaclab_ov/test/test_ovrtx_visual_material_sync.py index a7b502350e6d..80dc51074e5a 100644 --- a/source/isaaclab_ov/test/test_ovrtx_visual_material_sync.py +++ b/source/isaaclab_ov/test/test_ovrtx_visual_material_sync.py @@ -51,7 +51,7 @@ def bind_attribute(self, prim_paths, attribute_name, **kwargs): def step(self, **kwargs): self.events.append("step") - return {} + return {path: SimpleNamespace(frames=[object()]) for path in kwargs["render_products"]} class _NativeBinding: @@ -205,7 +205,8 @@ def test_ovstage_compiles_queries_and_publishes_selected_channel_zero_copy(): ) def test_render_publishes_and_drains_material_writes_at_backend_boundary(use_ovstage, expected_events): renderer, events = _renderer(use_ovstage=use_ovstage) - renderer._render_product_paths = ["/RenderCamera_0/Product"] + renderer._render_product_paths = ["/Render/Product0", "/Render/Product1"] + renderer._process_render_frame = lambda *args: None class Writer: def publish(self): @@ -216,8 +217,12 @@ def drain(self): writer = Writer() renderer._visual_material_writer_ref = lambda: writer - render = renderer._render_ovstage if use_ovstage else renderer._render_legacy - render(SimpleNamespace(render_product_path="/RenderCamera_0/Product", ppisp_pipeline=None)) + renderer.render_batch( + [ + SimpleNamespace(render_product_path=path, ppisp_pipeline=None, warp_buffers={}) + for path in renderer._render_product_paths + ] + ) assert events == expected_events @@ -249,7 +254,7 @@ def advance_write_floor(**_kwargs): renderer.backend.stage.advance_write_floor = advance_write_floor with pytest.raises(ValueError, match=failure): - renderer._render_ovstage(SimpleNamespace(render_product_path="/RenderCamera_0/Product", ppisp_pipeline=None)) + renderer.render(SimpleNamespace(render_product_path="/RenderCamera_0/Product", ppisp_pipeline=None)) assert events == expected_events diff --git a/source/isaaclab_physx/changelog.d/isaac-rtx-render-batch.rst b/source/isaaclab_physx/changelog.d/isaac-rtx-render-batch.rst new file mode 100644 index 000000000000..2c604eaa95e1 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/isaac-rtx-render-batch.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Avoided repeated Isaac RTX render-update checks by checking once per camera batch before + extracting each camera's annotator outputs. diff --git a/source/isaaclab_physx/changelog.d/shared-joint-wrench-contract.skip b/source/isaaclab_physx/changelog.d/shared-joint-wrench-contract.skip new file mode 100644 index 000000000000..fed859c9ff19 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/shared-joint-wrench-contract.skip @@ -0,0 +1 @@ +Collected the shared joint-wrench frame test directly with the existing PhysX simulation fixture. diff --git a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py index 011e0b55f093..de60212345bb 100644 --- a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py +++ b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer.py @@ -11,6 +11,7 @@ import logging import math import uuid +from collections.abc import Sequence from dataclasses import dataclass, field from typing import TYPE_CHECKING, Any, NoReturn @@ -612,12 +613,19 @@ def update_camera_intrinsics(self, render_data: IsaacRtxRenderData, intrinsics: device=parameters.device, ) - def render(self, render_data: IsaacRtxRenderData): - """Extract data from annotators and write to output buffers. - See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.render`.""" - spec = render_data.spec - output_data = render_data.output_data - if output_data is None or spec is None: + def render(self, render_data: IsaacRtxRenderData) -> None: + """Render one camera product into its bound output buffers.""" + self.render_batch((render_data,)) + + def render_batch(self, render_data: Sequence[IsaacRtxRenderData]) -> None: + """Ensure a shared RTX update once, then extract each camera's annotator outputs. + + Args: + render_data: Cameras whose poses and output buffers have been prepared. Entries + without a spec or output buffers are skipped. An empty sequence performs no work. + """ + cameras = [data for data in render_data if data.output_data is not None and data.spec is not None] + if not cameras: return # Ensure the RTX renderer has been pumped so annotator buffers are fresh. @@ -625,6 +633,15 @@ def render(self, render_data: IsaacRtxRenderData): # for the current physics step, or if a visualizer already pumped it. ensure_isaac_rtx_render_update() + for data in cameras: + self._read_annotator_output(data) + + def _read_annotator_output(self, render_data: IsaacRtxRenderData) -> None: + """Extract one camera's annotator data into its bound output buffers.""" + spec = render_data.spec + output_data = render_data.output_data + assert output_data is not None and spec is not None + view_count = spec.view_count cfg = spec.cfg device = spec.device diff --git a/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py b/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py index 34c05e227800..5f05e23f8420 100644 --- a/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py +++ b/source/isaaclab_physx/test/renderers/test_isaac_rtx_renderer_contract.py @@ -425,7 +425,8 @@ def test_deterministic_flag_gates_rtx_determinism_settings(monkeypatch, stored, @pytest.mark.parametrize("data_type", ["rgba", "normals"]) -def test_render_treats_empty_annotator_frame_as_not_ready(monkeypatch, data_type): +@pytest.mark.parametrize("batched", [False, True], ids=["single", "batch"]) +def test_render_treats_empty_annotator_frame_as_not_ready(monkeypatch, data_type, batched): """An empty warm-up frame should clear its output without slicing or launching a reshape.""" _install_omni_stubs(monkeypatch) import isaaclab_physx.renderers.isaac_rtx_renderer as rtx_renderer @@ -450,15 +451,71 @@ def test_render_treats_empty_annotator_frame_as_not_ready(monkeypatch, data_type renderer.cfg = IsaacRtxRendererCfg() with ( - patch.object(rtx_renderer, "ensure_isaac_rtx_render_update"), + patch.object(rtx_renderer, "ensure_isaac_rtx_render_update") as update, patch.object(rtx_renderer.wp, "launch") as launch, ): - renderer.render(render_data) + if batched: + renderer.render_batch([render_data]) + else: + renderer.render(render_data) + update.assert_called_once_with() output_buffer.zero_.assert_called_once_with() launch.assert_not_called() +@pytest.mark.parametrize( + "states", + [ + pytest.param((), id="empty"), + pytest.param(("no_spec", "no_output"), id="uninitialized"), + pytest.param(("ready", "ready"), id="ready"), + pytest.param(("no_spec", "ready", "no_output", "ready"), id="mixed"), + ], +) +def test_render_batch_updates_once_before_extracting_ready_cameras(monkeypatch, states): + """Ready cameras share one RTX update and receive their own annotator pixels.""" + _install_omni_stubs(monkeypatch) + import isaaclab_physx.renderers.isaac_rtx_renderer as rtx_renderer + from isaaclab_physx.renderers.isaac_rtx_renderer_cfg import IsaacRtxRendererCfg + + renderer = rtx_renderer.IsaacRtxRenderer.__new__(rtx_renderer.IsaacRtxRenderer) + renderer.cfg = IsaacRtxRendererCfg() + operations = MagicMock() + render_data_list = [] + expected_calls = [call.update()] if "ready" in states else [] + expected_outputs = [] + for index, state in enumerate(states): + annotator = getattr(operations, f"camera_{index}") + frame = np.full((1, 1, 4), index + 1, dtype=np.uint8) + annotator.get_data.return_value = frame + output_buffer = wp.zeros((1, 1, 1, 4), dtype=wp.uint8, device="cpu") + render_data_list.append( + SimpleNamespace( + annotators={"rgba": annotator}, + output_data=None if state == "no_output" else {"rgba": SimpleNamespace(warp=output_buffer)}, + spec=( + None + if state == "no_spec" + else SimpleNamespace(view_count=1, device="cpu", cfg=SimpleNamespace(width=1, height=1)) + ), + renderer_info={}, + ppisp_pipeline=None, + _hdr_scratch_wp=None, + ) + ) + if state == "ready": + expected_calls.append(getattr(call, f"camera_{index}").get_data()) + expected_outputs.append((output_buffer, frame[np.newaxis])) + + with patch.object(rtx_renderer, "ensure_isaac_rtx_render_update", operations.update): + renderer.render_batch(render_data_list) + + assert operations.mock_calls == expected_calls + for output_buffer, expected in expected_outputs: + np.testing.assert_array_equal(output_buffer.numpy(), expected) + + def test_isaac_rtx_read_output_clears_stale_metadata_and_keeps_seeded_keys(monkeypatch): """read_output replaces (not merges): a dropped annotator info resets its info entry, seeded keys persist.""" _install_omni_stubs(monkeypatch) diff --git a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py index 85f2fa918e67..19595b3ad56e 100644 --- a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py @@ -5,6 +5,11 @@ """Launch Isaac Sim Simulator first.""" +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sensors")) + from isaaclab.app import AppLauncher # launch omniverse app @@ -22,6 +27,7 @@ from isaaclab_physx.sensors.joint_wrench.joint_wrench_sensor import JointWrenchSensor as PhysxJointWrenchSensor from isaaclab_physx.sensors.joint_wrench.joint_wrench_sensor_data import JointWrenchSensorData from isaaclab_physx.sim.schemas import PhysxJointCfg +from joint_wrench_contract import test_joint_wrench_frame # noqa: F401 import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg @@ -31,7 +37,6 @@ from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor from isaaclab.sim import SimulationCfg from isaaclab.terrains import TerrainImporterCfg -from isaaclab.test.utils.joint_wrench import check_joint_wrench_frame from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR @@ -250,11 +255,6 @@ def test_force_and_torque_components_at_rest(sim): assert torch.any(raw_wrench[:, arm_idx, :] != 0.0) -def test_non_identity_joint_frame_transform(tmp_path): - """PhysX must satisfy the same physical joint-frame contract as Newton.""" - check_joint_wrench_frame(PhysxCfg(), tmp_path) - - def test_wrench_with_external_force_and_torque(sim): """Full wrench validation with external force and torque applied.""" scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1))