From d8d197c79bd750430064db006e3adfe4cfb3c118 Mon Sep 17 00:00:00 2001 From: camevor Date: Mon, 27 Apr 2026 18:22:28 +0200 Subject: [PATCH 01/16] Adds Newton Joint Wrench sensor --- docs/source/api/lab/isaaclab.sensors.rst | 16 ++ source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 20 +- source/isaaclab/isaaclab/sensors/__init__.pyi | 20 +- .../joint_wrench/base_joint_wrench_sensor.py | 69 +++++ .../base_joint_wrench_sensor_data.py | 44 +++ .../joint_wrench/joint_wrench_sensor.py | 27 ++ .../joint_wrench/joint_wrench_sensor_cfg.py | 29 ++ .../joint_wrench/joint_wrench_sensor_data.py | 25 ++ source/isaaclab_newton/config/extension.toml | 2 +- source/isaaclab_newton/docs/CHANGELOG.rst | 9 + .../isaaclab_newton/sensors/__init__.pyi | 5 +- .../joint_wrench/joint_wrench_sensor.py | 190 +++++++++++++ .../joint_wrench/joint_wrench_sensor_data.py | 58 ++++ .../sensors/joint_wrench/kernels.py | 70 +++++ .../test/sensors/test_joint_wrench_sensor.py | 257 ++++++++++++++++++ 16 files changed, 825 insertions(+), 18 deletions(-) create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py create mode 100644 source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py diff --git a/docs/source/api/lab/isaaclab.sensors.rst b/docs/source/api/lab/isaaclab.sensors.rst index 0d7fb3a9d1fe..15fa68e71349 100644 --- a/docs/source/api/lab/isaaclab.sensors.rst +++ b/docs/source/api/lab/isaaclab.sensors.rst @@ -36,6 +36,8 @@ MultiMeshRayCasterCameraCfg Imu ImuCfg + JointWrenchSensor + JointWrenchSensorCfg Sensor Base ----------- @@ -189,3 +191,17 @@ Inertia Measurement Unit :inherited-members: :show-inheritance: :exclude-members: __init__, class_type + +Joint Wrench Sensor +------------------- + +.. autoclass:: JointWrenchSensor + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: JointWrenchSensorCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 551e1ff12bf5..322e44e742c9 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "4.6.22" +version = "4.6.23" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 0731a7b2d3d5..2f5badae0789 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,15 @@ Changelog --------- +4.6.23 (2026-04-30) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.sensors.JointWrenchSensor`. + + 4.6.22 (2026-04-27) ~~~~~~~~~~~~~~~~~~~ @@ -33,17 +42,6 @@ Deprecated 4.6.21 (2026-04-27) ~~~~~~~~~~~~~~~~~~~ -Fixed -^^^^^ - -* Fixed ProxyArray migration guidance and static regression checks to cover - deprecated ``wp.to_torch(proxy_array)`` usage and direct tensor/wp.array - method calls on data properties. - - -4.6.20 (2026-04-27) -~~~~~~~~~~~~~~~~~~~ - Added ^^^^^ diff --git a/source/isaaclab/isaaclab/sensors/__init__.pyi b/source/isaaclab/isaaclab/sensors/__init__.pyi index 2a9d735f70ea..7e6274d6e45a 100644 --- a/source/isaaclab/isaaclab/sensors/__init__.pyi +++ b/source/isaaclab/isaaclab/sensors/__init__.pyi @@ -33,6 +33,11 @@ __all__ = [ "Imu", "ImuCfg", "ImuData", + "BaseJointWrenchSensor", + "BaseJointWrenchSensorData", + "JointWrenchSensor", + "JointWrenchSensorCfg", + "JointWrenchSensorData", "BasePva", "BasePvaData", "Pva", @@ -52,8 +57,6 @@ __all__ = [ "patterns", ] -from .sensor_base import SensorBase -from .sensor_base_cfg import SensorBaseCfg from .camera import ( Camera, CameraCfg, @@ -62,10 +65,10 @@ from .camera import ( RenderBufferSpec, TiledCamera, TiledCameraCfg, - transform_points, create_pointcloud_from_depth, create_pointcloud_from_rgbd, save_images_to_file, + transform_points, ) from .contact_sensor import ( BaseContactSensor, @@ -79,10 +82,17 @@ from .frame_transformer import ( BaseFrameTransformerData, FrameTransformer, FrameTransformerCfg, - OffsetCfg, FrameTransformerData, + OffsetCfg, ) from .imu import BaseImu, BaseImuData, Imu, ImuCfg, ImuData +from .joint_wrench import ( + BaseJointWrenchSensor, + BaseJointWrenchSensorData, + JointWrenchSensor, + JointWrenchSensorCfg, + JointWrenchSensorData, +) from .pva import BasePva, BasePvaData, Pva, PvaCfg, PvaData from .ray_caster import ( MultiMeshRayCaster, @@ -98,3 +108,5 @@ from .ray_caster import ( RayCasterData, patterns, ) +from .sensor_base import SensorBase +from .sensor_base_cfg import SensorBaseCfg diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py new file mode 100644 index 000000000000..2e2f5e8b057d --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py @@ -0,0 +1,69 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from abc import abstractmethod +from typing import TYPE_CHECKING + +import warp as wp + +from ..sensor_base import SensorBase +from .base_joint_wrench_sensor_data import BaseJointWrenchSensorData + +if TYPE_CHECKING: + from .joint_wrench_sensor_cfg import JointWrenchSensorCfg + + +class BaseJointWrenchSensor(SensorBase): + """The joint reaction wrench sensor. + + Reports the incoming joint wrench on each joint of an articulation as a + split force [N] / torque [N·m] pair expressed in the + ``INCOMING_JOINT_FRAME`` convention (child-side joint frame, child-side + joint anchor reference point). Backends convert from their native + representation to this convention internally. + """ + + cfg: JointWrenchSensorCfg + """The configuration parameters.""" + + __backend_name__: str = "base" + """The name of the backend for the joint wrench sensor.""" + + def __init__(self, cfg: JointWrenchSensorCfg): + """Initialize the joint wrench sensor. + + Args: + cfg: The configuration parameters. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + @abstractmethod + def data(self) -> BaseJointWrenchSensorData: + raise NotImplementedError + + """ + Implementation - Abstract methods to be implemented by backend-specific subclasses. + """ + + @abstractmethod + def _initialize_impl(self): + """Initialize the sensor handles and internal buffers. + + Subclasses should call ``super()._initialize_impl()`` first to + initialize the common sensor infrastructure from + :class:`~isaaclab.sensors.SensorBase`. + """ + super()._initialize_impl() + + @abstractmethod + def _update_buffers_impl(self, env_mask: wp.array): + raise NotImplementedError diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py new file mode 100644 index 000000000000..42bf438117ed --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py @@ -0,0 +1,44 @@ +# 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 + +"""Base class for joint-wrench sensor data containers.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod + +import warp as wp + + +class BaseJointWrenchSensorData(ABC): + """Data container for the joint reaction wrench sensor.""" + + @property + @abstractmethod + def force(self) -> wp.array | None: + """Linear component of the joint reaction wrench [N]. + + Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch + this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the + simulation is initialized. + """ + raise NotImplementedError + + @property + @abstractmethod + def torque(self) -> wp.array | None: + """Angular component of the joint reaction wrench [N·m]. + + Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch + this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the + simulation is initialized. + """ + raise NotImplementedError + + @property + @abstractmethod + def body_names(self) -> list[str]: + """Ordered names of the bodies whose incoming joint wrench is reported.""" + raise NotImplementedError diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py new file mode 100644 index 000000000000..f4a88d94b978 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py @@ -0,0 +1,27 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils.backend_utils import FactoryBase + +from .base_joint_wrench_sensor import BaseJointWrenchSensor +from .base_joint_wrench_sensor_data import BaseJointWrenchSensorData + +if TYPE_CHECKING: + from isaaclab_newton.sensors.joint_wrench import JointWrenchSensor as NewtonJointWrenchSensor + from isaaclab_newton.sensors.joint_wrench import JointWrenchSensorData as NewtonJointWrenchSensorData + + +class JointWrenchSensor(FactoryBase, BaseJointWrenchSensor): + """Factory for creating joint-wrench sensor instances.""" + + data: BaseJointWrenchSensorData | NewtonJointWrenchSensorData + + def __new__(cls, *args, **kwargs) -> BaseJointWrenchSensor | NewtonJointWrenchSensor: + """Create a new instance of a joint-wrench sensor based on the backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py new file mode 100644 index 000000000000..a2fff2e9a0c6 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py @@ -0,0 +1,29 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass + +from ..sensor_base_cfg import SensorBaseCfg + +if TYPE_CHECKING: + from .joint_wrench_sensor import JointWrenchSensor + + +@configclass +class JointWrenchSensorCfg(SensorBaseCfg): + """Configuration for a joint reaction wrench sensor. + + The sensor always exposes wrenches in the ``INCOMING_JOINT_FRAME`` + convention: child-side joint frame, child-side joint anchor as reference + point. This matches what a real 6-axis F/T sensor mounted at the joint + would measure. Backends convert to this convention internally so the + public surface is backend-independent. + """ + + class_type: type[JointWrenchSensor] | str = "{DIR}.joint_wrench_sensor:JointWrenchSensor" diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py new file mode 100644 index 000000000000..66bc9d1214bf --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -0,0 +1,25 @@ +# 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 + +"""Factory class for joint-wrench sensor data.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils.backend_utils import FactoryBase + +from .base_joint_wrench_sensor_data import BaseJointWrenchSensorData + +if TYPE_CHECKING: + from isaaclab_newton.sensors.joint_wrench import JointWrenchSensorData as NewtonJointWrenchSensorData + + +class JointWrenchSensorData(FactoryBase, BaseJointWrenchSensorData): + """Factory for creating joint-wrench sensor data instances.""" + + def __new__(cls, *args, **kwargs) -> BaseJointWrenchSensorData | NewtonJointWrenchSensorData: + """Create a new instance of joint-wrench sensor data based on the backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 03b74794b012..0a8eed8000c2 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/source/isaaclab_newton/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.25" +version = "0.5.26" # Description title = "Newton simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 710b3920ca77..9ef33bc51848 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,15 @@ Changelog --------- +0.5.26 (2026-04-30) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_newton.sensors.JointWrenchSensor`. + + 0.5.25 (2026-04-28) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi index 0eba5ef7bdcf..7df064d492de 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi @@ -11,11 +11,14 @@ __all__ = [ "FrameTransformerData", "Imu", "ImuData", + "JointWrenchSensor", + "JointWrenchSensorData", "Pva", "PvaData", ] -from .contact_sensor import ContactSensor, ContactSensorData, ContactSensorCfg +from .contact_sensor import ContactSensor, ContactSensorCfg, ContactSensorData from .frame_transformer import FrameTransformer, FrameTransformerData from .imu import Imu, ImuData +from .joint_wrench import JointWrenchSensor, JointWrenchSensorData from .pva import Pva, PvaData diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py new file mode 100644 index 000000000000..8afe7726bef1 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -0,0 +1,190 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import warp as wp +from newton import JointType +from newton.selection import ArticulationView + +from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor + +from isaaclab_newton.physics import NewtonManager + +from .joint_wrench_sensor_data import JointWrenchSensorData +from .kernels import joint_wrench_reset_kernel, joint_wrench_to_incoming_joint_frame_kernel + +if TYPE_CHECKING: + from isaaclab.sensors.joint_wrench import JointWrenchSensorCfg + +logger = logging.getLogger(__name__) + + +class JointWrenchSensor(BaseJointWrenchSensor): + """Newton joint reaction wrench sensor. + + Reads Newton's ``body_parent_f`` (world-frame wrench at child COM) and + converts each entry to the ``INCOMING_JOINT_FRAME`` convention + (child-side joint frame, child-side joint anchor as reference point) + before storing it in per-joint force / torque buffers. + + :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at the + articulation root prim (the one carrying ``ArticulationRootAPI``) in + every environment; the sensor uses it as the + :class:`~newton.selection.ArticulationView` pattern directly. ``FREE`` + and ``FIXED`` joints are excluded — neither has a meaningful joint + anchor. + """ + + cfg: JointWrenchSensorCfg + """The configuration parameters.""" + + __backend_name__: str = "newton" + """The name of the backend for the joint wrench sensor.""" + + def __init__(self, cfg: JointWrenchSensorCfg): + """Initialize the Newton joint-wrench sensor. + + Args: + cfg: The configuration parameters. + """ + super().__init__(cfg) + + self._data = JointWrenchSensorData() + self._root_view: ArticulationView | None = None + self._sim_bind_body_parent_f: wp.array | None = None + self._sim_bind_body_q: wp.array | None = None + self._sim_bind_body_com: wp.array | None = None + self._sim_bind_joint_X_c: wp.array | None = None + self._joint_child: wp.array | None = None + self._num_joints: int = 0 + + NewtonManager.request_extended_state_attribute("body_parent_f") + + def __str__(self) -> str: + """String representation of the sensor instance.""" + return ( + f"Joint wrench sensor @ '{self.cfg.prim_path}': \n" + f"\tbackend : newton\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of joints : {self._num_joints}\n" + ) + + """ + Properties + """ + + @property + def data(self) -> JointWrenchSensorData: + """The joint-wrench sensor data.""" + self._update_outdated_buffers() + return self._data + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None): + """Reset the sensor buffers for the given environments.""" + env_mask = self._resolve_indices_and_mask(env_ids, env_mask) + super().reset(None, env_mask) + wp.launch( + joint_wrench_reset_kernel, + dim=(self._num_envs, self._num_joints), + inputs=[env_mask, self._data._force, self._data._torque], + device=self._device, + ) + + """ + Implementation + """ + + def _initialize_impl(self): + """PHYSICS_READY callback: builds the articulation view and binds model / state arrays.""" + super()._initialize_impl() + + model = NewtonManager.get_model() + state_0 = NewtonManager.get_state_0() + + self._root_view = ArticulationView( + model, + self.cfg.prim_path.replace(".*", "*"), + verbose=False, + exclude_joint_types=[JointType.FREE, JointType.FIXED], + ) + self._num_joints = self._root_view.joint_count + if self._num_joints == 0: + raise RuntimeError( + "Joint wrench sensor matched zero reportable joints (all joints are FREE or FIXED)." + f" Check the articulation at '{self.cfg.prim_path}'." + ) + + try: + body_parent_f = self._root_view.get_attribute("body_parent_f", state_0) + except KeyError as err: + raise RuntimeError( + f"Joint wrench sensor '{self.cfg.prim_path}': Newton state does not expose" + " 'body_parent_f'. Construct the sensor before sim startup so the extended-state" + " request is forwarded to the model builder." + ) from err + + self._sim_bind_body_parent_f = body_parent_f[:, 0] + self._sim_bind_body_q = self._root_view.get_link_transforms(state_0)[:, 0] + self._sim_bind_body_com = self._root_view.get_attribute("body_com", model)[:, 0] + self._sim_bind_joint_X_c = self._root_view.get_attribute("joint_X_c", model)[:, 0] + + # joint_child is per-articulation; topology is identical across envs, + # so we take the first-env mapping as the 1-D kernel input. + joint_child_full = self._root_view.get_attribute("joint_child", model)[:, 0] + joint_child_np = joint_child_full.numpy()[0] + self._joint_child = wp.array(joint_child_np, dtype=wp.int32, device=self._device) + + link_names = list(self._root_view.link_names) + self._data._body_names = [link_names[int(b)] for b in joint_child_np] + + self._data.create_buffers(num_envs=self._num_envs, num_joints=self._num_joints, device=self._device) + + logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_joints} joints") + + def _update_buffers_impl(self, env_mask: wp.array): + """Convert Newton's body_parent_f into INCOMING_JOINT_FRAME force and torque buffers.""" + if self._sim_bind_body_parent_f is None: + raise RuntimeError( + f"Joint wrench sensor '{self.cfg.prim_path}': not initialized." + " Access sensor data only after sim.reset() has been called." + ) + wp.launch( + joint_wrench_to_incoming_joint_frame_kernel, + dim=(self._num_envs, self._num_joints), + inputs=[ + env_mask, + self._sim_bind_body_parent_f, + self._sim_bind_body_q, + self._sim_bind_body_com, + self._sim_bind_joint_X_c, + self._joint_child, + ], + outputs=[self._data._force, self._data._torque], + device=self._device, + ) + + def _invalidate_initialize_callback(self, event): + """Drop view, cached sizes, and buffers; re-register the extended-state request.""" + super()._invalidate_initialize_callback(event) + self._root_view = None + self._sim_bind_body_parent_f = None + self._sim_bind_body_q = None + self._sim_bind_body_com = None + self._sim_bind_joint_X_c = None + self._joint_child = None + self._num_joints = 0 + self._data._force = None + self._data._torque = None + self._data._body_names = [] + NewtonManager.request_extended_state_attribute("body_parent_f") diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py new file mode 100644 index 000000000000..db10ad66b602 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -0,0 +1,58 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import warp as wp + +from isaaclab.sensors.joint_wrench import BaseJointWrenchSensorData + + +class JointWrenchSensorData(BaseJointWrenchSensorData): + """Data container for the Newton joint-wrench sensor.""" + + def __init__(self): + self._force: wp.array | None = None + self._torque: wp.array | None = None + self._body_names: list[str] = [] + + @property + def force(self) -> wp.array | None: + """Linear component of the joint reaction wrench [N]. + + Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch + this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the + simulation is initialized. + """ + return self._force + + @property + def torque(self) -> wp.array | None: + """Angular component of the joint reaction wrench [N·m]. + + Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch + this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the + simulation is initialized. + """ + return self._torque + + @property + def body_names(self) -> list[str]: + """Ordered names of the bodies whose incoming joint wrench is reported. + + Empty before the simulation is initialized. + """ + return self._body_names + + def create_buffers(self, num_envs: int, num_joints: int, device: str) -> None: + """Allocate internal buffers. + + Args: + num_envs: Number of environments. + num_joints: Number of reported joints (excludes FREE and FIXED joint types). + device: Device for array storage. + """ + self._force = wp.zeros((num_envs, num_joints), dtype=wp.vec3f, device=device) + self._torque = wp.zeros((num_envs, num_joints), dtype=wp.vec3f, device=device) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py new file mode 100644 index 000000000000..edd22c80e4d6 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py @@ -0,0 +1,70 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import warp as wp + + +@wp.kernel +def joint_wrench_to_incoming_joint_frame_kernel( + env_mask: wp.array(dtype=wp.bool), + body_parent_f: wp.array(dtype=wp.spatial_vectorf, ndim=2), + body_q: wp.array(dtype=wp.transformf, ndim=2), + body_com: wp.array(dtype=wp.vec3f, ndim=2), + joint_X_c: wp.array(dtype=wp.transformf, ndim=2), + joint_child: wp.array(dtype=wp.int32), + out_force: wp.array(dtype=wp.vec3f, ndim=2), + out_torque: wp.array(dtype=wp.vec3f, ndim=2), +): + """Convert Newton's ``body_parent_f`` to the INCOMING_JOINT_FRAME convention. + + Newton reports ``body_parent_f[env, body]`` as a spatial wrench in world + frame, referenced at the child body's centre of mass. The output is that + same wrench re-expressed in the child-side joint frame and with the + child-side joint anchor as the reference point — matching what a 6-axis + force/torque sensor mounted at the joint would measure. + """ + env, j = wp.tid() + if not env_mask[env]: + return + + body_idx = joint_child[j] + + # Source wrench: (force, torque-about-COM) in world frame. + src = body_parent_f[env, body_idx] + f_world = wp.vec3f(src[0], src[1], src[2]) + tau_world_com = wp.vec3f(src[3], src[4], src[5]) + + # Child link transform in world and COM offset in link frame. + link_xform = body_q[env, body_idx] + link_quat = wp.transform_get_rotation(link_xform) + link_pos = wp.transform_get_translation(link_xform) + com_world = link_pos + wp.quat_rotate(link_quat, body_com[env, body_idx]) + + # Child-side joint frame in world = body link pose composed with joint_X_c. + joint_xform_world = link_xform * joint_X_c[env, j] + anchor_world = wp.transform_get_translation(joint_xform_world) + joint_quat_world = wp.transform_get_rotation(joint_xform_world) + + # Shift torque reference from COM to joint anchor: tau_B = tau_A + (A - B) x f. + r_com_to_anchor = com_world - anchor_world + tau_world_anchor = tau_world_com + wp.cross(r_com_to_anchor, f_world) + + # Rotate both components into the child-side joint frame. + out_force[env, j] = wp.quat_rotate_inv(joint_quat_world, f_world) + out_torque[env, j] = wp.quat_rotate_inv(joint_quat_world, tau_world_anchor) + + +@wp.kernel +def joint_wrench_reset_kernel( + env_mask: wp.array(dtype=wp.bool), + out_force: wp.array(dtype=wp.vec3f, ndim=2), + out_torque: wp.array(dtype=wp.vec3f, ndim=2), +): + """Zero force / torque entries for the environments selected by ``env_mask``.""" + env, joint = wp.tid() + if not env_mask[env]: + return + out_force[env, joint] = wp.vec3f(0.0, 0.0, 0.0) + out_torque[env, joint] = wp.vec3f(0.0, 0.0, 0.0) diff --git a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py new file mode 100644 index 000000000000..a8e3c647100f --- /dev/null +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -0,0 +1,257 @@ +# 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 + +"""Tests for the Newton JointWrenchSensor.""" + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +import pytest +import torch +import warp as wp +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors.joint_wrench import JointWrenchSensor, JointWrenchSensorCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + + +def _make_single_joint_articulation_cfg() -> ArticulationCfg: + """Single-joint revolute test articulation (root ``CenterPivot`` + arm ``Arm``).""" + return ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", + joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), + ), + actuators={ + "joint": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=2000.0, + damping=100.0, + ), + }, + init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + + +def _make_cartpole_articulation_cfg() -> ArticulationCfg: + """Two-joint cartpole articulation (cart + pole).""" + return ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Classic/Cartpole/cartpole.usd", + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 2.0), + joint_pos={"slider_to_cart": 0.0, "cart_to_pole": 0.0}, + ), + actuators={ + "cart_actuator": ImplicitActuatorCfg( + joint_names_expr=["slider_to_cart"], effort_limit_sim=400.0, stiffness=0.0, damping=10.0 + ), + "pole_actuator": ImplicitActuatorCfg( + joint_names_expr=["cart_to_pole"], effort_limit_sim=400.0, stiffness=0.0, damping=0.0 + ), + }, + ) + + +@configclass +class _SingleJointSceneCfg(InteractiveSceneCfg): + """Scene with a single-joint articulation and the joint-wrench sensor.""" + + env_spacing = 2.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = _make_single_joint_articulation_cfg() + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class _CartpoleSceneCfg(InteractiveSceneCfg): + """Scene with a cartpole (2-joint) articulation and the joint-wrench sensor.""" + + env_spacing = 4.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = _make_cartpole_articulation_cfg() + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + +@pytest.fixture +def sim(): + """Simulation context using the Newton backend.""" + sim_cfg = SimulationCfg( + dt=1.0 / 200.0, + physics=NewtonCfg( + solver_cfg=MJWarpSolverCfg(), + num_substeps=1, + ), + ) + with sim_utils.build_simulation_context(sim_cfg=sim_cfg) as sim_ctx: + sim_ctx._app_control_on_stop_handle = None + yield sim_ctx + + +# --------------------------------------------------------------------------- +# Sensor data — pre-init contract +# --------------------------------------------------------------------------- + + +def test_data_before_init_is_none(): + """``force``/``torque`` return ``None`` before :meth:`create_buffers` runs.""" + from isaaclab_newton.sensors.joint_wrench import JointWrenchSensorData + + data = JointWrenchSensorData() + assert data.force is None + assert data.torque is None + assert data.body_names == [] + + +# --------------------------------------------------------------------------- +# Initialization and shapes +# --------------------------------------------------------------------------- + + +def test_initialization_and_shapes(sim): + """Sensor initializes on sim reset and exposes correctly-shaped buffers.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + sim.step() + scene.update(sim.get_physics_dt()) + + # revolute_articulation has one joint whose child is "Arm". + num_envs = 2 + num_joints = 1 + assert wp.to_torch(sensor.data.force).shape == (num_envs, num_joints, 3) + assert wp.to_torch(sensor.data.torque).shape == (num_envs, num_joints, 3) + assert sensor.data.body_names == ["Arm"] + + +def test_multi_body_articulation(sim): + """Cartpole (2 joints) exposes a wrench for each joint labelled by its child body.""" + scene = InteractiveScene(_CartpoleSceneCfg(num_envs=2)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + sim.step() + scene.update(sim.get_physics_dt()) + + num_envs = 2 + num_joints = 2 + assert wp.to_torch(sensor.data.force).shape == (num_envs, num_joints, 3) + assert wp.to_torch(sensor.data.torque).shape == (num_envs, num_joints, 3) + assert len(sensor.data.body_names) == 2 + assert "rail" not in [n.lower() for n in sensor.data.body_names] + + +# --------------------------------------------------------------------------- +# Physical correctness +# --------------------------------------------------------------------------- + + +def test_force_magnitude_matches_weight_at_rest(sim): + """At steady state, |force| on the arm joint should be close to its weight.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + robot: Articulation = scene["robot"] + # PD control damps out residual oscillation within a few hundred steps. + for _ in range(400): + sim.step() + scene.update(sim.get_physics_dt()) + + force = wp.to_torch(sensor.data.force)[0, 0] # (3,) + torque = wp.to_torch(sensor.data.torque)[0, 0] + + assert torch.isfinite(force).all(), f"Force contains non-finite values: {force}" + assert torch.isfinite(torque).all(), f"Torque contains non-finite values: {torque}" + + # The arm COM is offset from the joint anchor, so gravity creates a non-zero + # moment about the joint. A zero torque would indicate a broken torque path. + torque_mag = torque.norm().item() + assert torque_mag > 0.1, f"|torque|={torque_mag:.3f} N·m, expected non-trivial (>0.1)" + + # Frame-independent check: at rest the only external effect is gravity on the arm. + arm_idx = robot.body_names.index("Arm") + arm_mass = wp.to_torch(robot.data.body_mass)[0, arm_idx].item() + expected_weight = arm_mass * 9.81 + assert abs(force.norm().item() - expected_weight) < 0.1 * expected_weight + 0.5, ( + f"|force|={force.norm().item():.3f} N, expected ~{expected_weight:.3f} N" + ) + + +# --------------------------------------------------------------------------- +# String representation +# --------------------------------------------------------------------------- + + +def test_sensor_print(sim): + """Test that the sensor string representation works.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + sensor_str = str(sensor) + assert "newton" in sensor_str + assert "Joint wrench sensor" in sensor_str + + +# --------------------------------------------------------------------------- +# Reset behavior +# --------------------------------------------------------------------------- + + +def test_reset_zeros_buffers(sim): + """Resetting the sensor clears the force / torque buffers.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + for _ in range(100): + sim.step() + scene.update(sim.get_physics_dt()) + + assert torch.any(wp.to_torch(sensor.data.force) != 0), "Expected non-zero data before reset" + + sensor.reset() + + # Access raw buffers to skip lazy re-population from the Newton view on the next data read. + force_after = wp.to_torch(sensor._data._force) + torque_after = wp.to_torch(sensor._data._torque) + torch.testing.assert_close(force_after, torch.zeros_like(force_after)) + torch.testing.assert_close(torque_after, torch.zeros_like(torque_after)) + + +def test_reset_with_env_ids_only_zeros_selected_envs(sim): + """Partial reset via env_ids should zero the selected envs and preserve the others.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=4)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + for _ in range(100): + sim.step() + scene.update(sim.get_physics_dt()) + + force_before = wp.to_torch(sensor.data.force).clone() + assert torch.any(force_before != 0), "Expected non-zero data before reset" + + sensor.reset(env_ids=[0, 2]) + + force_after = wp.to_torch(sensor._data._force) + torch.testing.assert_close(force_after[0], torch.zeros_like(force_after[0])) + torch.testing.assert_close(force_after[2], torch.zeros_like(force_after[2])) + torch.testing.assert_close(force_after[1], force_before[1]) + torch.testing.assert_close(force_after[3], force_before[3]) From 8ff920d9ffaed2e8ff8950344aa210faedfc729d Mon Sep 17 00:00:00 2001 From: camevor Date: Mon, 27 Apr 2026 18:49:46 +0200 Subject: [PATCH 02/16] Fixes --- source/isaaclab/isaaclab/sensors/__init__.pyi | 8 ++++---- .../isaaclab_newton/isaaclab_newton/sensors/__init__.pyi | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/__init__.pyi b/source/isaaclab/isaaclab/sensors/__init__.pyi index 7e6274d6e45a..1bd092de46e6 100644 --- a/source/isaaclab/isaaclab/sensors/__init__.pyi +++ b/source/isaaclab/isaaclab/sensors/__init__.pyi @@ -57,6 +57,8 @@ __all__ = [ "patterns", ] +from .sensor_base import SensorBase +from .sensor_base_cfg import SensorBaseCfg from .camera import ( Camera, CameraCfg, @@ -65,10 +67,10 @@ from .camera import ( RenderBufferSpec, TiledCamera, TiledCameraCfg, + transform_points, create_pointcloud_from_depth, create_pointcloud_from_rgbd, save_images_to_file, - transform_points, ) from .contact_sensor import ( BaseContactSensor, @@ -82,8 +84,8 @@ from .frame_transformer import ( BaseFrameTransformerData, FrameTransformer, FrameTransformerCfg, - FrameTransformerData, OffsetCfg, + FrameTransformerData, ) from .imu import BaseImu, BaseImuData, Imu, ImuCfg, ImuData from .joint_wrench import ( @@ -108,5 +110,3 @@ from .ray_caster import ( RayCasterData, patterns, ) -from .sensor_base import SensorBase -from .sensor_base_cfg import SensorBaseCfg diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi index 7df064d492de..e536b281952b 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/sensors/__init__.pyi @@ -17,7 +17,7 @@ __all__ = [ "PvaData", ] -from .contact_sensor import ContactSensor, ContactSensorCfg, ContactSensorData +from .contact_sensor import ContactSensor, ContactSensorData, ContactSensorCfg from .frame_transformer import FrameTransformer, FrameTransformerData from .imu import Imu, ImuData from .joint_wrench import JointWrenchSensor, JointWrenchSensorData From cd3c6cee0a79a2b35f5a4f7ee89e38e2c9c20798 Mon Sep 17 00:00:00 2001 From: camevor Date: Tue, 28 Apr 2026 14:47:53 +0200 Subject: [PATCH 03/16] Address review comments --- .../joint_wrench/base_joint_wrench_sensor.py | 6 ++++ .../base_joint_wrench_sensor_data.py | 12 ++------ .../joint_wrench/joint_wrench_sensor_cfg.py | 18 ++++++------ .../joint_wrench/joint_wrench_sensor.py | 8 +++++ .../joint_wrench/joint_wrench_sensor_data.py | 29 +++++++++++-------- .../sensors/joint_wrench/kernels.py | 17 ++++++++--- .../test/sensors/test_joint_wrench_sensor.py | 23 +++++++-------- 7 files changed, 67 insertions(+), 46 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py index 2e2f5e8b057d..727f201d1c7f 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py @@ -50,6 +50,12 @@ def __init__(self, cfg: JointWrenchSensorCfg): def data(self) -> BaseJointWrenchSensorData: raise NotImplementedError + @property + @abstractmethod + def body_names(self) -> list[str]: + """Ordered names of the bodies whose incoming joint wrench is reported.""" + raise NotImplementedError + """ Implementation - Abstract methods to be implemented by backend-specific subclasses. """ diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py index 42bf438117ed..7a56503d9829 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py @@ -9,7 +9,7 @@ from abc import ABC, abstractmethod -import warp as wp +from isaaclab.utils.warp import ProxyArray class BaseJointWrenchSensorData(ABC): @@ -17,7 +17,7 @@ class BaseJointWrenchSensorData(ABC): @property @abstractmethod - def force(self) -> wp.array | None: + def force(self) -> ProxyArray | None: """Linear component of the joint reaction wrench [N]. Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch @@ -28,7 +28,7 @@ def force(self) -> wp.array | None: @property @abstractmethod - def torque(self) -> wp.array | None: + def torque(self) -> ProxyArray | None: """Angular component of the joint reaction wrench [N·m]. Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch @@ -36,9 +36,3 @@ def torque(self) -> wp.array | None: simulation is initialized. """ raise NotImplementedError - - @property - @abstractmethod - def body_names(self) -> list[str]: - """Ordered names of the bodies whose incoming joint wrench is reported.""" - raise NotImplementedError diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py index a2fff2e9a0c6..4e2df0690402 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py @@ -5,7 +5,7 @@ from __future__ import annotations -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Literal from isaaclab.utils import configclass @@ -17,13 +17,13 @@ @configclass class JointWrenchSensorCfg(SensorBaseCfg): - """Configuration for a joint reaction wrench sensor. - - The sensor always exposes wrenches in the ``INCOMING_JOINT_FRAME`` - convention: child-side joint frame, child-side joint anchor as reference - point. This matches what a real 6-axis F/T sensor mounted at the joint - would measure. Backends convert to this convention internally so the - public surface is backend-independent. - """ + """Configuration for a joint reaction wrench sensor.""" class_type: type[JointWrenchSensor] | str = "{DIR}.joint_wrench_sensor:JointWrenchSensor" + + convention: Literal["incoming_joint_frame"] = "incoming_joint_frame" + """Coordinate convention for the reported wrench. Defaults to ``"incoming_joint_frame"``. + + - ``"incoming_joint_frame"`` — child-side joint frame, child-side joint anchor as reference point. + Matches what a real 6-axis F/T sensor mounted at the joint would measure. + """ diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index 8afe7726bef1..6002eb1c1857 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -51,6 +51,9 @@ class JointWrenchSensor(BaseJointWrenchSensor): def __init__(self, cfg: JointWrenchSensorCfg): """Initialize the Newton joint-wrench sensor. + Requests the ``body_parent_f`` extended state attribute from :class:`NewtonManager` so the + model builder allocates it during simulation startup. + Args: cfg: The configuration parameters. """ @@ -80,6 +83,11 @@ def __str__(self) -> str: Properties """ + @property + def body_names(self) -> list[str]: + """Ordered names of the bodies whose incoming joint wrench is reported.""" + return self._data._body_names + @property def data(self) -> JointWrenchSensorData: """The joint-wrench sensor data.""" diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py index db10ad66b602..ddf98d9113b8 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -8,6 +8,7 @@ import warp as wp from isaaclab.sensors.joint_wrench import BaseJointWrenchSensorData +from isaaclab.utils.warp import ProxyArray class JointWrenchSensorData(BaseJointWrenchSensorData): @@ -17,34 +18,36 @@ def __init__(self): self._force: wp.array | None = None self._torque: wp.array | None = None self._body_names: list[str] = [] + self._force_ta: ProxyArray | None = None + self._torque_ta: ProxyArray | None = None @property - def force(self) -> wp.array | None: + def force(self) -> ProxyArray | None: """Linear component of the joint reaction wrench [N]. Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is initialized. """ - return self._force + if self._force is None: + return None + if self._force_ta is None: + self._force_ta = ProxyArray(self._force) + return self._force_ta @property - def torque(self) -> wp.array | None: + def torque(self) -> ProxyArray | None: """Angular component of the joint reaction wrench [N·m]. Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is initialized. """ - return self._torque - - @property - def body_names(self) -> list[str]: - """Ordered names of the bodies whose incoming joint wrench is reported. - - Empty before the simulation is initialized. - """ - return self._body_names + if self._torque is None: + return None + if self._torque_ta is None: + self._torque_ta = ProxyArray(self._torque) + return self._torque_ta def create_buffers(self, num_envs: int, num_joints: int, device: str) -> None: """Allocate internal buffers. @@ -56,3 +59,5 @@ def create_buffers(self, num_envs: int, num_joints: int, device: str) -> None: """ self._force = wp.zeros((num_envs, num_joints), dtype=wp.vec3f, device=device) self._torque = wp.zeros((num_envs, num_joints), dtype=wp.vec3f, device=device) + self._force_ta = None + self._torque_ta = None diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py index edd22c80e4d6..7bd82a8d70b3 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py @@ -19,11 +19,20 @@ def joint_wrench_to_incoming_joint_frame_kernel( ): """Convert Newton's ``body_parent_f`` to the INCOMING_JOINT_FRAME convention. - Newton reports ``body_parent_f[env, body]`` as a spatial wrench in world - frame, referenced at the child body's centre of mass. The output is that - same wrench re-expressed in the child-side joint frame and with the - child-side joint anchor as the reference point — matching what a 6-axis + Newton reports ``body_parent_f[env, body]`` as a spatial wrench in world frame, referenced at + the child body's centre of mass. The output is that same wrench re-expressed in the child-side + joint frame and with the child-side joint anchor as the reference point — matching what a 6-axis force/torque sensor mounted at the joint would measure. + + Args: + env_mask: Boolean mask selecting which environments to update. + body_parent_f: Newton state — world-frame spatial wrench at child COM ``(num_envs, num_bodies)``. + body_q: Newton state — child link transforms in world frame ``(num_envs, num_bodies)``. + body_com: Newton model — COM offset in link-local frame ``(num_envs, num_bodies)``. + joint_X_c: Newton model — child-side joint frame relative to child link ``(num_envs, num_joints)``. + joint_child: Newton model — body index of each joint's child link ``(num_joints,)``. + out_force: Output force in joint frame [N] ``(num_envs, num_joints)``. + out_torque: Output torque in joint frame [N·m] ``(num_envs, num_joints)``. """ env, j = wp.tid() if not env_mask[env]: 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 a8e3c647100f..d78850e643f5 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -114,7 +114,6 @@ def test_data_before_init_is_none(): data = JointWrenchSensorData() assert data.force is None assert data.torque is None - assert data.body_names == [] # --------------------------------------------------------------------------- @@ -134,9 +133,9 @@ def test_initialization_and_shapes(sim): # revolute_articulation has one joint whose child is "Arm". num_envs = 2 num_joints = 1 - assert wp.to_torch(sensor.data.force).shape == (num_envs, num_joints, 3) - assert wp.to_torch(sensor.data.torque).shape == (num_envs, num_joints, 3) - assert sensor.data.body_names == ["Arm"] + assert sensor.data.force.torch.shape == (num_envs, num_joints, 3) + assert sensor.data.torque.torch.shape == (num_envs, num_joints, 3) + assert sensor.body_names == ["Arm"] def test_multi_body_articulation(sim): @@ -150,10 +149,10 @@ def test_multi_body_articulation(sim): num_envs = 2 num_joints = 2 - assert wp.to_torch(sensor.data.force).shape == (num_envs, num_joints, 3) - assert wp.to_torch(sensor.data.torque).shape == (num_envs, num_joints, 3) - assert len(sensor.data.body_names) == 2 - assert "rail" not in [n.lower() for n in sensor.data.body_names] + assert sensor.data.force.torch.shape == (num_envs, num_joints, 3) + assert sensor.data.torque.torch.shape == (num_envs, num_joints, 3) + assert len(sensor.body_names) == 2 + assert "rail" not in [n.lower() for n in sensor.body_names] # --------------------------------------------------------------------------- @@ -173,8 +172,8 @@ def test_force_magnitude_matches_weight_at_rest(sim): sim.step() scene.update(sim.get_physics_dt()) - force = wp.to_torch(sensor.data.force)[0, 0] # (3,) - torque = wp.to_torch(sensor.data.torque)[0, 0] + force = sensor.data.force.torch[0, 0] # (3,) + torque = sensor.data.torque.torch[0, 0] assert torch.isfinite(force).all(), f"Force contains non-finite values: {force}" assert torch.isfinite(torque).all(), f"Torque contains non-finite values: {torque}" @@ -224,7 +223,7 @@ def test_reset_zeros_buffers(sim): sim.step() scene.update(sim.get_physics_dt()) - assert torch.any(wp.to_torch(sensor.data.force) != 0), "Expected non-zero data before reset" + assert torch.any(sensor.data.force.torch != 0), "Expected non-zero data before reset" sensor.reset() @@ -245,7 +244,7 @@ def test_reset_with_env_ids_only_zeros_selected_envs(sim): sim.step() scene.update(sim.get_physics_dt()) - force_before = wp.to_torch(sensor.data.force).clone() + force_before = sensor.data.force.torch.clone() assert torch.any(force_before != 0), "Expected non-zero data before reset" sensor.reset(env_ids=[0, 2]) From c0edb5825cfb58483b739f0901131660234a25b3 Mon Sep 17 00:00:00 2001 From: camevor Date: Tue, 28 Apr 2026 17:01:38 +0200 Subject: [PATCH 04/16] Change import order --- .../isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index 6002eb1c1857..3e8ed2bc7440 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -10,12 +10,12 @@ from typing import TYPE_CHECKING import warp as wp +from isaaclab_newton.physics import NewtonManager from newton import JointType from newton.selection import ArticulationView from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor -from isaaclab_newton.physics import NewtonManager from .joint_wrench_sensor_data import JointWrenchSensorData from .kernels import joint_wrench_reset_kernel, joint_wrench_to_incoming_joint_frame_kernel From f7653bcc0b51029729ec9867c2335ff7e81ecf31 Mon Sep 17 00:00:00 2001 From: camevor Date: Tue, 28 Apr 2026 18:14:35 +0200 Subject: [PATCH 05/16] Add git-ignored files --- .../isaaclab/sensors/joint_wrench/__init__.py | 10 ++++++++++ .../isaaclab/sensors/joint_wrench/__init__.pyi | 18 ++++++++++++++++++ .../sensors/joint_wrench/__init__.py | 10 ++++++++++ .../sensors/joint_wrench/__init__.pyi | 9 +++++++++ 4 files changed, 47 insertions(+) create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/__init__.py create mode 100644 source/isaaclab/isaaclab/sensors/joint_wrench/__init__.pyi create mode 100644 source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.pyi diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/__init__.py b/source/isaaclab/isaaclab/sensors/joint_wrench/__init__.py new file mode 100644 index 000000000000..aae021e8e98d --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Joint Wrench Sensor.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/__init__.pyi b/source/isaaclab/isaaclab/sensors/joint_wrench/__init__.pyi new file mode 100644 index 000000000000..97980db01c57 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/__init__.pyi @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "BaseJointWrenchSensor", + "BaseJointWrenchSensorData", + "JointWrenchSensor", + "JointWrenchSensorCfg", + "JointWrenchSensorData", +] + +from .base_joint_wrench_sensor import BaseJointWrenchSensor +from .base_joint_wrench_sensor_data import BaseJointWrenchSensorData +from .joint_wrench_sensor import JointWrenchSensor +from .joint_wrench_sensor_cfg import JointWrenchSensorCfg +from .joint_wrench_sensor_data import JointWrenchSensorData diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.py new file mode 100644 index 000000000000..fb10acc86182 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for the Newton joint-wrench sensor.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.pyi new file mode 100644 index 000000000000..b2bcd3582d44 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/__init__.pyi @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = ["JointWrenchSensor", "JointWrenchSensorData"] + +from .joint_wrench_sensor import JointWrenchSensor +from .joint_wrench_sensor_data import JointWrenchSensorData From c1cffc9589b1618415370937aeb2db185c4ed681 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 10:37:40 +0200 Subject: [PATCH 06/16] Minor fixes --- source/isaaclab/docs/CHANGELOG.rst | 11 +++++++++++ .../joint_wrench/base_joint_wrench_sensor.py | 1 + .../base_joint_wrench_sensor_data.py | 16 ++++++++++------ .../sensors/joint_wrench/joint_wrench_sensor.py | 5 +++-- .../joint_wrench/joint_wrench_sensor_data.py | 16 ++++++++++------ .../sensors/joint_wrench/kernels.py | 7 ++++--- 6 files changed, 39 insertions(+), 17 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 2f5badae0789..07007ad709f2 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -42,6 +42,17 @@ Deprecated 4.6.21 (2026-04-27) ~~~~~~~~~~~~~~~~~~~ +Fixed +^^^^^ + +* Fixed ProxyArray migration guidance and static regression checks to cover + deprecated ``wp.to_torch(proxy_array)`` usage and direct tensor/wp.array + method calls on data properties. + + +4.6.20 (2026-04-27) +~~~~~~~~~~~~~~~~~~~ + Added ^^^^^ diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py index 727f201d1c7f..e80c7ffab435 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py @@ -48,6 +48,7 @@ def __init__(self, cfg: JointWrenchSensorCfg): @property @abstractmethod def data(self) -> BaseJointWrenchSensorData: + """The sensor data container, populated after simulation initialization.""" raise NotImplementedError @property diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py index 7a56503d9829..fd1380212990 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py @@ -20,9 +20,11 @@ class BaseJointWrenchSensorData(ABC): def force(self) -> ProxyArray | None: """Linear component of the joint reaction wrench [N]. - Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch - this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the - simulation is initialized. + Expressed in the frame selected by + :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is + ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + initialized. """ raise NotImplementedError @@ -31,8 +33,10 @@ def force(self) -> ProxyArray | None: def torque(self) -> ProxyArray | None: """Angular component of the joint reaction wrench [N·m]. - Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch - this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the - simulation is initialized. + Expressed in the frame selected by + :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is + ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + initialized. """ raise NotImplementedError diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index 3e8ed2bc7440..aedd826457a0 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -16,7 +16,6 @@ from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor - from .joint_wrench_sensor_data import JointWrenchSensorData from .kernels import joint_wrench_reset_kernel, joint_wrench_to_incoming_joint_frame_kernel @@ -135,7 +134,7 @@ def _initialize_impl(self): try: body_parent_f = self._root_view.get_attribute("body_parent_f", state_0) - except KeyError as err: + except AttributeError as err: raise RuntimeError( f"Joint wrench sensor '{self.cfg.prim_path}': Newton state does not expose" " 'body_parent_f'. Construct the sensor before sim startup so the extended-state" @@ -195,4 +194,6 @@ def _invalidate_initialize_callback(self, event): self._data._force = None self._data._torque = None self._data._body_names = [] + self._data._force_ta = None + self._data._torque_ta = None NewtonManager.request_extended_state_attribute("body_parent_f") diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py index ddf98d9113b8..640fd784978c 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -25,9 +25,11 @@ def __init__(self): def force(self) -> ProxyArray | None: """Linear component of the joint reaction wrench [N]. - Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch - this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the - simulation is initialized. + Expressed in the frame selected by + :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is + ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + initialized. """ if self._force is None: return None @@ -39,9 +41,11 @@ def force(self) -> ProxyArray | None: def torque(self) -> ProxyArray | None: """Angular component of the joint reaction wrench [N·m]. - Shape is ``(num_envs, num_joints)``, dtype :class:`wp.vec3f`. In torch - this resolves to ``(num_envs, num_joints, 3)``. ``None`` before the - simulation is initialized. + Expressed in the frame selected by + :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is + ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + initialized. """ if self._torque is None: return None diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py index 7bd82a8d70b3..99b0972e8d10 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py @@ -39,11 +39,12 @@ def joint_wrench_to_incoming_joint_frame_kernel( return body_idx = joint_child[j] + assert body_idx >= 0 and body_idx < body_parent_f.shape[1] - # Source wrench: (force, torque-about-COM) in world frame. + # Source wrench in world frame. Newton's body_parent_f stores (force, torque-about-COM). src = body_parent_f[env, body_idx] - f_world = wp.vec3f(src[0], src[1], src[2]) - tau_world_com = wp.vec3f(src[3], src[4], src[5]) + f_world = wp.spatial_top(src) + tau_world_com = wp.spatial_bottom(src) # Child link transform in world and COM offset in link frame. link_xform = body_q[env, body_idx] From e4939f21f88e4e9f889379c1eab6a3564d4c4aeb Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 10:38:25 +0200 Subject: [PATCH 07/16] Add more comprehensive tests --- .../test/sensors/test_joint_wrench_sensor.py | 235 ++++++++++++++++-- 1 file changed, 217 insertions(+), 18 deletions(-) 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 d78850e643f5..4ada56f53db8 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -23,6 +23,7 @@ from isaaclab.sim import SimulationCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass +from isaaclab.utils import math as math_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR @@ -45,8 +46,12 @@ def _make_single_joint_articulation_cfg() -> ArticulationCfg: ) -def _make_cartpole_articulation_cfg() -> ArticulationCfg: - """Two-joint cartpole articulation (cart + pole).""" +def _make_cartpole_articulation_cfg(pole_damping: float = 0.0) -> ArticulationCfg: + """Two-joint cartpole articulation (cart + pole). + + Args: + pole_damping: Damping for the cart-to-pole revolute joint. + """ return ArticulationCfg( prim_path="{ENV_REGEX_NS}/Robot", spawn=sim_utils.UsdFileCfg( @@ -61,7 +66,7 @@ def _make_cartpole_articulation_cfg() -> ArticulationCfg: joint_names_expr=["slider_to_cart"], effort_limit_sim=400.0, stiffness=0.0, damping=10.0 ), "pole_actuator": ImplicitActuatorCfg( - joint_names_expr=["cart_to_pole"], effort_limit_sim=400.0, stiffness=0.0, damping=0.0 + joint_names_expr=["cart_to_pole"], effort_limit_sim=400.0, stiffness=0.0, damping=pole_damping ), }, ) @@ -87,6 +92,16 @@ class _CartpoleSceneCfg(InteractiveSceneCfg): wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") +@configclass +class _CartpoleDampedSceneCfg(InteractiveSceneCfg): + """Cartpole with pole damping for steady-state physics validation tests.""" + + env_spacing = 4.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = _make_cartpole_articulation_cfg(pole_damping=10.0) + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + @pytest.fixture def sim(): """Simulation context using the Newton backend.""" @@ -160,37 +175,221 @@ def test_multi_body_articulation(sim): # --------------------------------------------------------------------------- -def test_force_magnitude_matches_weight_at_rest(sim): - """At steady state, |force| on the arm joint should be close to its weight.""" +def _compute_expected_wrench_in_joint_frame( + sensor, + robot, + env: int, + joint: int, + gravity: torch.Tensor, + ext_force_b: torch.Tensor | None = None, + ext_torque_b: torch.Tensor | None = None, + descendant_body_names: list[str] | None = None, +): + """Compute the analytical joint-frame wrench for a single joint. + + Uses the same geometric data (body_com, joint_X_c, body_q) and frame + transformations as the kernel, but computes the wrench analytically from + known loads rather than reading body_parent_f. Computes the moment of + forces about the joint anchor and rotates the result into the child-side + joint frame. + + For terminal links, the wrench is due to the child body alone. For + interior joints, pass all bodies in the subtree below the joint via + ``descendant_body_names`` so the helper sums their gravitational + contributions. + + Args: + sensor: The JointWrenchSensor instance (used to read Newton model bindings). + robot: The Articulation asset (used for body mass lookup). + env: Environment index. + joint: Joint index within the sensor. + gravity: Gravity vector in world frame, shape (3,). + ext_force_b: External force on the child body in body frame [N], shape (3,). + ext_torque_b: External torque on the child body in body frame [N·m], shape (3,). + descendant_body_names: Bodies whose gravitational load acts through this + joint. Defaults to the child body only (correct for terminal links). + For an interior joint, pass all bodies in the subtree below the joint. + + Returns: + A tuple of (force, torque) tensors, each shape (3,), in the child-side + joint frame. + """ + body_idx = wp.to_torch(sensor._joint_child)[joint].item() + + # Link transform in world (of the child body — defines the joint frame). + link_xform = wp.to_torch(sensor._sim_bind_body_q)[env, body_idx] # (7,) = pos(3) + quat(4) + link_pos = link_xform[:3] + link_quat = link_xform[3:] # wp.quatf = (x, y, z, w) + + # Joint anchor and orientation in world = link_xform * joint_X_c. + joint_X_c = wp.to_torch(sensor._sim_bind_joint_X_c)[env, joint] # (7,) + jxc_pos = joint_X_c[:3] + jxc_quat = joint_X_c[3:] + anchor_world = link_pos + math_utils.quat_apply(link_quat.unsqueeze(0), jxc_pos.unsqueeze(0)).squeeze(0) + joint_quat_world = math_utils.quat_mul(link_quat.unsqueeze(0), jxc_quat.unsqueeze(0)).squeeze(0) + + # Bodies whose weight contributes to the wrench at this joint. + if descendant_body_names is None: + descendant_body_names = [sensor.body_names[joint]] + + link_names = list(sensor._root_view.link_names) + + total_force_w = torch.zeros(3, device=gravity.device) + total_torque_w = torch.zeros(3, device=gravity.device) + + for body_name in descendant_body_names: + b_idx = link_names.index(body_name) + b_xform = wp.to_torch(sensor._sim_bind_body_q)[env, b_idx] + b_pos = b_xform[:3] + b_quat = b_xform[3:] + b_com_local = wp.to_torch(sensor._sim_bind_body_com)[env, b_idx] + b_com_world = b_pos + math_utils.quat_apply(b_quat.unsqueeze(0), b_com_local.unsqueeze(0)).squeeze(0) + + art_b_idx = robot.body_names.index(body_name) + mass = robot.data.body_mass.torch[env, art_b_idx].item() + weight_w = mass * gravity + + total_force_w = total_force_w + weight_w + r = b_com_world - anchor_world + total_torque_w = total_torque_w + torch.cross(r, weight_w, dim=-1) + + # External force/torque on the child body only (if provided). Actuator + # torque is intentionally omitted; see tolerance comment in calling tests. + if ext_force_b is not None: + total_force_w = total_force_w + math_utils.quat_apply(link_quat.unsqueeze(0), ext_force_b.unsqueeze(0)).squeeze( + 0 + ) + if ext_torque_b is not None: + total_torque_w = total_torque_w + math_utils.quat_apply( + link_quat.unsqueeze(0), ext_torque_b.unsqueeze(0) + ).squeeze(0) + + # Reaction wrench = negation of total wrench (joint supports against all loads). + reaction_force_w = -total_force_w + reaction_torque_w = -total_torque_w + + # Rotate into joint frame. + expected_force = math_utils.quat_apply_inverse( + joint_quat_world.unsqueeze(0), reaction_force_w.unsqueeze(0) + ).squeeze(0) + expected_torque = math_utils.quat_apply_inverse( + joint_quat_world.unsqueeze(0), reaction_torque_w.unsqueeze(0) + ).squeeze(0) + + return expected_force, expected_torque + + +def test_force_and_torque_components_at_rest(sim): + """Component-level validation of force and torque against analytical expectations (gravity only).""" scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) sim.reset() sensor: JointWrenchSensor = scene["wrench"] robot: Articulation = scene["robot"] - # PD control damps out residual oscillation within a few hundred steps. for _ in range(400): sim.step() scene.update(sim.get_physics_dt()) - force = sensor.data.force.torch[0, 0] # (3,) + gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( + sensor, + robot, + env=0, + joint=0, + gravity=gravity, + ) + + force = sensor.data.force.torch[0, 0] torque = sensor.data.torque.torch[0, 0] - assert torch.isfinite(force).all(), f"Force contains non-finite values: {force}" - assert torch.isfinite(torque).all(), f"Torque contains non-finite values: {torque}" + torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) + torch.testing.assert_close(torque, expected_torque, atol=1e-2, rtol=1e-3) - # The arm COM is offset from the joint anchor, so gravity creates a non-zero - # moment about the joint. A zero torque would indicate a broken torque path. - torque_mag = torque.norm().item() - assert torque_mag > 0.1, f"|torque|={torque_mag:.3f} N·m, expected non-trivial (>0.1)" - # Frame-independent check: at rest the only external effect is gravity on the arm. +def test_wrench_with_external_force_and_torque(sim): + """Full analytical wrench validation with external force and torque applied. + + Mirrors the PhysX ``test_body_incoming_joint_wrench_b_single_joint`` pattern: + apply a known wrench, settle, compute the expected reaction wrench analytically, + and compare component-by-component. + """ + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + robot: Articulation = scene["robot"] arm_idx = robot.body_names.index("Arm") - arm_mass = wp.to_torch(robot.data.body_mass)[0, arm_idx].item() - expected_weight = arm_mass * 9.81 - assert abs(force.norm().item() - expected_weight) < 0.1 * expected_weight + 0.5, ( - f"|force|={force.norm().item():.3f} N, expected ~{expected_weight:.3f} N" + + # Apply 10 N in body-Y and 10 N·m in body-Z on the arm (matches PhysX test). + ext_force_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) + ext_force_b[:, arm_idx, 1] = 10.0 + ext_torque_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) + ext_torque_b[:, arm_idx, 2] = 10.0 + + for _ in range(800): + robot.permanent_wrench_composer.set_forces_and_torques_index(forces=ext_force_b, torques=ext_torque_b) + robot.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( + sensor, + robot, + env=0, + joint=0, + gravity=gravity, + ext_force_b=ext_force_b[0, arm_idx], + ext_torque_b=ext_torque_b[0, arm_idx], ) + force = sensor.data.force.torch[0, 0] + torque = sensor.data.torque.torch[0, 0] + + # The PD actuator contributes a small torque (~0.1 N·m) to body_parent_f that is + # not modelled in the analytical helper. Force is unaffected (actuator is pure torque). + torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) + torch.testing.assert_close(torque, expected_torque, atol=0.15, rtol=1e-2) + + +def test_interior_joint_wrench_at_rest(sim): + """Interior joint wrench accounts for the weight of all descendant bodies. + + The cartpole has two joints: ``slider_to_cart`` (interior, supports cart + and pole) and ``cart_to_pole`` (terminal, supports pole only). At steady + state with gravity as the only load, the reaction wrench at the interior + joint must equal the combined weight of cart and pole, with torque + computed from each body's moment about the joint anchor. + """ + scene = InteractiveScene(_CartpoleDampedSceneCfg(num_envs=1)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + robot: Articulation = scene["robot"] + + for _ in range(800): + sim.step() + scene.update(sim.get_physics_dt()) + + gravity = torch.tensor(sim.cfg.gravity, device=sim.device) + + # Interior joint (index 0, slider_to_cart): reaction wrench supports + # all bodies in the subtree — both cart and pole. + expected_force, expected_torque = _compute_expected_wrench_in_joint_frame( + sensor, + robot, + env=0, + joint=0, + gravity=gravity, + descendant_body_names=list(sensor.body_names), + ) + + force = sensor.data.force.torch[0, 0] + torque = sensor.data.torque.torch[0, 0] + + torch.testing.assert_close(force, expected_force, atol=1e-2, rtol=1e-3) + torch.testing.assert_close(torque, expected_torque, atol=1e-2, rtol=1e-3) + # --------------------------------------------------------------------------- # String representation From f1b199edd657e17af397fbdadd731e2548f3a5c7 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 12:13:31 +0200 Subject: [PATCH 08/16] pre-commit --- .../sensors/joint_wrench/joint_wrench_sensor.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index aedd826457a0..0ec5477d0daa 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -10,12 +10,13 @@ from typing import TYPE_CHECKING import warp as wp -from isaaclab_newton.physics import NewtonManager from newton import JointType from newton.selection import ArticulationView from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor +from isaaclab_newton.physics import NewtonManager + from .joint_wrench_sensor_data import JointWrenchSensorData from .kernels import joint_wrench_reset_kernel, joint_wrench_to_incoming_joint_frame_kernel From a3feca669f9e60f1d745d2088b3159c71186b935 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 14:28:35 +0200 Subject: [PATCH 09/16] Fix test; address pr comments --- .../sensors/joint_wrench/joint_wrench_sensor.py | 4 ++++ .../isaaclab_newton/sensors/joint_wrench/kernels.py | 8 ++++---- .../test/sensors/test_joint_wrench_sensor.py | 11 ++++++++--- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index 0ec5477d0daa..ada113cd3d58 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -100,6 +100,8 @@ def data(self) -> JointWrenchSensorData: def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None): """Reset the sensor buffers for the given environments.""" + if self._data._force is None or self._data._torque is None: + return env_mask = self._resolve_indices_and_mask(env_ids, env_mask) super().reset(None, env_mask) wp.launch( @@ -151,6 +153,8 @@ def _initialize_impl(self): # so we take the first-env mapping as the 1-D kernel input. joint_child_full = self._root_view.get_attribute("joint_child", model)[:, 0] joint_child_np = joint_child_full.numpy()[0] + if not all(0 <= b < self._sim_bind_body_parent_f.shape[1] for b in joint_child_np): + raise RuntimeError(f"joint_child contains out-of-range body indices for '{self.cfg.prim_path}'") self._joint_child = wp.array(joint_child_np, dtype=wp.int32, device=self._device) link_names = list(self._root_view.link_names) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py index 99b0972e8d10..64a0b4f0dda2 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/kernels.py @@ -39,7 +39,6 @@ def joint_wrench_to_incoming_joint_frame_kernel( return body_idx = joint_child[j] - assert body_idx >= 0 and body_idx < body_parent_f.shape[1] # Source wrench in world frame. Newton's body_parent_f stores (force, torque-about-COM). src = body_parent_f[env, body_idx] @@ -57,9 +56,10 @@ def joint_wrench_to_incoming_joint_frame_kernel( anchor_world = wp.transform_get_translation(joint_xform_world) joint_quat_world = wp.transform_get_rotation(joint_xform_world) - # Shift torque reference from COM to joint anchor: tau_B = tau_A + (A - B) x f. - r_com_to_anchor = com_world - anchor_world - tau_world_anchor = tau_world_com + wp.cross(r_com_to_anchor, f_world) + # Shift torque reference from COM to joint anchor: + # tau_anchor = tau_com + (com - anchor) x f = tau_com + r_anchor_to_com x f. + r_anchor_to_com = com_world - anchor_world + tau_world_anchor = tau_world_com + wp.cross(r_anchor_to_com, f_world) # Rotate both components into the child-side joint frame. out_force[env, j] = wp.quat_rotate_inv(joint_quat_world, f_world) 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 4ada56f53db8..7e0ac0ea6960 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -256,9 +256,14 @@ def _compute_expected_wrench_in_joint_frame( # External force/torque on the child body only (if provided). Actuator # torque is intentionally omitted; see tolerance comment in calling tests. if ext_force_b is not None: - total_force_w = total_force_w + math_utils.quat_apply(link_quat.unsqueeze(0), ext_force_b.unsqueeze(0)).squeeze( - 0 - ) + ext_force_w = math_utils.quat_apply(link_quat.unsqueeze(0), ext_force_b.unsqueeze(0)).squeeze(0) + total_force_w = total_force_w + ext_force_w + # Moment of the external force about the joint anchor (applied at child COM). + child_com_local = wp.to_torch(sensor._sim_bind_body_com)[env, body_idx] + child_com_world = link_pos + math_utils.quat_apply( + link_quat.unsqueeze(0), child_com_local.unsqueeze(0) + ).squeeze(0) + total_torque_w = total_torque_w + torch.cross(child_com_world - anchor_world, ext_force_w, dim=-1) if ext_torque_b is not None: total_torque_w = total_torque_w + math_utils.quat_apply( link_quat.unsqueeze(0), ext_torque_b.unsqueeze(0) From 52410ff2e7586e98989be42880a17a3725218195 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 14:49:09 +0200 Subject: [PATCH 10/16] Update source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py Co-authored-by: Antoine RICHARD Signed-off-by: camevor --- .../isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py index 4e2df0690402..aae63076156b 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py @@ -25,5 +25,6 @@ class JointWrenchSensorCfg(SensorBaseCfg): """Coordinate convention for the reported wrench. Defaults to ``"incoming_joint_frame"``. - ``"incoming_joint_frame"`` — child-side joint frame, child-side joint anchor as reference point. - Matches what a real 6-axis F/T sensor mounted at the joint would measure. + Matches what a real 6-axis F/T sensor mounted at the joint would measure. This is the same + as PhysX convention in IsaacLab2.3 """ From 272609536ac374125cee87df0921c2bf78d3fcec Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 14:51:16 +0200 Subject: [PATCH 11/16] Apply suggestions from code review Co-authored-by: Antoine RICHARD Signed-off-by: camevor --- .../joint_wrench/base_joint_wrench_sensor.py | 4 +-- .../joint_wrench/joint_wrench_sensor.py | 26 ++++++++++++++----- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py index e80c7ffab435..e11811b451bb 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py @@ -62,7 +62,7 @@ def body_names(self) -> list[str]: """ @abstractmethod - def _initialize_impl(self): + def _initialize_impl(self) -> None: """Initialize the sensor handles and internal buffers. Subclasses should call ``super()._initialize_impl()`` first to @@ -72,5 +72,5 @@ def _initialize_impl(self): super()._initialize_impl() @abstractmethod - def _update_buffers_impl(self, env_mask: wp.array): + def _update_buffers_impl(self, env_mask: wp.array) -> None: raise NotImplementedError diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index ada113cd3d58..f9f5db13e2b8 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -98,8 +98,12 @@ def data(self) -> JointWrenchSensorData: Operations """ - def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None): - """Reset the sensor buffers for the given environments.""" + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + """Reset the sensor buffers for the given environments. + + Args: + env_ids: the environment ids to reset. + env_mask: the mask used to reset the environments. Shape is (num_envs).""" if self._data._force is None or self._data._torque is None: return env_mask = self._resolve_indices_and_mask(env_ids, env_mask) @@ -115,7 +119,7 @@ def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None Implementation """ - def _initialize_impl(self): + def _initialize_impl(self) -> None: """PHYSICS_READY callback: builds the articulation view and binds model / state arrays.""" super()._initialize_impl() @@ -164,8 +168,12 @@ def _initialize_impl(self): logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_joints} joints") - def _update_buffers_impl(self, env_mask: wp.array): - """Convert Newton's body_parent_f into INCOMING_JOINT_FRAME force and torque buffers.""" + def _update_buffers_impl(self, env_mask: wp.array) -> None: + """Convert Newton's body_parent_f into INCOMING_JOINT_FRAME force and torque buffers. + + Args: + env_mask: A mask containing which environments need to be updated. Shape is (num_envs) + """ if self._sim_bind_body_parent_f is None: raise RuntimeError( f"Joint wrench sensor '{self.cfg.prim_path}': not initialized." @@ -186,8 +194,12 @@ def _update_buffers_impl(self, env_mask: wp.array): device=self._device, ) - def _invalidate_initialize_callback(self, event): - """Drop view, cached sizes, and buffers; re-register the extended-state request.""" + def _invalidate_initialize_callback(self, event) -> None: + """Drop view, cached sizes, and buffers; re-register the extended-state request. + + Args: + event: An invalidate event. + """ super()._invalidate_initialize_callback(event) self._root_view = None self._sim_bind_body_parent_f = None From f5e1b28da1b8b4dcd966614f34110535338df5b1 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 30 Apr 2026 14:52:14 +0200 Subject: [PATCH 12/16] pre-commit --- .../sensors/joint_wrench/joint_wrench_sensor.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index f9f5db13e2b8..4f59487e882d 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -98,9 +98,9 @@ def data(self) -> JointWrenchSensorData: Operations """ - def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: """Reset the sensor buffers for the given environments. - + Args: env_ids: the environment ids to reset. env_mask: the mask used to reset the environments. Shape is (num_envs).""" @@ -170,7 +170,7 @@ def _initialize_impl(self) -> None: def _update_buffers_impl(self, env_mask: wp.array) -> None: """Convert Newton's body_parent_f into INCOMING_JOINT_FRAME force and torque buffers. - + Args: env_mask: A mask containing which environments need to be updated. Shape is (num_envs) """ @@ -196,7 +196,7 @@ def _update_buffers_impl(self, env_mask: wp.array) -> None: def _invalidate_initialize_callback(self, event) -> None: """Drop view, cached sizes, and buffers; re-register the extended-state request. - + Args: event: An invalidate event. """ From 103ae68e6e04e11628b99712b1549093bd95dc1a Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 30 Apr 2026 16:21:55 +0200 Subject: [PATCH 13/16] Add PhysX joint wrench sensor Add the PhysX backend implementation for JointWrenchSensor and migrate body incoming wrench observations off ArticulationData. Remove the old articulation data accessor and document the Isaac Lab 3.0 migration path. --- docs/source/api/lab/isaaclab.sensors.rst | 6 + .../migration/migrating_to_isaaclab_3-0.rst | 72 ++++ source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 20 ++ .../articulation/base_articulation_data.py | 17 - .../isaaclab/envs/mdp/observations.py | 21 +- .../joint_wrench/base_joint_wrench_sensor.py | 33 +- .../base_joint_wrench_sensor_data.py | 8 +- .../joint_wrench/joint_wrench_sensor.py | 6 +- .../joint_wrench/joint_wrench_sensor_data.py | 5 +- .../assets/mock_articulation.py | 18 - .../test/assets/test_articulation_iface.py | 17 - .../test_mock_data_properties.py | 1 - source/isaaclab_newton/config/extension.toml | 2 +- source/isaaclab_newton/docs/CHANGELOG.rst | 12 + .../assets/articulation/articulation_data.py | 17 - .../joint_wrench/joint_wrench_sensor_data.py | 8 +- .../test/assets/test_articulation.py | 102 ------ .../test/sensors/test_joint_wrench_sensor.py | 5 +- source/isaaclab_ovphysx/config/extension.toml | 2 +- source/isaaclab_ovphysx/docs/CHANGELOG.rst | 12 + .../assets/articulation/articulation_data.py | 20 -- .../assets/benchmark_articulation_data.py | 1 - source/isaaclab_physx/config/extension.toml | 2 +- source/isaaclab_physx/docs/CHANGELOG.rst | 18 + .../assets/articulation/articulation_data.py | 31 -- .../isaaclab_physx/sensors/__init__.pyi | 3 + .../sensors/joint_wrench/__init__.py | 10 + .../sensors/joint_wrench/__init__.pyi | 9 + .../joint_wrench/joint_wrench_sensor.py | 166 +++++++++ .../joint_wrench/joint_wrench_sensor_data.py | 67 ++++ .../sensors/joint_wrench/kernels.py | 38 ++ .../test/assets/test_articulation.py | 102 ------ .../test/sensors/test_joint_wrench_sensor.py | 331 ++++++++++++++++++ source/isaaclab_tasks/config/extension.toml | 2 +- source/isaaclab_tasks/docs/CHANGELOG.rst | 14 + .../inhand_manipulation_env.py | 45 ++- .../shadow_hand/shadow_hand_vision_env.py | 10 +- .../manager_based/classic/ant/ant_env_cfg.py | 9 +- .../classic/humanoid/humanoid_env_cfg.py | 6 +- 40 files changed, 895 insertions(+), 375 deletions(-) create mode 100644 source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.py create mode 100644 source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.pyi create mode 100644 source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py create mode 100644 source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor_data.py create mode 100644 source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py create mode 100644 source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py diff --git a/docs/source/api/lab/isaaclab.sensors.rst b/docs/source/api/lab/isaaclab.sensors.rst index 15fa68e71349..1beccd5481f1 100644 --- a/docs/source/api/lab/isaaclab.sensors.rst +++ b/docs/source/api/lab/isaaclab.sensors.rst @@ -37,6 +37,7 @@ Imu ImuCfg JointWrenchSensor + JointWrenchSensorData JointWrenchSensorCfg Sensor Base @@ -200,6 +201,11 @@ Joint Wrench Sensor :inherited-members: :show-inheritance: +.. autoclass:: JointWrenchSensorData + :members: + :inherited-members: + :exclude-members: __init__ + .. autoclass:: JointWrenchSensorCfg :members: :inherited-members: diff --git a/docs/source/migration/migrating_to_isaaclab_3-0.rst b/docs/source/migration/migrating_to_isaaclab_3-0.rst index f90d47496d47..2e9fd76fcd80 100644 --- a/docs/source/migration/migrating_to_isaaclab_3-0.rst +++ b/docs/source/migration/migrating_to_isaaclab_3-0.rst @@ -158,6 +158,8 @@ The following sensor classes also remain in the ``isaaclab`` package with unchan - :class:`~isaaclab.sensors.Imu`, :class:`~isaaclab.sensors.ImuCfg`, :class:`~isaaclab.sensors.ImuData` - :class:`~isaaclab.sensors.Pva`, :class:`~isaaclab.sensors.PvaCfg`, :class:`~isaaclab.sensors.PvaData` - :class:`~isaaclab.sensors.FrameTransformer`, :class:`~isaaclab.sensors.FrameTransformerCfg`, :class:`~isaaclab.sensors.FrameTransformerData` +- :class:`~isaaclab.sensors.JointWrenchSensor`, :class:`~isaaclab.sensors.JointWrenchSensorCfg`, + :class:`~isaaclab.sensors.JointWrenchSensorData` These sensor classes now use factory patterns that automatically instantiate the appropriate backend implementation (PhysX by default), maintaining full backward compatibility. @@ -179,6 +181,7 @@ you can import from ``isaaclab_physx.sensors``: from isaaclab_physx.sensors import Imu, ImuData from isaaclab_physx.sensors import Pva, PvaData from isaaclab_physx.sensors import FrameTransformer, FrameTransformerData + from isaaclab_physx.sensors import JointWrenchSensor, JointWrenchSensorData New ``isaaclab_newton`` Extension @@ -188,6 +191,8 @@ A new extension ``isaaclab_newton`` provides Newton physics backend implementati - :class:`~isaaclab_newton.assets.Articulation` and :class:`~isaaclab_newton.assets.ArticulationData` - :class:`~isaaclab_newton.assets.RigidObject` and :class:`~isaaclab_newton.assets.RigidObjectData` +- :class:`~isaaclab_newton.sensors.JointWrenchSensor` and + :class:`~isaaclab_newton.sensors.JointWrenchSensorData` These classes implement the same base interfaces as their PhysX counterparts (:class:`~isaaclab.assets.BaseArticulation`, :class:`~isaaclab.assets.BaseRigidObject`), @@ -331,6 +336,73 @@ If you need to track sensor poses in world frame, please use a dedicated sensor sensor_quat = frame_transformer.data.target_quat_w +Articulation Joint Wrench Data Moved to ``JointWrenchSensor`` +------------------------------------------------------------- + +The ``ArticulationData.body_incoming_joint_wrench_b`` property has been removed. In +Isaac Lab 3.0, incoming joint reaction wrenches are exposed through +:class:`~isaaclab.sensors.JointWrenchSensor`, which has PhysX and Newton backend +implementations and returns separate force [N] and torque [N·m] buffers. + +**Before (Isaac Lab 2.x):** + +.. code-block:: python + + wrench_b = robot.data.body_incoming_joint_wrench_b.torch[:, body_ids] + +**After (Isaac Lab 3.x):** + +.. code-block:: python + + import torch + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.sensors import JointWrenchSensorCfg + + class MySceneCfg(InteractiveSceneCfg): + robot = ROBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + joint_wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + sensor = env.scene.sensors["joint_wrench"] + data = sensor.data + wrench_b = torch.cat( + ( + data.force.torch[:, body_ids], + data.torque.torch[:, body_ids], + ), + dim=-1, + ) + +Use :attr:`~isaaclab.sensors.BaseJointWrenchSensor.body_names` or +:meth:`~isaaclab.sensors.BaseJointWrenchSensor.find_bodies` to map sensor entries to +articulation body names. PhysX reports one entry for every link, including the articulation +root link. Newton reports the child bodies of reportable incoming joints. + +For manager-based environments, update observations that used the articulation data property to +depend on the joint-wrench sensor instead: + +.. code-block:: python + + import isaaclab.envs.mdp as mdp + from isaaclab.managers import SceneEntityCfg + from isaaclab.managers import ObservationTermCfg as ObsTerm + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.sensors import JointWrenchSensorCfg + + class MySceneCfg(InteractiveSceneCfg): + robot = ROBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + joint_wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + feet_body_forces = ObsTerm( + func=mdp.body_incoming_wrench, + params={ + "sensor_cfg": SceneEntityCfg( + "joint_wrench", + body_names=["left_foot", "right_foot"], + ) + }, + ) + + Multi-Backend Support: PresetCfg Pattern ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 322e44e742c9..70eb0d6ffa90 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "4.6.23" +version = "4.6.24" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 07007ad709f2..737a207efe56 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,26 @@ Changelog --------- +4.6.24 (2026-04-30) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed :func:`~isaaclab.envs.mdp.body_incoming_wrench` to read from + :class:`~isaaclab.sensors.JointWrenchSensor`. Pass + ``sensor_cfg=SceneEntityCfg("joint_wrench", body_names=...)`` instead of an + articulation asset config. + +Removed +^^^^^^^ + +* Removed ``BaseArticulationData.body_incoming_joint_wrench_b``. + Add :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and read + :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and + :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. + + 4.6.23 (2026-04-30) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py index fdd79ce6d474..89cd04b4b93a 100644 --- a/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py @@ -603,23 +603,6 @@ def body_com_pose_b(self) -> ProxyArray: """ raise NotImplementedError - @property - @abstractmethod - def body_incoming_joint_wrench_b(self) -> ProxyArray: - """Joint reaction wrench applied from body parent to child body in parent body frame. - - Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to - (num_instances, num_bodies, 6). All body reaction wrenches are provided including the root body to the - world of an articulation. - - For more information on joint wrenches, please check the `PhysX documentation`_ and the - underlying `PhysX Tensor API`_. - - .. _PhysX documentation: https://nvidia-omniverse.github.io/PhysX/physx/5.5.1/docs/Articulations.html#link-incoming-joint-force - .. _PhysX Tensor API: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.api.ArticulationView.get_link_incoming_joint_force - """ - raise NotImplementedError - ## # Joint state properties. ## diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 4fc24fc5b944..8f97da3595dd 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -23,7 +23,7 @@ if TYPE_CHECKING: from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv - from isaaclab.sensors import Camera, Imu, Pva, RayCaster, RayCasterCamera + from isaaclab.sensors import Camera, Imu, JointWrenchSensor, Pva, RayCaster, RayCasterCamera from isaaclab.envs.utils.io_descriptors import ( generic_io_descriptor, @@ -304,16 +304,21 @@ def height_scan(env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg, offset: float return sensor.data.pos_w.torch[:, 2].unsqueeze(1) - sensor.data.ray_hits_w.torch[..., 2] - offset -def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: - """Incoming spatial wrench on bodies of an articulation in the simulation world frame. +def body_incoming_wrench(env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Incoming spatial wrench [N, N·m] on bodies of an articulation in the sensor convention. - This is the 6-D wrench (force and torque) applied to the body link by the incoming joint force. + This is the 6-D wrench (force followed by torque) applied to the body link by the incoming joint force. """ # extract the used quantities (to enable type-hinting) - asset: Articulation = env.scene[asset_cfg.name] - # obtain the link incoming forces in world frame - body_incoming_joint_wrench_b = asset.data.body_incoming_joint_wrench_b.torch[:, asset_cfg.body_ids] - return body_incoming_joint_wrench_b.view(env.num_envs, -1) + sensor: JointWrenchSensor = env.scene.sensors[sensor_cfg.name] + sensor_data = sensor.data + force_data = sensor_data.force + torque_data = sensor_data.torque + if force_data is None or torque_data is None: + raise RuntimeError("Joint wrench sensor data is not initialized. Call sim.reset() before reading observations.") + force = force_data.torch[:, sensor_cfg.body_ids] + torque = torque_data.torch[:, sensor_cfg.body_ids] + return torch.cat((force, torque), dim=-1).view(env.num_envs, -1) def pva_orientation(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("pva")) -> torch.Tensor: diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py index e11811b451bb..158ba4a75dd7 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor.py @@ -6,10 +6,13 @@ from __future__ import annotations from abc import abstractmethod +from collections.abc import Sequence from typing import TYPE_CHECKING import warp as wp +import isaaclab.utils.string as string_utils + from ..sensor_base import SensorBase from .base_joint_wrench_sensor_data import BaseJointWrenchSensorData @@ -20,11 +23,10 @@ class BaseJointWrenchSensor(SensorBase): """The joint reaction wrench sensor. - Reports the incoming joint wrench on each joint of an articulation as a - split force [N] / torque [N·m] pair expressed in the - ``INCOMING_JOINT_FRAME`` convention (child-side joint frame, child-side - joint anchor reference point). Backends convert from their native - representation to this convention internally. + Reports incoming joint wrenches for the bodies selected by the backend as + split force [N] / torque [N·m] pairs expressed in the + ``INCOMING_JOINT_FRAME`` convention. Use :attr:`body_names` or + :meth:`find_bodies` to map entries to articulation bodies. """ cfg: JointWrenchSensorCfg @@ -57,6 +59,27 @@ def body_names(self) -> list[str]: """Ordered names of the bodies whose incoming joint wrench is reported.""" raise NotImplementedError + @property + def num_bodies(self) -> int: + """Number of bodies whose incoming joint wrench is reported.""" + return len(self.body_names) + + """ + Operations + """ + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find reported bodies based on name keys. + + Args: + name_keys: A regular expression or list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + The matching body indices and names. + """ + return string_utils.resolve_matching_names(name_keys, self.body_names, preserve_order) + """ Implementation - Abstract methods to be implemented by backend-specific subclasses. """ diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py index fd1380212990..282021a36784 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/base_joint_wrench_sensor_data.py @@ -22,8 +22,8 @@ def force(self) -> ProxyArray | None: Expressed in the frame selected by :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is - ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves - to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + ``(num_envs, num_bodies)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_bodies, 3)``. ``None`` before the simulation is initialized. """ raise NotImplementedError @@ -35,8 +35,8 @@ def torque(self) -> ProxyArray | None: Expressed in the frame selected by :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is - ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves - to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + ``(num_envs, num_bodies)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_bodies, 3)``. ``None`` before the simulation is initialized. """ raise NotImplementedError diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py index f4a88d94b978..40103b20e9f1 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor.py @@ -15,13 +15,15 @@ if TYPE_CHECKING: from isaaclab_newton.sensors.joint_wrench import JointWrenchSensor as NewtonJointWrenchSensor from isaaclab_newton.sensors.joint_wrench import JointWrenchSensorData as NewtonJointWrenchSensorData + from isaaclab_physx.sensors.joint_wrench import JointWrenchSensor as PhysXJointWrenchSensor + from isaaclab_physx.sensors.joint_wrench import JointWrenchSensorData as PhysXJointWrenchSensorData class JointWrenchSensor(FactoryBase, BaseJointWrenchSensor): """Factory for creating joint-wrench sensor instances.""" - data: BaseJointWrenchSensorData | NewtonJointWrenchSensorData + data: BaseJointWrenchSensorData | PhysXJointWrenchSensorData | NewtonJointWrenchSensorData - def __new__(cls, *args, **kwargs) -> BaseJointWrenchSensor | NewtonJointWrenchSensor: + def __new__(cls, *args, **kwargs) -> BaseJointWrenchSensor | PhysXJointWrenchSensor | NewtonJointWrenchSensor: """Create a new instance of a joint-wrench sensor based on the backend.""" return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py index 66bc9d1214bf..5872640d143c 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -15,11 +15,14 @@ if TYPE_CHECKING: from isaaclab_newton.sensors.joint_wrench import JointWrenchSensorData as NewtonJointWrenchSensorData + from isaaclab_physx.sensors.joint_wrench import JointWrenchSensorData as PhysXJointWrenchSensorData class JointWrenchSensorData(FactoryBase, BaseJointWrenchSensorData): """Factory for creating joint-wrench sensor data instances.""" - def __new__(cls, *args, **kwargs) -> BaseJointWrenchSensorData | NewtonJointWrenchSensorData: + def __new__( + cls, *args, **kwargs + ) -> BaseJointWrenchSensorData | PhysXJointWrenchSensorData | NewtonJointWrenchSensorData: """Create a new instance of joint-wrench sensor data based on the backend.""" return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py index aa72bf2bbfd9..b7a29a55b389 100644 --- a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py +++ b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py @@ -134,7 +134,6 @@ def __init__( # Body properties self._body_mass: wp.array | None = None self._body_inertia: wp.array | None = None - self._body_incoming_joint_wrench_b: wp.array | None = None # Tendon properties (fixed) self._fixed_tendon_stiffness: wp.array | None = None @@ -187,7 +186,6 @@ def __init__( self._body_com_pose_b_ta: ProxyArray | None = None self._body_mass_ta: ProxyArray | None = None self._body_inertia_ta: ProxyArray | None = None - self._body_incoming_joint_wrench_b_ta: ProxyArray | None = None self._fixed_tendon_stiffness_ta: ProxyArray | None = None self._fixed_tendon_damping_ta: ProxyArray | None = None self._fixed_tendon_limit_stiffness_ta: ProxyArray | None = None @@ -830,18 +828,6 @@ def body_inertia(self) -> ProxyArray: self._body_inertia_ta = ProxyArray(self._body_inertia) return self._body_inertia_ta - @property - def body_incoming_joint_wrench_b(self) -> ProxyArray: - """Body incoming joint wrenches. dtype=wp.spatial_vectorf, shape: (N, num_bodies).""" - if self._body_incoming_joint_wrench_b is None: - self._body_incoming_joint_wrench_b = wp.zeros( - (self._num_instances, self._num_bodies, 6), dtype=wp.float32, device=self.device - ).view(wp.spatial_vectorf) - self._body_incoming_joint_wrench_b_ta = None - if self._body_incoming_joint_wrench_b_ta is None: - self._body_incoming_joint_wrench_b_ta = ProxyArray(self._body_incoming_joint_wrench_b) - return self._body_incoming_joint_wrench_b_ta - # -- Derived properties -- @property @@ -1176,10 +1162,6 @@ def set_body_inertia(self, value: torch.Tensor) -> None: self._body_inertia = wp.from_torch(value.to(self.device).contiguous()) self._body_inertia_ta = None - def set_body_incoming_joint_wrench_b(self, value: torch.Tensor) -> None: - self._body_incoming_joint_wrench_b = wp.from_torch(value.to(self.device).contiguous()) - self._body_incoming_joint_wrench_b_ta = None - def set_fixed_tendon_stiffness(self, value: torch.Tensor) -> None: self._fixed_tendon_stiffness = wp.from_torch(value.to(self.device).contiguous()) self._fixed_tendon_stiffness_ta = None diff --git a/source/isaaclab/test/assets/test_articulation_iface.py b/source/isaaclab/test/assets/test_articulation_iface.py index 9682df92f3a5..115492da01b8 100644 --- a/source/isaaclab/test/assets/test_articulation_iface.py +++ b/source/isaaclab/test/assets/test_articulation_iface.py @@ -1018,23 +1018,6 @@ def test_body_inertia(self, backend, num_instances, num_joints, num_bodies, devi name="body_inertia", ) - @_backends - @_default_dims - @_default_devices - def test_body_incoming_joint_wrench_b( - self, backend, num_instances, num_joints, num_bodies, device, articulation_iface - ): - if backend == "newton": - pytest.xfail("Newton does not support joint wrench reporting") - art, _ = articulation_iface - art.data.update(dt=0.01) - _check_proxy_array( - art.data.body_incoming_joint_wrench_b, - expected_shape=(num_instances, num_bodies), - expected_dtype=wp.spatial_vectorf, - name="body_incoming_joint_wrench_b", - ) - @_backends @_default_dims @_default_devices diff --git a/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py b/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py index c688975ca3eb..b68bb8141e1c 100644 --- a/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py +++ b/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py @@ -438,7 +438,6 @@ def test_body_state_shapes(self, data, property_name, expected_shape): [ ("body_mass", (4, 13)), ("body_inertia", (4, 13, 9)), - ("body_incoming_joint_wrench_b", (4, 13, 6)), ], ) def test_body_property_shapes(self, data, property_name, expected_shape): diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 0a8eed8000c2..2e598bf86303 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/source/isaaclab_newton/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.26" +version = "0.5.27" # Description title = "Newton simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 9ef33bc51848..2325473353f1 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,18 @@ Changelog --------- +0.5.27 (2026-04-30) +~~~~~~~~~~~~~~~~~~~ + +Removed +^^^^^^^ + +* Removed the unimplemented ``ArticulationData.body_incoming_joint_wrench_b`` + accessor. Add :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene + and read :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and + :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. + + 0.5.26 (2026-04-30) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index ff74915db71b..a22ba73e1725 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -805,22 +805,6 @@ def body_com_pose_b(self) -> ProxyArray: self._body_com_pose_b.timestamp = self._sim_timestamp return self._body_com_pose_b_ta - @property - def body_incoming_joint_wrench_b(self) -> ProxyArray: - """Joint reaction wrench applied from body parent to child body in parent body frame. - - Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to - (num_instances, num_bodies, 6). All body reaction wrenches are provided including the root body to the - world of an articulation. - - For more information on joint wrenches, please check the `PhysX documentation`_ and the - underlying `PhysX Tensor API`_. - - .. _PhysX documentation: https://nvidia-omniverse.github.io/PhysX/physx/5.5.1/docs/Articulations.html#link-incoming-joint-force - .. _PhysX Tensor API: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.api.ArticulationView.get_link_incoming_joint_force - """ - raise NotImplementedError("Not implemented for Newton") - """ Joint state properties. """ @@ -1526,7 +1510,6 @@ def _create_buffers(self) -> None: shape=(self._num_instances, self._num_joints), dtype=wp.float32, device=self.device ) # Empty memory pre-allocations - self._body_incoming_joint_wrench_b = None self._root_link_lin_vel_b = None self._root_link_ang_vel_b = None self._root_com_lin_vel_b = None diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py index 640fd784978c..76c5a565bdfb 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -27,8 +27,8 @@ def force(self) -> ProxyArray | None: Expressed in the frame selected by :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is - ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves - to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + ``(num_envs, num_bodies)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_bodies, 3)``. ``None`` before the simulation is initialized. """ if self._force is None: @@ -43,8 +43,8 @@ def torque(self) -> ProxyArray | None: Expressed in the frame selected by :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is - ``(num_envs, num_joints)``, dtype ``wp.vec3f``. In torch this resolves - to ``(num_envs, num_joints, 3)``. ``None`` before the simulation is + ``(num_envs, num_bodies)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_bodies, 3)``. ``None`` before the simulation is initialized. """ if self._torque is None: diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 5a4e77fd9eaf..a5eacf95045e 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2006,108 +2006,6 @@ def test_write_root_state( torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_link_vel_w.torch) -@pytest.mark.isaacsim_ci -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.parametrize("articulation_type", ["single_joint_implicit"]) -@pytest.mark.xfail(reason="Newton body_parent_f uses different convention than PhysX get_link_incoming_joint_force") -def test_body_incoming_joint_wrench_b_single_joint(sim, num_articulations, device, articulation_type): - """Test the data.body_incoming_joint_wrench_b buffer is populated correctly and statically correct for single joint. - - This test verifies that: - 1. The body incoming joint wrench buffer has correct shape - 2. The wrench values are statically correct for a single joint - 3. The wrench values match expected values from gravity and external forces - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Resolve body indices by name (ordering may differ across physics backends) - arm_idx = articulation.body_names.index("Arm") - root_idx = articulation.body_names.index("CenterPivot") - # apply external force - external_force_vector_b = torch.zeros((num_articulations, articulation.num_bodies, 3), device=device) - external_force_vector_b[:, arm_idx, 1] = 10.0 # 10 N in Y direction - external_torque_vector_b = torch.zeros((num_articulations, articulation.num_bodies, 3), device=device) - external_torque_vector_b[:, arm_idx, 2] = 10.0 # 10 Nm in z direction - - # apply action to the articulation - joint_pos = torch.ones_like(articulation.data.joint_pos.torch) * 1.5708 / 2.0 - articulation.write_joint_position_to_sim_index( - position=torch.ones_like(articulation.data.joint_pos.torch), - ) - articulation.write_joint_velocity_to_sim_index( - velocity=torch.zeros_like(articulation.data.joint_vel.torch), - ) - articulation.set_joint_position_target_index(target=joint_pos) - articulation.write_data_to_sim() - for _ in range(50): - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_force_vector_b, torques=external_torque_vector_b - ) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # check shape - assert articulation.data.body_incoming_joint_wrench_b.torch.shape == ( - num_articulations, - articulation.num_bodies, - 6, - ) - - # calculate expected static - mass = articulation.data.body_mass.torch.to("cpu") - pos_w = articulation.data.body_pos_w.torch - quat_w = articulation.data.body_quat_w.torch - - mass_link2 = mass[:, arm_idx].view(num_articulations, -1) - gravity = torch.tensor(sim.cfg.gravity, device="cpu").repeat(num_articulations, 1).view((num_articulations, 3)) - - # NOTE: the com and link pose for single joint are colocated - weight_vector_w = mass_link2 * gravity - # expected wrench from link mass and external wrench - # The incoming joint wrench is the constraint/support force from parent onto child (body1=Arm), - # expressed in Arm's frame. In static equilibrium this equals -(gravity + external forces on Arm). - total_force_w = weight_vector_w.to(device) + math_utils.quat_apply( - quat_w[:, arm_idx, :], external_force_vector_b[:, arm_idx, :] - ) - total_torque_w = torch.cross( - pos_w[:, arm_idx, :].to(device) - pos_w[:, root_idx, :].to(device), - total_force_w, - dim=-1, - ) + math_utils.quat_apply(quat_w[:, arm_idx, :], external_torque_vector_b[:, arm_idx, :]) - expected_wrench = torch.zeros((num_articulations, 6), device=device) - expected_wrench[:, :3] = math_utils.quat_apply( - math_utils.quat_conjugate(quat_w[:, arm_idx, :]), - -total_force_w, - ) - expected_wrench[:, 3:] = math_utils.quat_apply( - math_utils.quat_conjugate(quat_w[:, arm_idx, :]), - -total_torque_w, - ) - - # check value of last joint wrench - torch.testing.assert_close( - expected_wrench, - articulation.data.body_incoming_joint_wrench_b.torch[:, arm_idx, :].squeeze(1), - atol=1e-2, - rtol=1e-3, - ) - - @pytest.mark.isaacsim_ci @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) 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 7e0ac0ea6960..dba86b2c0c8c 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -314,9 +314,8 @@ def test_force_and_torque_components_at_rest(sim): def test_wrench_with_external_force_and_torque(sim): """Full analytical wrench validation with external force and torque applied. - Mirrors the PhysX ``test_body_incoming_joint_wrench_b_single_joint`` pattern: - apply a known wrench, settle, compute the expected reaction wrench analytically, - and compare component-by-component. + Applies a known wrench, settles, computes the expected reaction wrench analytically, + and compares component-by-component. """ scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) sim.reset() diff --git a/source/isaaclab_ovphysx/config/extension.toml b/source/isaaclab_ovphysx/config/extension.toml index 8648b7fa9587..1e541402d828 100644 --- a/source/isaaclab_ovphysx/config/extension.toml +++ b/source/isaaclab_ovphysx/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.1.2" +version = "0.1.3" # Description title = "OvPhysX simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_ovphysx/docs/CHANGELOG.rst b/source/isaaclab_ovphysx/docs/CHANGELOG.rst index b2eb969d7845..109440081a95 100644 --- a/source/isaaclab_ovphysx/docs/CHANGELOG.rst +++ b/source/isaaclab_ovphysx/docs/CHANGELOG.rst @@ -1,6 +1,18 @@ Changelog --------- +0.1.3 (2026-04-30) +~~~~~~~~~~~~~~~~~~ + +Removed +^^^^^^^ + +* Removed ``ArticulationData.body_incoming_joint_wrench_b`` + to match the shared articulation data API. Code that needs incoming joint + reaction wrenches should use a backend joint-wrench sensor instead of the + articulation data object. + + 0.1.2 (2026-04-23) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py index fe264924c184..10c7e4b7ecd6 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py @@ -810,23 +810,6 @@ def body_com_pose_b(self) -> ProxyArray: self._body_com_pose_b_ta = ProxyArray(self._body_com_pose_b.data) return self._body_com_pose_b_ta - @property - def body_incoming_joint_wrench_b(self) -> ProxyArray: - """Incoming joint wrenches on each body in the body frame [N, N*m]. - - Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to - (num_instances, num_bodies, 6). - - All body reaction wrenches are provided including the root body to the world of an articulation. - """ - self._read_spatial_vector_binding( - TT.LINK_INCOMING_JOINT_FORCE, - self._body_incoming_joint_wrench_buf, - ) - if self._body_incoming_joint_wrench_b_ta is None: - self._body_incoming_joint_wrench_b_ta = ProxyArray(self._body_incoming_joint_wrench_buf.data) - return self._body_incoming_joint_wrench_b_ta - """ Joint state properties. """ @@ -1361,8 +1344,6 @@ def _create_buffers(self) -> None: # noqa: C901 self._body_com_pose_w = TimestampedBuffer((N, L), dev, wp.transformf) self._body_com_vel_w = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) self._body_com_acc_w = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) - self._body_incoming_joint_wrench_buf = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) - # -- Joint state buffers self._joint_pos_buf = TimestampedBuffer((N, D), dev, wp.float32) self._joint_vel_buf = TimestampedBuffer((N, D), dev, wp.float32) @@ -1584,7 +1565,6 @@ def _pin_proxy_arrays(self) -> None: self._body_com_pose_w_ta: ProxyArray | None = None self._body_com_acc_w_ta: ProxyArray | None = None self._body_com_pose_b_ta: ProxyArray | None = None - self._body_incoming_joint_wrench_b_ta: ProxyArray | None = None # Body properties self._body_mass_ta: ProxyArray | None = None self._body_inertia_ta: ProxyArray | None = None diff --git a/source/isaaclab_physx/benchmark/assets/benchmark_articulation_data.py b/source/isaaclab_physx/benchmark/assets/benchmark_articulation_data.py index 5778f16017bc..f463f73ea6aa 100644 --- a/source/isaaclab_physx/benchmark/assets/benchmark_articulation_data.py +++ b/source/isaaclab_physx/benchmark/assets/benchmark_articulation_data.py @@ -131,7 +131,6 @@ "spatial_tendon_damping", "spatial_tendon_limit_stiffness", "spatial_tendon_offset", - "body_incoming_joint_wrench_b", } # Removed default_* properties that raise RuntimeError diff --git a/source/isaaclab_physx/config/extension.toml b/source/isaaclab_physx/config/extension.toml index 3c0711934431..5c63b0e6322f 100644 --- a/source/isaaclab_physx/config/extension.toml +++ b/source/isaaclab_physx/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.28" +version = "0.5.29" # Description title = "PhysX simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index 5f284dc3c891..d403a94f541f 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -1,6 +1,24 @@ Changelog --------- +0.5.29 (2026-04-30) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_physx.sensors.JointWrenchSensor` for reading PhysX + incoming joint reaction wrenches as split force [N] and torque [N·m] buffers. + +Removed +^^^^^^^ + +* Removed ``ArticulationData.body_incoming_joint_wrench_b``. + Add :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and read + :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and + :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. + + 0.5.28 (2026-04-29) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py index 0e27e05bd141..8c2056d9cbfa 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py @@ -850,33 +850,6 @@ def body_com_pose_b(self) -> ProxyArray: self._body_com_pose_b_ta = ProxyArray(self._body_com_pose_b.data) return self._body_com_pose_b_ta - @property - def body_incoming_joint_wrench_b(self) -> ProxyArray: - """Joint reaction wrench applied to each body through its incoming joint, expressed in that body's frame. - - Shape is (num_instances, num_bodies, 6). All body reaction wrenches are provided including the root body to the - world of an articulation. - - .. note:: - PhysX expresses this wrench in the frame of ``body1`` as defined in the USD joint, which corresponds to - the child body's own frame when the USD convention is ``body0`` = parent, ``body1`` = child. - - For more information on joint wrenches, please check the `PhysX documentation`_ and the underlying - `PhysX Tensor API`_. - - .. _`PhysX documentation`: https://nvidia-omniverse.github.io/PhysX/physx/5.5.1/docs/Articulations.html#link-incoming-joint-force - .. _`PhysX Tensor API`: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.api.ArticulationView.get_link_incoming_joint_force - """ - - if self._body_incoming_joint_wrench_b.timestamp < self._sim_timestamp: - self._body_incoming_joint_wrench_b.data = self._root_view.get_link_incoming_joint_force().view( - wp.spatial_vectorf - ) - self._body_incoming_joint_wrench_b.timestamp = self._sim_timestamp - if self._body_incoming_joint_wrench_b_ta is None: - self._body_incoming_joint_wrench_b_ta = ProxyArray(self._body_incoming_joint_wrench_b.data) - return self._body_incoming_joint_wrench_b_ta - """ Joint state properties. """ @@ -1382,9 +1355,6 @@ def _create_buffers(self) -> None: self._joint_pos = TimestampedBuffer((self._num_instances, self._num_joints), self.device, wp.float32) self._joint_vel = TimestampedBuffer((self._num_instances, self._num_joints), self.device, wp.float32) self._joint_acc = TimestampedBuffer((self._num_instances, self._num_joints), self.device, wp.float32) - self._body_incoming_joint_wrench_b = TimestampedBuffer( - (self._num_instances, self._num_bodies, self._num_joints), self.device, wp.spatial_vectorf - ) # -- derived properties (these are cached to avoid repeated memory allocations) self._projected_gravity_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) self._heading_w = TimestampedBuffer((self._num_instances), self.device, wp.float32) @@ -1555,7 +1525,6 @@ def _pin_proxy_arrays(self) -> None: self._body_com_vel_w_ta: ProxyArray | None = None self._body_com_acc_w_ta: ProxyArray | None = None self._body_com_pose_b_ta: ProxyArray | None = None - self._body_incoming_joint_wrench_b_ta: ProxyArray | None = None # Body properties self._body_mass_ta: ProxyArray | None = None self._body_inertia_ta: ProxyArray | None = None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sensors/__init__.pyi index 0eba5ef7bdcf..e536b281952b 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sensors/__init__.pyi @@ -11,6 +11,8 @@ __all__ = [ "FrameTransformerData", "Imu", "ImuData", + "JointWrenchSensor", + "JointWrenchSensorData", "Pva", "PvaData", ] @@ -18,4 +20,5 @@ __all__ = [ from .contact_sensor import ContactSensor, ContactSensorData, ContactSensorCfg from .frame_transformer import FrameTransformer, FrameTransformerData from .imu import Imu, ImuData +from .joint_wrench import JointWrenchSensor, JointWrenchSensorData from .pva import Pva, PvaData diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.py new file mode 100644 index 000000000000..539f0250a568 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for the PhysX joint-wrench sensor.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.pyi new file mode 100644 index 000000000000..b2bcd3582d44 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/__init__.pyi @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = ["JointWrenchSensor", "JointWrenchSensorData"] + +from .joint_wrench_sensor import JointWrenchSensor +from .joint_wrench_sensor_data import JointWrenchSensorData diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py new file mode 100644 index 000000000000..e2124f244834 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py @@ -0,0 +1,166 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# pyright: reportPrivateUsage=false + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import warp as wp + +from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor + +from isaaclab_physx.physics import PhysxManager as SimulationManager + +from .joint_wrench_sensor_data import JointWrenchSensorData +from .kernels import joint_wrench_reset_kernel, joint_wrench_split_kernel + +if TYPE_CHECKING: + import omni.physics.tensors.api as physx + + from isaaclab.sensors.joint_wrench import JointWrenchSensorCfg + +logger = logging.getLogger(__name__) + + +class JointWrenchSensor(BaseJointWrenchSensor): + """PhysX joint reaction wrench sensor. + + The sensor reads PhysX's incoming joint wrench for every articulation link + and exposes the linear force [N] and angular torque [N·m] components. PhysX + reports each wrench in the frame of ``body1`` from the USD joint, which is + the child body's frame for the standard ``body0`` = parent, ``body1`` = + child convention. The root body's entry is included. + + :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at the + articulation root prim in every environment. + """ + + cfg: JointWrenchSensorCfg + """The configuration parameters.""" + + __backend_name__: str = "physx" + """The name of the backend for the joint wrench sensor.""" + + def __init__(self, cfg: JointWrenchSensorCfg): + """Initialize the PhysX joint-wrench sensor. + + Args: + cfg: The configuration parameters. + """ + super().__init__(cfg) + + self._data = JointWrenchSensorData() + self._physics_sim_view = None + self._root_view: physx.ArticulationView | None = None + self._num_bodies: int = 0 + + def __str__(self) -> str: + """String representation of the sensor instance.""" + return ( + f"Joint wrench sensor @ '{self.cfg.prim_path}': \n" + f"\tbackend : physx\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of bodies : {self._num_bodies}\n" + f"\tbody names : {self.body_names}\n" + ) + + """ + Properties + """ + + @property + def body_names(self) -> list[str]: + """Ordered names of the bodies whose incoming joint wrench is reported.""" + return self._data._body_names + + @property + def data(self) -> JointWrenchSensorData: + """The joint-wrench sensor data.""" + self._update_outdated_buffers() + return self._data + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + """Reset the sensor buffers for the given environments. + + Args: + env_ids: The environment ids to reset. + env_mask: The mask used to reset the environments. Shape is ``(num_envs,)``. + """ + if self._data._force is None or self._data._torque is None: + return + env_mask = self._resolve_indices_and_mask(env_ids, env_mask) + super().reset(None, env_mask) + wp.launch( + joint_wrench_reset_kernel, + dim=(self._num_envs, self._num_bodies), + inputs=[env_mask, self._data._force, self._data._torque], + device=self._device, + ) + + """ + Implementation + """ + + def _initialize_impl(self) -> None: + """PHYSICS_READY callback: builds the articulation view and allocates buffers.""" + super()._initialize_impl() + + self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._root_view = self._physics_sim_view.create_articulation_view(self.cfg.prim_path.replace(".*", "*")) + if self._root_view._backend is None: + raise RuntimeError(f"Failed to create articulation view at: {self.cfg.prim_path}. Check PhysX logs.") + + self._num_bodies = self._root_view.shared_metatype.link_count + if self._num_bodies == 0: + raise RuntimeError(f"Joint wrench sensor matched zero bodies at '{self.cfg.prim_path}'.") + + self._data._body_names = list(self._root_view.shared_metatype.link_names) + self._data.create_buffers(num_envs=self._num_envs, num_bodies=self._num_bodies, device=self._device) + + logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_bodies} bodies") + + def _update_buffers_impl(self, env_mask: wp.array) -> None: + """Read PhysX incoming joint wrenches and split them into force / torque buffers. + + Args: + env_mask: A mask containing which environments need to be updated. Shape is ``(num_envs,)``. + """ + if self._root_view is None: + raise RuntimeError( + f"Joint wrench sensor '{self.cfg.prim_path}': not initialized." + " Access sensor data only after sim.reset() has been called." + ) + + incoming_joint_wrench = self._root_view.get_link_incoming_joint_force().view(wp.spatial_vectorf) + wp.launch( + joint_wrench_split_kernel, + dim=(self._num_envs, self._num_bodies), + inputs=[env_mask, incoming_joint_wrench, self._data._force, self._data._torque], + device=self._device, + ) + + def _invalidate_initialize_callback(self, event) -> None: + """Drop view, cached sizes, and buffers when physics stops. + + Args: + event: An invalidate event. + """ + super()._invalidate_initialize_callback(event) + self._physics_sim_view = None + self._root_view = None + self._num_bodies = 0 + self._data._force = None + self._data._torque = None + self._data._body_names = [] + self._data._force_ta = None + self._data._torque_ta = None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor_data.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor_data.py new file mode 100644 index 000000000000..80cea4c1a5ce --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor_data.py @@ -0,0 +1,67 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import warp as wp + +from isaaclab.sensors.joint_wrench import BaseJointWrenchSensorData +from isaaclab.utils.warp import ProxyArray + + +class JointWrenchSensorData(BaseJointWrenchSensorData): + """Data container for the PhysX joint-wrench sensor.""" + + def __init__(self): + self._force: wp.array | None = None + self._torque: wp.array | None = None + self._body_names: list[str] = [] + self._force_ta: ProxyArray | None = None + self._torque_ta: ProxyArray | None = None + + @property + def force(self) -> ProxyArray | None: + """Linear component of the incoming joint wrench [N]. + + Expressed in the frame selected by + :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is + ``(num_envs, num_bodies)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_bodies, 3)``. ``None`` before the simulation is + initialized. + """ + if self._force is None: + return None + if self._force_ta is None: + self._force_ta = ProxyArray(self._force) + return self._force_ta + + @property + def torque(self) -> ProxyArray | None: + """Angular component of the incoming joint wrench [N·m]. + + Expressed in the frame selected by + :attr:`~isaaclab.sensors.JointWrenchSensorCfg.convention`. Shape is + ``(num_envs, num_bodies)``, dtype ``wp.vec3f``. In torch this resolves + to ``(num_envs, num_bodies, 3)``. ``None`` before the simulation is + initialized. + """ + if self._torque is None: + return None + if self._torque_ta is None: + self._torque_ta = ProxyArray(self._torque) + return self._torque_ta + + def create_buffers(self, num_envs: int, num_bodies: int, device: str) -> None: + """Allocate internal buffers. + + Args: + num_envs: Number of environments. + num_bodies: Number of bodies with incoming joint wrench reports. + device: Device for array storage. + """ + self._force = wp.zeros((num_envs, num_bodies), dtype=wp.vec3f, device=device) + self._torque = wp.zeros((num_envs, num_bodies), dtype=wp.vec3f, device=device) + self._force_ta = None + self._torque_ta = None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py new file mode 100644 index 000000000000..3abc7f8c67b4 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import warp as wp + + +@wp.kernel +def joint_wrench_split_kernel( + env_mask: wp.array(dtype=wp.bool), + incoming_joint_wrench: wp.array(dtype=wp.spatial_vectorf, ndim=2), + out_force: wp.array(dtype=wp.vec3f, ndim=2), + out_torque: wp.array(dtype=wp.vec3f, ndim=2), +): + """Split PhysX incoming joint spatial wrenches into force and torque components.""" + env, body = wp.tid() + if not env_mask[env]: + return + + wrench = incoming_joint_wrench[env, body] + out_force[env, body] = wp.spatial_top(wrench) + out_torque[env, body] = wp.spatial_bottom(wrench) + + +@wp.kernel +def joint_wrench_reset_kernel( + env_mask: wp.array(dtype=wp.bool), + out_force: wp.array(dtype=wp.vec3f, ndim=2), + out_torque: wp.array(dtype=wp.vec3f, ndim=2), +): + """Zero force and torque entries for the environments selected by ``env_mask``.""" + env, body = wp.tid() + if not env_mask[env]: + return + + out_force[env, body] = wp.vec3f(0.0, 0.0, 0.0) + out_torque[env, body] = wp.vec3f(0.0, 0.0, 0.0) diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 6f57999db307..3687dae5961d 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -1830,108 +1830,6 @@ def test_write_root_state(sim, num_articulations, device, with_offset, state_loc torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_link_vel_w.torch) -@pytest.mark.parametrize("num_articulations", [1, 2]) -@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci -def test_body_incoming_joint_wrench_b_single_joint(sim, num_articulations, device): - """Test the data.body_incoming_joint_wrench_b buffer is populated correctly and statically correct for single joint. - - This test verifies that: - 1. The body incoming joint wrench buffer has correct shape - 2. The wrench values are statically correct for a single joint - 3. The wrench values match expected values from gravity and external forces - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - device: The device to run the simulation on - """ - articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit") - articulation, _ = generate_articulation( - articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device - ) - - # Play the simulator - sim.reset() - - # Resolve body indices by name (ordering may differ across physics backends) - arm_idx = articulation.body_names.index("Arm") - root_idx = articulation.body_names.index("CenterPivot") - # apply external force - external_force_vector_b = torch.zeros((num_articulations, articulation.num_bodies, 3), device=device) - external_force_vector_b[:, arm_idx, 1] = 10.0 # 10 N in Y direction - external_torque_vector_b = torch.zeros((num_articulations, articulation.num_bodies, 3), device=device) - external_torque_vector_b[:, arm_idx, 2] = 10.0 # 10 Nm in z direction - - # apply action to the articulation - joint_pos = torch.ones_like(articulation.data.joint_pos.torch) * 1.5708 / 2.0 - articulation.write_joint_position_to_sim_index( - position=torch.ones_like(articulation.data.joint_pos.torch), - ) - articulation.write_joint_velocity_to_sim_index( - velocity=torch.zeros_like(articulation.data.joint_vel.torch), - ) - articulation.set_joint_position_target_index(target=joint_pos) - articulation.write_data_to_sim() - for _ in range(50): - articulation.permanent_wrench_composer.set_forces_and_torques_index( - forces=external_force_vector_b, torques=external_torque_vector_b - ) - articulation.write_data_to_sim() - # perform step - sim.step() - # update buffers - articulation.update(sim.cfg.dt) - - # check shape - assert articulation.data.body_incoming_joint_wrench_b.torch.shape == ( - num_articulations, - articulation.num_bodies, - 6, - ) - - # calculate expected static - mass = articulation.data.body_mass.torch.to("cpu") - pos_w = articulation.data.body_pos_w.torch - quat_w = articulation.data.body_quat_w.torch - - mass_link2 = mass[:, arm_idx].view(num_articulations, -1) - gravity = torch.tensor(sim.cfg.gravity, device="cpu").repeat(num_articulations, 1).view((num_articulations, 3)) - - # NOTE: the com and link pose for single joint are colocated - weight_vector_w = mass_link2 * gravity - # expected wrench from link mass and external wrench - # PhysX reports the incoming joint wrench as the force FROM body0 ONTO body1 (body1's frame). - # The USD asset defines body0=CenterPivot, body1=Arm, so the wrench is the constraint/support - # force from CenterPivot onto Arm, expressed in Arm's frame. - # In static equilibrium this equals -(gravity + external forces on Arm). - total_force_w = weight_vector_w.to(device) + math_utils.quat_apply( - quat_w[:, arm_idx, :], external_force_vector_b[:, arm_idx, :] - ) - total_torque_w = torch.cross( - pos_w[:, arm_idx, :].to(device) - pos_w[:, root_idx, :].to(device), - total_force_w, - dim=-1, - ) + math_utils.quat_apply(quat_w[:, arm_idx, :], external_torque_vector_b[:, arm_idx, :]) - expected_wrench = torch.zeros((num_articulations, 6), device=device) - expected_wrench[:, :3] = math_utils.quat_apply( - math_utils.quat_conjugate(quat_w[:, arm_idx, :]), - -total_force_w, - ) - expected_wrench[:, 3:] = math_utils.quat_apply( - math_utils.quat_conjugate(quat_w[:, arm_idx, :]), - -total_torque_w, - ) - - # check value of last joint wrench - torch.testing.assert_close( - expected_wrench, - articulation.data.body_incoming_joint_wrench_b.torch[:, arm_idx, :].squeeze(1), - atol=1e-2, - rtol=1e-3, - ) - - @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci def test_setting_articulation_root_prim_path(sim, device): diff --git a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py new file mode 100644 index 000000000000..abdfcc754b1b --- /dev/null +++ b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py @@ -0,0 +1,331 @@ +# 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 + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +import torch +import warp as wp +from isaaclab_physx.physics import PhysxCfg + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import JointWrenchSensor, JointWrenchSensorCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + + +def _make_single_joint_articulation_cfg() -> ArticulationCfg: + """Single-joint revolute test articulation (root ``CenterPivot`` + arm ``Arm``).""" + return ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", + joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), + ), + actuators={ + "joint": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=2000.0, + damping=100.0, + ), + }, + init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + + +def _make_cartpole_articulation_cfg(pole_damping: float = 0.0) -> ArticulationCfg: + """Two-joint cartpole articulation (cart + pole). + + Args: + pole_damping: Damping for the cart-to-pole revolute joint. + """ + return ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Classic/Cartpole/cartpole.usd", + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 2.0), + joint_pos={"slider_to_cart": 0.0, "cart_to_pole": 0.0}, + ), + actuators={ + "cart_actuator": ImplicitActuatorCfg( + joint_names_expr=["slider_to_cart"], effort_limit_sim=400.0, stiffness=0.0, damping=10.0 + ), + "pole_actuator": ImplicitActuatorCfg( + joint_names_expr=["cart_to_pole"], effort_limit_sim=400.0, stiffness=0.0, damping=pole_damping + ), + }, + ) + + +@configclass +class _SingleJointSceneCfg(InteractiveSceneCfg): + """Scene with a single-joint articulation and the joint-wrench sensor.""" + + env_spacing = 2.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = _make_single_joint_articulation_cfg() + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class _CartpoleSceneCfg(InteractiveSceneCfg): + """Scene with a cartpole (2-joint) articulation and the joint-wrench sensor.""" + + env_spacing = 4.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = _make_cartpole_articulation_cfg() + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class _CartpoleDampedSceneCfg(InteractiveSceneCfg): + """Cartpole with pole damping for steady-state physics validation tests.""" + + env_spacing = 4.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = _make_cartpole_articulation_cfg(pole_damping=10.0) + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + +@pytest.fixture +def sim(): + """Simulation context using the PhysX backend.""" + sim_cfg = SimulationCfg( + dt=1.0 / 120.0, + physics=PhysxCfg(), + ) + with sim_utils.build_simulation_context(sim_cfg=sim_cfg) as sim_ctx: + sim_ctx._app_control_on_stop_handle = None + yield sim_ctx + + +def _physx_incoming_joint_wrench(sensor: JointWrenchSensor) -> torch.Tensor: + """Read the raw PhysX incoming joint wrench tensor. + + PhysX reports spatial vectors as force followed by torque. Shape is + ``(num_envs, num_bodies, 6)``. + """ + raw_wrench = sensor._root_view.get_link_incoming_joint_force().view(wp.spatial_vectorf) + return wp.to_torch(raw_wrench) + + +def _assert_sensor_matches_physx_tensor(sensor: JointWrenchSensor) -> None: + """Compare the sensor buffers to the raw PhysX tensor API.""" + raw_wrench = _physx_incoming_joint_wrench(sensor) + sensor_data = sensor.data + + torch.testing.assert_close(sensor_data.force.torch, raw_wrench[..., :3]) + torch.testing.assert_close(sensor_data.torque.torch, raw_wrench[..., 3:]) + + +# --------------------------------------------------------------------------- +# Sensor data — pre-init contract +# --------------------------------------------------------------------------- + + +def test_data_before_init_is_none(): + """``force``/``torque`` return ``None`` before :meth:`create_buffers` runs.""" + from isaaclab_physx.sensors.joint_wrench import JointWrenchSensorData + + data = JointWrenchSensorData() + assert data.force is None + assert data.torque is None + + +# --------------------------------------------------------------------------- +# Initialization and shapes +# --------------------------------------------------------------------------- + + +def test_initialization_and_shapes(sim): + """Sensor initializes on sim reset and exposes correctly-shaped buffers.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) + sim.reset() + + robot: Articulation = scene["robot"] + sensor: JointWrenchSensor = scene["wrench"] + sim.step() + scene.update(sim.get_physics_dt()) + + # PhysX reports one incoming joint wrench per articulation link, including the root link. + num_envs = 2 + num_bodies = robot.num_bodies + assert sensor.data.force.torch.shape == (num_envs, num_bodies, 3) + assert sensor.data.torque.torch.shape == (num_envs, num_bodies, 3) + assert sensor.body_names == robot.body_names + assert sensor.find_bodies("Arm") == ([robot.body_names.index("Arm")], ["Arm"]) + _assert_sensor_matches_physx_tensor(sensor) + + +def test_multi_body_articulation(sim): + """Cartpole exposes a wrench for each link labelled by body name.""" + scene = InteractiveScene(_CartpoleSceneCfg(num_envs=2)) + sim.reset() + + robot: Articulation = scene["robot"] + sensor: JointWrenchSensor = scene["wrench"] + sim.step() + scene.update(sim.get_physics_dt()) + + num_envs = 2 + num_bodies = robot.num_bodies + assert sensor.data.force.torch.shape == (num_envs, num_bodies, 3) + assert sensor.data.torque.torch.shape == (num_envs, num_bodies, 3) + assert sensor.body_names == robot.body_names + assert len(sensor.body_names) == num_bodies + _assert_sensor_matches_physx_tensor(sensor) + + +# --------------------------------------------------------------------------- +# Physical correctness +# --------------------------------------------------------------------------- + + +def test_force_and_torque_components_at_rest(sim): + """Component-level validation of force and torque against the PhysX tensor API.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + robot: Articulation = scene["robot"] + for _ in range(400): + sim.step() + scene.update(sim.get_physics_dt()) + + _assert_sensor_matches_physx_tensor(sensor) + + arm_idx = robot.body_names.index("Arm") + raw_wrench = _physx_incoming_joint_wrench(sensor) + assert torch.any(raw_wrench[:, arm_idx, :] != 0.0) + + +def test_wrench_with_external_force_and_torque(sim): + """Full wrench validation with external force and torque applied.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + robot: Articulation = scene["robot"] + arm_idx = robot.body_names.index("Arm") + + # Apply 10 N in body-Y and 10 N·m in body-Z on the arm (matches Newton test). + ext_force_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) + ext_force_b[:, arm_idx, 1] = 10.0 + ext_torque_b = torch.zeros((1, robot.num_bodies, 3), device=sim.device) + ext_torque_b[:, arm_idx, 2] = 10.0 + + for _ in range(800): + robot.permanent_wrench_composer.set_forces_and_torques_index(forces=ext_force_b, torques=ext_torque_b) + robot.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + _assert_sensor_matches_physx_tensor(sensor) + + raw_wrench = _physx_incoming_joint_wrench(sensor) + assert torch.any(raw_wrench[:, arm_idx, :] != 0.0) + + +def test_interior_joint_wrench_at_rest(sim): + """Interior joint wrench matches the raw PhysX incoming-joint tensor. + + The cartpole has an interior joint (``slider_to_cart``) and a terminal + joint (``cart_to_pole``). PhysX reports one entry for every link, so this + test compares all link entries, including the cart link controlled by the + interior joint, against the underlying tensor API. + """ + scene = InteractiveScene(_CartpoleDampedSceneCfg(num_envs=1)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + robot: Articulation = scene["robot"] + + for _ in range(800): + sim.step() + scene.update(sim.get_physics_dt()) + + _assert_sensor_matches_physx_tensor(sensor) + + cart_idx = robot.body_names.index("cart") + raw_wrench = _physx_incoming_joint_wrench(sensor) + assert torch.any(raw_wrench[:, cart_idx, :] != 0.0) + + +# --------------------------------------------------------------------------- +# String representation +# --------------------------------------------------------------------------- + + +def test_sensor_print(sim): + """Test that the sensor string representation works.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + sensor_str = str(sensor) + assert "physx" in sensor_str + assert "Joint wrench sensor" in sensor_str + + +# --------------------------------------------------------------------------- +# Reset behavior +# --------------------------------------------------------------------------- + + +def test_reset_zeros_buffers(sim): + """Resetting the sensor clears the force / torque buffers.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=2)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + for _ in range(100): + sim.step() + scene.update(sim.get_physics_dt()) + + assert torch.any(sensor.data.force.torch != 0), "Expected non-zero data before reset" + + sensor.reset() + + # Access raw buffers to skip lazy re-population from the PhysX view on the next data read. + force_after = wp.to_torch(sensor._data._force) + torque_after = wp.to_torch(sensor._data._torque) + torch.testing.assert_close(force_after, torch.zeros_like(force_after)) + torch.testing.assert_close(torque_after, torch.zeros_like(torque_after)) + + +def test_reset_with_env_ids_only_zeros_selected_envs(sim): + """Partial reset via env_ids should zero the selected envs and preserve the others.""" + scene = InteractiveScene(_SingleJointSceneCfg(num_envs=4)) + sim.reset() + + sensor: JointWrenchSensor = scene["wrench"] + for _ in range(100): + sim.step() + scene.update(sim.get_physics_dt()) + + force_before = sensor.data.force.torch.clone() + assert torch.any(force_before != 0), "Expected non-zero data before reset" + + sensor.reset(env_ids=[0, 2]) + + force_after = wp.to_torch(sensor._data._force) + torch.testing.assert_close(force_after[0], torch.zeros_like(force_after[0])) + torch.testing.assert_close(force_after[2], torch.zeros_like(force_after[2])) + torch.testing.assert_close(force_after[1], force_before[1]) + torch.testing.assert_close(force_after[3], force_before[3]) diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index 2e7ee00764d7..c6e5ea6bc181 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "1.5.30" +version = "1.5.32" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 2bb26e429744..54f41b239963 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,20 @@ Changelog --------- +1.5.32 (2026-04-30) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated classic Ant/Humanoid manager-based environments and direct in-hand + manipulation environments to read body incoming wrenches from + :class:`~isaaclab.sensors.JointWrenchSensor` instead of + ``ArticulationData.body_incoming_joint_wrench_b``. Add a + :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and pass its + :class:`~isaaclab.managers.SceneEntityCfg` as ``sensor_cfg``. + + 1.5.31 (2026-04-29) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py index e997d7743379..5969d8c9c4be 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py @@ -16,6 +16,7 @@ from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import DirectRLEnv from isaaclab.markers import VisualizationMarkers +from isaaclab.sensors import JointWrenchSensor, JointWrenchSensorCfg from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane from isaaclab.utils.math import quat_conjugate, quat_from_angle_axis, quat_mul, sample_uniform, saturate @@ -50,6 +51,12 @@ def __init__(self, cfg: AllegroHandEnvCfg | ShadowHandEnvCfg, render_mode: str | self.finger_bodies.sort() self.num_fingertips = len(self.finger_bodies) + self.finger_wrench_bodies = [] + if getattr(self, "_joint_wrench_sensor", None) is not None: + for body_name in self.cfg.fingertip_body_names: + self.finger_wrench_bodies.append(self._joint_wrench_sensor.body_names.index(body_name)) + self.finger_wrench_bodies.sort() + # joint limits joint_pos_limits = self.hand.data.joint_limits.torch.to(self.device) self.hand_dof_lower_limits = joint_pos_limits[..., 0] @@ -97,6 +104,9 @@ def _setup_scene(self): # add hand, in-hand object, and goal object self.hand = Articulation(self.cfg.robot_cfg) self.object: Articulation | RigidObject = self.cfg.object_cfg.class_type(self.cfg.object_cfg) + self._joint_wrench_sensor = None + if self.cfg.asymmetric_obs: + self._joint_wrench_sensor = self._create_joint_wrench_sensor() # add ground plane spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) # clone and replicate (no need to filter for this environment) @@ -104,10 +114,16 @@ def _setup_scene(self): # add articulation to scene - we must register to scene to randomize with EventManager self.scene.articulations["robot"] = self.hand self.scene.rigid_objects["object"] = self.object + if self._joint_wrench_sensor is not None: + self.scene.sensors["joint_wrench"] = self._joint_wrench_sensor # add lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) light_cfg.func("/World/Light", light_cfg) + def _create_joint_wrench_sensor(self) -> JointWrenchSensor: + """Create the joint-wrench sensor used for fingertip force/torque observations.""" + return JointWrenchSensor(JointWrenchSensorCfg(prim_path=self.cfg.robot_cfg.prim_path)) + def _pre_physics_step(self, actions: torch.Tensor) -> None: self.actions = actions.clone() @@ -135,13 +151,7 @@ def _apply_action(self) -> None: def _get_observations(self) -> dict: if self.cfg.asymmetric_obs: - # Newton does not implement body_incoming_joint_wrench_b; fall back to zeros. - try: - self.fingertip_force_sensors = self.hand.data.body_incoming_joint_wrench_b.torch[:, self.finger_bodies] - except NotImplementedError: - self.fingertip_force_sensors = torch.zeros( - self.num_envs, len(self.finger_bodies), 6, dtype=torch.float32, device=self.device - ) + self._update_fingertip_force_sensors() if self.cfg.obs_type == "openai": obs = self.compute_reduced_observations() @@ -158,6 +168,27 @@ def _get_observations(self) -> dict: observations = {"policy": obs, "critic": states} return observations + def _update_fingertip_force_sensors(self) -> None: + """Update fingertip force/torque observations from the joint-wrench sensor.""" + if getattr(self, "_joint_wrench_sensor", None) is None: + self.fingertip_force_sensors = torch.zeros( + self.num_envs, len(self.finger_bodies), 6, dtype=torch.float32, device=self.device + ) + return + + sensor_data = self._joint_wrench_sensor.data + force_data = sensor_data.force + torque_data = sensor_data.torque + if force_data is None or torque_data is None: + self.fingertip_force_sensors = torch.zeros( + self.num_envs, len(self.finger_bodies), 6, dtype=torch.float32, device=self.device + ) + return + + force = force_data.torch[:, self.finger_wrench_bodies] + torque = torque_data.torch[:, self.finger_wrench_bodies] + self.fingertip_force_sensors = torch.cat((force, torque), dim=-1) + def _get_rewards(self) -> torch.Tensor: ( total_reward, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py index 6783823b56f7..7da4db48e853 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py @@ -49,12 +49,14 @@ def _setup_scene(self): # add hand, in-hand object, and goal object self.hand = Articulation(self.cfg.robot_cfg) self.object: Articulation | RigidObject = self.cfg.object_cfg.class_type(self.cfg.object_cfg) + self._joint_wrench_sensor = self._create_joint_wrench_sensor() self._tiled_camera = Camera(self.cfg.tiled_camera) # clone and replicate (no need to filter for this environment) self.scene.clone_environments(copy_from_source=False) # add articulation to scene - we must register to scene to randomize with EventManager self.scene.articulations["robot"] = self.hand self.scene.rigid_objects["object"] = self.object + self.scene.sensors["joint_wrench"] = self._joint_wrench_sensor self.scene.sensors["tiled_camera"] = self._tiled_camera # add lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) @@ -127,13 +129,7 @@ def _get_observations(self) -> dict: # vision observations from CMM image_obs = self._compute_image_observations() obs = torch.cat((state_obs, image_obs), dim=-1) - # asymmetric critic states — Newton does not implement body_incoming_joint_wrench_b - try: - self.fingertip_force_sensors = self.hand.data.body_incoming_joint_wrench_b.torch[:, self.finger_bodies] - except NotImplementedError: - self.fingertip_force_sensors = torch.zeros( - self.num_envs, len(self.finger_bodies), 6, dtype=torch.float32, device=self.device - ) + self._update_fingertip_force_sensors() state = self._compute_states() observations = {"policy": obs, "critic": state} diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py index e2f9cca14377..706d115cebc8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py @@ -16,6 +16,7 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import JointWrenchSensorCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass @@ -90,6 +91,9 @@ class MySceneCfg(InteractiveSceneCfg): # robot robot = ANT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # sensors + joint_wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + # lights light = AssetBaseCfg( prim_path="/World/light", @@ -130,8 +134,9 @@ class PolicyCfg(ObsGroup): func=mdp.body_incoming_wrench, scale=0.1, params={ - "asset_cfg": SceneEntityCfg( - "robot", body_names=["front_left_foot", "front_right_foot", "left_back_foot", "right_back_foot"] + "sensor_cfg": SceneEntityCfg( + "joint_wrench", + body_names=["front_left_foot", "front_right_foot", "left_back_foot", "right_back_foot"], ) }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py index c610159a5c51..fe68a3058336 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py @@ -16,6 +16,7 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import JointWrenchSensorCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass @@ -64,6 +65,9 @@ class MySceneCfg(InteractiveSceneCfg): # robot robot = HUMANOID_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # sensors + joint_wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + # lights light = AssetBaseCfg( prim_path="/World/light", @@ -117,7 +121,7 @@ class PolicyCfg(ObsGroup): feet_body_forces = ObsTerm( func=mdp.body_incoming_wrench, scale=0.01, - params={"asset_cfg": SceneEntityCfg("robot", body_names=["left_foot", "right_foot"])}, + params={"sensor_cfg": SceneEntityCfg("joint_wrench", body_names=["left_foot", "right_foot"])}, ) actions = ObsTerm(func=mdp.last_action) From 7d83171570c5ca2f1470853a052af7bf9f420086 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 30 Apr 2026 16:46:19 +0200 Subject: [PATCH 14/16] Resolve PhysX joint wrench articulation roots Resolve nested USD articulation roots before creating the PhysX articulation view so classic Ant and Humanoid environments can use the joint wrench sensor from the asset prim path. --- .../joint_wrench/joint_wrench_sensor.py | 42 +++++++++++++++++-- .../test/sensors/test_joint_wrench_sensor.py | 28 +++++++++++++ 2 files changed, 66 insertions(+), 4 deletions(-) diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py index e2124f244834..155437758bbe 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py @@ -13,7 +13,10 @@ import warp as wp +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 from isaaclab_physx.physics import PhysxManager as SimulationManager @@ -37,8 +40,9 @@ class JointWrenchSensor(BaseJointWrenchSensor): the child body's frame for the standard ``body0`` = parent, ``body1`` = child convention. The root body's entry is included. - :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at the - articulation root prim in every environment. + :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at either + the articulation root prim or a parent prim containing a single + articulation root in every environment. """ cfg: JointWrenchSensorCfg @@ -116,9 +120,10 @@ def _initialize_impl(self) -> None: super()._initialize_impl() self._physics_sim_view = SimulationManager.get_physics_sim_view() - self._root_view = self._physics_sim_view.create_articulation_view(self.cfg.prim_path.replace(".*", "*")) + root_prim_path_expr = self._resolve_articulation_root_prim_path() + self._root_view = self._physics_sim_view.create_articulation_view(root_prim_path_expr.replace(".*", "*")) if self._root_view._backend is None: - raise RuntimeError(f"Failed to create articulation view at: {self.cfg.prim_path}. Check PhysX logs.") + raise RuntimeError(f"Failed to create articulation view at: {root_prim_path_expr}. Check PhysX logs.") self._num_bodies = self._root_view.shared_metatype.link_count if self._num_bodies == 0: @@ -129,6 +134,35 @@ def _initialize_impl(self) -> None: logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_bodies} bodies") + def _resolve_articulation_root_prim_path(self) -> str: + """Resolve the articulation root prim path expression from the configured asset prim path.""" + 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}'.") + first_env_matching_prim_path = first_env_matching_prim.GetPath().pathString + + first_env_root_prims = get_all_matching_child_prims( + first_env_matching_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) + and prim.GetAttribute("physxArticulation:articulationEnabled").Get() is not False, + traverse_instance_prims=False, + ) + if len(first_env_root_prims) == 0: + raise RuntimeError( + f"Failed to find an articulation when resolving '{first_env_matching_prim_path}'." + " Please ensure that the prim has 'USD ArticulationRootAPI' applied." + ) + if len(first_env_root_prims) > 1: + raise RuntimeError( + f"Failed to find a single articulation when resolving '{first_env_matching_prim_path}'." + f" Found multiple '{first_env_root_prims}' under '{first_env_matching_prim_path}'." + " Please ensure that there is only one articulation in the prim path tree." + ) + + first_env_root_prim_path = first_env_root_prims[0].GetPath().pathString + 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 _update_buffers_impl(self, env_mask: wp.array) -> None: """Read PhysX incoming joint wrenches and split them into force / torque buffers. 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 abdfcc754b1b..b3f1890e4db4 100644 --- a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py @@ -27,6 +27,8 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab_assets.robots.ant import ANT_CFG + def _make_single_joint_articulation_cfg() -> ArticulationCfg: """Single-joint revolute test articulation (root ``CenterPivot`` + arm ``Arm``).""" @@ -103,6 +105,16 @@ class _CartpoleDampedSceneCfg(InteractiveSceneCfg): wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") +@configclass +class _NestedRootAntSceneCfg(InteractiveSceneCfg): + """Ant USD asset whose articulation root is nested under the configured asset prim.""" + + env_spacing = 4.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = ANT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + @pytest.fixture def sim(): """Simulation context using the PhysX backend.""" @@ -192,6 +204,22 @@ def test_multi_body_articulation(sim): _assert_sensor_matches_physx_tensor(sensor) +def test_nested_articulation_root_resolution(sim): + """Sensor accepts an asset prim path whose articulation root is nested in the USD asset.""" + scene = InteractiveScene(_NestedRootAntSceneCfg(num_envs=1)) + sim.reset() + + robot: Articulation = scene["robot"] + sensor: JointWrenchSensor = scene["wrench"] + sim.step() + scene.update(sim.get_physics_dt()) + + assert sensor.body_names == robot.body_names + assert sensor.data.force.torch.shape == (1, robot.num_bodies, 3) + assert sensor.data.torque.torch.shape == (1, robot.num_bodies, 3) + _assert_sensor_matches_physx_tensor(sensor) + + # --------------------------------------------------------------------------- # Physical correctness # --------------------------------------------------------------------------- From a7f68eefc5c2fef0a6785bff5eda6b3bd4d51ea1 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 30 Apr 2026 16:57:11 +0200 Subject: [PATCH 15/16] Enable classic Newton wrench observations Resolve nested articulation roots for the Newton joint wrench sensor and let classic Ant and Humanoid Newton presets use the same wrench observation terms as PhysX. --- source/isaaclab_newton/docs/CHANGELOG.rst | 6 +++ .../joint_wrench/joint_wrench_sensor.py | 47 +++++++++++++++---- .../test/sensors/test_joint_wrench_sensor.py | 28 +++++++++++ source/isaaclab_physx/docs/CHANGELOG.rst | 2 + source/isaaclab_tasks/docs/CHANGELOG.rst | 3 +- .../manager_based/classic/ant/ant_env_cfg.py | 29 +----------- .../classic/humanoid/humanoid_env_cfg.py | 29 +----------- 7 files changed, 79 insertions(+), 65 deletions(-) diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 2325473353f1..26f55c065970 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -12,6 +12,12 @@ Removed and read :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_newton.sensors.JointWrenchSensor` initialization for + USD assets whose articulation root is nested below the configured asset prim. + 0.5.26 (2026-04-30) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py index 4f59487e882d..bd53751d6a5f 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/joint_wrench/joint_wrench_sensor.py @@ -13,7 +13,10 @@ from newton import JointType from newton.selection import ArticulationView +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 from isaaclab_newton.physics import NewtonManager @@ -34,12 +37,10 @@ class JointWrenchSensor(BaseJointWrenchSensor): (child-side joint frame, child-side joint anchor as reference point) before storing it in per-joint force / torque buffers. - :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at the - articulation root prim (the one carrying ``ArticulationRootAPI``) in - every environment; the sensor uses it as the - :class:`~newton.selection.ArticulationView` pattern directly. ``FREE`` - and ``FIXED`` joints are excluded — neither has a meaningful joint - anchor. + :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at either + the articulation root prim or a parent prim containing a single + articulation root in every environment. ``FREE`` and ``FIXED`` joints are + excluded — neither has a meaningful joint anchor. """ cfg: JointWrenchSensorCfg @@ -126,9 +127,10 @@ def _initialize_impl(self) -> None: model = NewtonManager.get_model() state_0 = NewtonManager.get_state_0() + root_prim_path_expr = self._resolve_articulation_root_prim_path() self._root_view = ArticulationView( model, - self.cfg.prim_path.replace(".*", "*"), + root_prim_path_expr.replace(".*", "*"), verbose=False, exclude_joint_types=[JointType.FREE, JointType.FIXED], ) @@ -136,7 +138,7 @@ def _initialize_impl(self) -> None: if self._num_joints == 0: raise RuntimeError( "Joint wrench sensor matched zero reportable joints (all joints are FREE or FIXED)." - f" Check the articulation at '{self.cfg.prim_path}'." + f" Check the articulation at '{root_prim_path_expr}'." ) try: @@ -168,6 +170,35 @@ def _initialize_impl(self) -> None: logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_joints} joints") + def _resolve_articulation_root_prim_path(self) -> str: + """Resolve the articulation root prim path expression from the configured asset prim path.""" + 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}'.") + first_env_matching_prim_path = first_env_matching_prim.GetPath().pathString + + first_env_root_prims = get_all_matching_child_prims( + first_env_matching_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) + and prim.GetAttribute("physxArticulation:articulationEnabled").Get() is not False, + traverse_instance_prims=False, + ) + if len(first_env_root_prims) == 0: + raise RuntimeError( + f"Failed to find an articulation when resolving '{first_env_matching_prim_path}'." + " Please ensure that the prim has 'USD ArticulationRootAPI' applied." + ) + if len(first_env_root_prims) > 1: + raise RuntimeError( + f"Failed to find a single articulation when resolving '{first_env_matching_prim_path}'." + f" Found multiple '{first_env_root_prims}' under '{first_env_matching_prim_path}'." + " Please ensure that there is only one articulation in the prim path tree." + ) + + first_env_root_prim_path = first_env_root_prims[0].GetPath().pathString + 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 _update_buffers_impl(self, env_mask: wp.array) -> None: """Convert Newton's body_parent_f into INCOMING_JOINT_FRAME force and torque buffers. 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 dba86b2c0c8c..34e1db15b184 100644 --- a/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_joint_wrench_sensor.py @@ -26,6 +26,8 @@ from isaaclab.utils import math as math_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab_assets.robots.ant import ANT_CFG + def _make_single_joint_articulation_cfg() -> ArticulationCfg: """Single-joint revolute test articulation (root ``CenterPivot`` + arm ``Arm``).""" @@ -102,6 +104,16 @@ class _CartpoleDampedSceneCfg(InteractiveSceneCfg): wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") +@configclass +class _NestedRootAntSceneCfg(InteractiveSceneCfg): + """Ant USD asset whose articulation root is nested under the configured asset prim.""" + + env_spacing = 4.0 + terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + robot = ANT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + wrench = JointWrenchSensorCfg(prim_path="{ENV_REGEX_NS}/Robot") + + @pytest.fixture def sim(): """Simulation context using the Newton backend.""" @@ -170,6 +182,22 @@ def test_multi_body_articulation(sim): assert "rail" not in [n.lower() for n in sensor.body_names] +def test_nested_articulation_root_resolution(sim): + """Sensor accepts an asset prim path whose articulation root is nested in the USD asset.""" + scene = InteractiveScene(_NestedRootAntSceneCfg(num_envs=1)) + sim.reset() + + robot: Articulation = scene["robot"] + sensor: JointWrenchSensor = scene["wrench"] + sim.step() + scene.update(sim.get_physics_dt()) + + assert len(sensor.body_names) == robot.num_joints + assert set(sensor.body_names).issubset(set(robot.body_names)) + assert sensor.data.force.torch.shape == (1, robot.num_joints, 3) + assert sensor.data.torque.torch.shape == (1, robot.num_joints, 3) + + # --------------------------------------------------------------------------- # Physical correctness # --------------------------------------------------------------------------- diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index d403a94f541f..e0154fd393cf 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -9,6 +9,8 @@ Added * Added :class:`~isaaclab_physx.sensors.JointWrenchSensor` for reading PhysX incoming joint reaction wrenches as split force [N] and torque [N·m] buffers. + The sensor accepts asset prim paths whose articulation root is nested below + the configured prim. Removed ^^^^^^^ diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 54f41b239963..aaa1f8a9bec7 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -12,7 +12,8 @@ Changed :class:`~isaaclab.sensors.JointWrenchSensor` instead of ``ArticulationData.body_incoming_joint_wrench_b``. Add a :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and pass its - :class:`~isaaclab.managers.SceneEntityCfg` as ``sensor_cfg``. + :class:`~isaaclab.managers.SceneEntityCfg` as ``sensor_cfg``. The classic + Ant/Humanoid Newton presets now use the same wrench observations as PhysX. 1.5.31 (2026-04-29) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py index 706d115cebc8..2e8f356ef975 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py @@ -150,38 +150,11 @@ def __post_init__(self): policy: PolicyCfg = PolicyCfg() -@configclass -class _AntNewtonObservationsCfg: - """Newton-compatible observations: excludes feet_body_forces (not implemented in Newton).""" - - @configclass - class PolicyCfg(ObsGroup): - """Observations for the policy.""" - - base_height = ObsTerm(func=mdp.base_pos_z) - base_lin_vel = ObsTerm(func=mdp.base_lin_vel) - base_ang_vel = ObsTerm(func=mdp.base_ang_vel) - base_yaw_roll = ObsTerm(func=mdp.base_yaw_roll) - base_angle_to_target = ObsTerm(func=mdp.base_angle_to_target, params={"target_pos": (1000.0, 0.0, 0.0)}) - base_up_proj = ObsTerm(func=mdp.base_up_proj) - base_heading_proj = ObsTerm(func=mdp.base_heading_proj, params={"target_pos": (1000.0, 0.0, 0.0)}) - joint_pos_norm = ObsTerm(func=mdp.joint_pos_limit_normalized) - joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel, scale=0.2) - actions = ObsTerm(func=mdp.last_action) - - def __post_init__(self): - self.enable_corruption = False - self.concatenate_terms = True - - # observation groups - policy: PolicyCfg = PolicyCfg() - - @configclass class AntObservationsCfg(PresetCfg): default: ObservationsCfg = ObservationsCfg() physx: ObservationsCfg = ObservationsCfg() - newton: _AntNewtonObservationsCfg = _AntNewtonObservationsCfg() + newton: ObservationsCfg = ObservationsCfg() @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py index fe68a3058336..91f0c86a4eed 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py @@ -133,38 +133,11 @@ def __post_init__(self): policy: PolicyCfg = PolicyCfg() -@configclass -class _HumanoidNewtonObservationsCfg: - """Newton-compatible observations: excludes feet_body_forces (not implemented in Newton).""" - - @configclass - class PolicyCfg(ObsGroup): - """Observations for the policy.""" - - base_height = ObsTerm(func=mdp.base_pos_z) - base_lin_vel = ObsTerm(func=mdp.base_lin_vel) - base_ang_vel = ObsTerm(func=mdp.base_ang_vel, scale=0.25) - base_yaw_roll = ObsTerm(func=mdp.base_yaw_roll) - base_angle_to_target = ObsTerm(func=mdp.base_angle_to_target, params={"target_pos": (1000.0, 0.0, 0.0)}) - base_up_proj = ObsTerm(func=mdp.base_up_proj) - base_heading_proj = ObsTerm(func=mdp.base_heading_proj, params={"target_pos": (1000.0, 0.0, 0.0)}) - joint_pos_norm = ObsTerm(func=mdp.joint_pos_limit_normalized) - joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel, scale=0.1) - actions = ObsTerm(func=mdp.last_action) - - def __post_init__(self): - self.enable_corruption = False - self.concatenate_terms = True - - # observation groups - policy: PolicyCfg = PolicyCfg() - - @configclass class HumanoidObservationsCfg(PresetCfg): default: ObservationsCfg = ObservationsCfg() physx: ObservationsCfg = ObservationsCfg() - newton: _HumanoidNewtonObservationsCfg = _HumanoidNewtonObservationsCfg() + newton: ObservationsCfg = ObservationsCfg() @configclass From da5ff06e4d62064e3c30a6cb7d59b5c46c2df7bc Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 30 Apr 2026 17:04:38 +0200 Subject: [PATCH 16/16] Honor PhysX joint wrench frame convention Convert PhysX incoming joint wrenches from body1 frame at the body origin into the shared child-side joint frame with torque referenced at the child-side joint anchor. --- .../migration/migrating_to_isaaclab_3-0.rst | 4 +- .../joint_wrench/joint_wrench_sensor_cfg.py | 4 +- source/isaaclab_physx/docs/CHANGELOG.rst | 3 +- .../joint_wrench/joint_wrench_sensor.py | 65 +++++++++++++++-- .../sensors/joint_wrench/kernels.py | 15 +++- .../test/sensors/test_joint_wrench_sensor.py | 73 ++++++++++++++++++- 6 files changed, 148 insertions(+), 16 deletions(-) diff --git a/docs/source/migration/migrating_to_isaaclab_3-0.rst b/docs/source/migration/migrating_to_isaaclab_3-0.rst index 2e9fd76fcd80..a559443274c2 100644 --- a/docs/source/migration/migrating_to_isaaclab_3-0.rst +++ b/docs/source/migration/migrating_to_isaaclab_3-0.rst @@ -343,6 +343,8 @@ The ``ArticulationData.body_incoming_joint_wrench_b`` property has been removed. Isaac Lab 3.0, incoming joint reaction wrenches are exposed through :class:`~isaaclab.sensors.JointWrenchSensor`, which has PhysX and Newton backend implementations and returns separate force [N] and torque [N·m] buffers. +The sensor reports wrenches in the child-side incoming joint frame, with torque +referenced at the child-side joint anchor. **Before (Isaac Lab 2.x):** @@ -364,7 +366,7 @@ implementations and returns separate force [N] and torque [N·m] buffers. sensor = env.scene.sensors["joint_wrench"] data = sensor.data - wrench_b = torch.cat( + wrench_j = torch.cat( ( data.force.torch[:, body_ids], data.torque.torch[:, body_ids], diff --git a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py index aae63076156b..4bdb0ab74f06 100644 --- a/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py +++ b/source/isaaclab/isaaclab/sensors/joint_wrench/joint_wrench_sensor_cfg.py @@ -25,6 +25,6 @@ class JointWrenchSensorCfg(SensorBaseCfg): """Coordinate convention for the reported wrench. Defaults to ``"incoming_joint_frame"``. - ``"incoming_joint_frame"`` — child-side joint frame, child-side joint anchor as reference point. - Matches what a real 6-axis F/T sensor mounted at the joint would measure. This is the same - as PhysX convention in IsaacLab2.3 + Matches what a real 6-axis F/T sensor mounted at the joint would measure. Backends convert + their native solver outputs to this convention. """ diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index e0154fd393cf..528d6c9867af 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -10,7 +10,8 @@ Added * Added :class:`~isaaclab_physx.sensors.JointWrenchSensor` for reading PhysX incoming joint reaction wrenches as split force [N] and torque [N·m] buffers. The sensor accepts asset prim paths whose articulation root is nested below - the configured prim. + the configured prim and converts PhysX's native body-frame wrench to the + shared child-side joint-frame convention. Removed ^^^^^^^ diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py index 155437758bbe..9ae39c621240 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py @@ -11,9 +11,10 @@ from collections.abc import Sequence from typing import TYPE_CHECKING +import numpy as np import warp as wp -from pxr import UsdPhysics +from pxr import Usd, UsdPhysics from isaaclab.sensors.joint_wrench import BaseJointWrenchSensor from isaaclab.sim.utils.queries import find_first_matching_prim, get_all_matching_child_prims @@ -35,10 +36,9 @@ class JointWrenchSensor(BaseJointWrenchSensor): """PhysX joint reaction wrench sensor. The sensor reads PhysX's incoming joint wrench for every articulation link - and exposes the linear force [N] and angular torque [N·m] components. PhysX - reports each wrench in the frame of ``body1`` from the USD joint, which is - the child body's frame for the standard ``body0`` = parent, ``body1`` = - child convention. The root body's entry is included. + and exposes the linear force [N] and angular torque [N·m] components in + the child-side joint frame, with torque referenced at the child-side joint + anchor. The root body's entry is included. :attr:`~isaaclab.sensors.SensorBaseCfg.prim_path` must point at either the articulation root prim or a parent prim containing a single @@ -62,6 +62,8 @@ def __init__(self, cfg: JointWrenchSensorCfg): self._data = JointWrenchSensorData() self._physics_sim_view = None self._root_view: physx.ArticulationView | 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: @@ -130,6 +132,7 @@ def _initialize_impl(self) -> None: raise RuntimeError(f"Joint wrench sensor matched zero bodies at '{self.cfg.prim_path}'.") self._data._body_names = list(self._root_view.shared_metatype.link_names) + self._create_joint_frame_buffers() self._data.create_buffers(num_envs=self._num_envs, num_bodies=self._num_bodies, device=self._device) logger.info(f"Joint wrench sensor initialized: {self._num_envs} envs, {self._num_bodies} bodies") @@ -163,6 +166,45 @@ 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 PhysX 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 PhysX incoming joint wrenches and split them into force / torque buffers. @@ -174,12 +216,21 @@ 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.") incoming_joint_wrench = self._root_view.get_link_incoming_joint_force().view(wp.spatial_vectorf) wp.launch( joint_wrench_split_kernel, dim=(self._num_envs, self._num_bodies), - inputs=[env_mask, incoming_joint_wrench, self._data._force, self._data._torque], + inputs=[ + env_mask, + incoming_joint_wrench, + self._joint_pos_b, + self._joint_quat_b, + self._data._force, + self._data._torque, + ], device=self._device, ) @@ -192,6 +243,8 @@ def _invalidate_initialize_callback(self, event) -> None: super()._invalidate_initialize_callback(event) self._physics_sim_view = None self._root_view = 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_physx/isaaclab_physx/sensors/joint_wrench/kernels.py b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py index 3abc7f8c67b4..bb3d0c7cf67c 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py @@ -10,17 +10,26 @@ def joint_wrench_split_kernel( 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), out_force: wp.array(dtype=wp.vec3f, ndim=2), out_torque: wp.array(dtype=wp.vec3f, ndim=2), ): - """Split PhysX incoming joint spatial wrenches into force and torque components.""" + """Convert PhysX incoming joint spatial wrenches into the child-side joint frame.""" env, body = wp.tid() if not env_mask[env]: return wrench = incoming_joint_wrench[env, body] - out_force[env, body] = wp.spatial_top(wrench) - out_torque[env, body] = wp.spatial_bottom(wrench) + force_b = wp.spatial_top(wrench) + torque_b = wp.spatial_bottom(wrench) + + # PhysX 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) @wp.kernel 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 b3f1890e4db4..4f10f2506ebb 100644 --- a/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py +++ b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py @@ -12,11 +12,15 @@ """Rest everything follows.""" +import math + import pytest import torch import warp as wp from isaaclab_physx.physics import PhysxCfg +from pxr import Gf, UsdPhysics + import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg @@ -25,6 +29,7 @@ from isaaclab.sim import SimulationCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass +from isaaclab.utils import math as math_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab_assets.robots.ant import ANT_CFG @@ -138,12 +143,49 @@ def _physx_incoming_joint_wrench(sensor: JointWrenchSensor) -> torch.Tensor: def _assert_sensor_matches_physx_tensor(sensor: JointWrenchSensor) -> None: - """Compare the sensor buffers to the raw PhysX tensor API.""" + """Compare sensor buffers to the raw PhysX tensor transformed into joint frames.""" raw_wrench = _physx_incoming_joint_wrench(sensor) sensor_data = sensor.data - torch.testing.assert_close(sensor_data.force.torch, raw_wrench[..., :3]) - torch.testing.assert_close(sensor_data.torque.torch, raw_wrench[..., 3:]) + expected_force, expected_torque = _physx_incoming_joint_wrench_in_joint_frame(sensor, raw_wrench) + torch.testing.assert_close(sensor_data.force.torch, expected_force) + torch.testing.assert_close(sensor_data.torque.torch, expected_torque) + + +def _physx_incoming_joint_wrench_in_joint_frame( + sensor: JointWrenchSensor, raw_wrench: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + """Transform raw PhysX 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}'.") # --------------------------------------------------------------------------- @@ -243,6 +285,31 @@ 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(sim): + """PhysX 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 = _physx_incoming_joint_wrench(sensor) + expected_force, expected_torque = _physx_incoming_joint_wrench_in_joint_frame(sensor, raw_wrench) + torch.testing.assert_close(sensor.data.force.torch, expected_force) + torch.testing.assert_close(sensor.data.torque.torch, expected_torque) + + 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) + + def test_wrench_with_external_force_and_torque(sim): """Full wrench validation with external force and torque applied.""" scene = InteractiveScene(_SingleJointSceneCfg(num_envs=1))