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..a559443274c2 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,75 @@ 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. +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):** + +.. 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_j = 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/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab/changelog.d/pr-5458-merge-develop.rst new file mode 100644 index 000000000000..c94e6b8921a6 --- /dev/null +++ b/source/isaaclab/changelog.d/pr-5458-merge-develop.rst @@ -0,0 +1,15 @@ +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. diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index dbc2e07aa8f9..701f53744f84 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,7 +1,6 @@ Changelog --------- - 4.6.27 (2026-05-01) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py index 7587ca9e120a..1a2dc5f1b278 100644 --- a/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py @@ -664,24 +664,6 @@ def body_com_pose_b(self) -> ProxyArray: """ raise NotImplementedError - @property - @abstractmethod - @leapp_tensor_semantics(kind=InputKindEnum.WRENCH) - 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..5ed4e0382bbe 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,12 @@ 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 + 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 (child-side joint frame, child-side joint anchor reference point). Backends convert from their native - representation to this convention internally. + representation to this convention internally. Use :attr:`body_names` or + :meth:`find_bodies` to map entries to articulation bodies. """ cfg: JointWrenchSensorCfg @@ -57,6 +61,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_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/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 3283a0e73459..3e471026a9dc 100644 --- a/source/isaaclab/test/assets/test_articulation_iface.py +++ b/source/isaaclab/test/assets/test_articulation_iface.py @@ -1096,23 +1096,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/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_newton/changelog.d/pr-5458-merge-develop.rst new file mode 100644 index 000000000000..30eb959531c2 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/pr-5458-merge-develop.rst @@ -0,0 +1,13 @@ +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. + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_newton.sensors.JointWrenchSensor` initialization for + USD assets whose articulation root is nested below the configured asset prim. 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.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/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..fedf7c359081 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_ovphysx/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_ovphysx/changelog.d/pr-5458-merge-develop.rst new file mode 100644 index 000000000000..546cb8acc1c7 --- /dev/null +++ b/source/isaaclab_ovphysx/changelog.d/pr-5458-merge-develop.rst @@ -0,0 +1,7 @@ +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. 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/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_physx/changelog.d/pr-5458-merge-develop.rst new file mode 100644 index 000000000000..1fe6a600bb99 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/pr-5458-merge-develop.rst @@ -0,0 +1,16 @@ +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 and converts PhysX's native body-frame wrench to the + shared child-side joint-frame convention. + +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. 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..9ae39c621240 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/joint_wrench_sensor.py @@ -0,0 +1,253 @@ +# 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 numpy as np +import warp as wp + +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 + +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 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 + articulation root 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._joint_pos_b: wp.array | None = None + self._joint_quat_b: wp.array | 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() + 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: {root_prim_path_expr}. 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._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") + + 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 _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. + + 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." + ) + 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._joint_pos_b, + self._joint_quat_b, + 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._joint_pos_b = None + self._joint_quat_b = 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..bb3d0c7cf67c --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sensors/joint_wrench/kernels.py @@ -0,0 +1,47 @@ +# 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), + 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), +): + """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] + 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 +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..4f10f2506ebb --- /dev/null +++ b/source/isaaclab_physx/test/sensors/test_joint_wrench_sensor.py @@ -0,0 +1,426 @@ +# 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 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 +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 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``).""" + 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") + + +@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.""" + 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 sensor buffers to the raw PhysX tensor transformed into joint frames.""" + raw_wrench = _physx_incoming_joint_wrench(sensor) + sensor_data = sensor.data + + 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}'.") + + +# --------------------------------------------------------------------------- +# 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) + + +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 +# --------------------------------------------------------------------------- + + +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_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)) + 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/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_tasks/changelog.d/pr-5458-merge-develop.rst new file mode 100644 index 000000000000..cd4dc7e674d4 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/pr-5458-merge-develop.rst @@ -0,0 +1,10 @@ +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``. The classic + Ant/Humanoid Newton presets now use the same wrench observations as PhysX. 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..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 @@ -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"], ) }, ) @@ -145,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 c610159a5c51..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 @@ -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) @@ -129,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