Fix wrench composer to correctly update composed wrenches

CONTRIBUTORS.md

@@ -102,6 +102,7 @@ Guidelines for modifications:
 * Lotus Li
 * Louis Le Lay
 * Lorenz Wellhausen
+* Lorenzo Moretti
 * Lukas Fröhlich
 * Manuel Schweiger
 * Masoud Moghani

source/isaaclab/config/extension.toml

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.54.3"
+version = "0.54.4"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,17 @@
 Changelog
 ---------
 
+0.54.4 (2026-02-13)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed :class:`~isaaclab.utils.wrench_composer.WrenchComposer` to correctly handle composed wrenches when link poses
+  change during simulation. Forces and torques are composed and stored in "mixed" frame representation (global frame
+  orientation, link frame position) and passed to Physx  with `is_global=True`.
+
+
 0.54.3 (2026-02-04)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/assets/articulation/articulation.py

@@ -234,14 +234,15 @@ def write_data_to_sim(self):
                     torques=self._permanent_wrench_composer.composed_torque,
                     body_ids=self._ALL_BODY_INDICES_WP,
                     env_ids=self._ALL_INDICES_WP,
+                    is_global=True,
                 )
                 # Apply both instantaneous and permanent wrench to the simulation
                 self.root_physx_view.apply_forces_and_torques_at_position(
                     force_data=self._instantaneous_wrench_composer.composed_force_as_torch.view(-1, 3),
                     torque_data=self._instantaneous_wrench_composer.composed_torque_as_torch.view(-1, 3),
                     position_data=None,
                     indices=self._ALL_INDICES,
-                    is_global=False,
+                    is_global=True,
                 )
             else:
                 # Apply permanent wrench to the simulation
@@ -250,7 +251,7 @@ def write_data_to_sim(self):
                     torque_data=self._permanent_wrench_composer.composed_torque_as_torch.view(-1, 3),
                     position_data=None,
                     indices=self._ALL_INDICES,
-                    is_global=False,
+                    is_global=True,
                 )
         self._instantaneous_wrench_composer.reset()
 

source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py

@@ -147,14 +147,15 @@ def write_data_to_sim(self):
                     torques=self._permanent_wrench_composer.composed_torque,
                     body_ids=self._ALL_BODY_INDICES_WP,
                     env_ids=self._ALL_INDICES_WP,
+                    is_global=True,
                 )
                 # Apply both instantaneous and permanent wrench to the simulation
                 self.root_physx_view.apply_forces_and_torques_at_position(
                     force_data=self._instantaneous_wrench_composer.composed_force_as_torch.view(-1, 3),
                     torque_data=self._instantaneous_wrench_composer.composed_torque_as_torch.view(-1, 3),
                     position_data=None,
                     indices=self._ALL_INDICES,
-                    is_global=False,
+                    is_global=True,
                 )
             else:
                 # Apply permanent wrench to the simulation
@@ -163,7 +164,7 @@ def write_data_to_sim(self):
                     torque_data=self._permanent_wrench_composer.composed_torque_as_torch.view(-1, 3),
                     position_data=None,
                     indices=self._ALL_INDICES,
-                    is_global=False,
+                    is_global=True,
                 )
         self._instantaneous_wrench_composer.reset()
 

source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py

@@ -191,14 +191,15 @@ def write_data_to_sim(self):
                     torques=self._permanent_wrench_composer.composed_torque,
                     body_ids=self._ALL_OBJ_INDICES_WP,
                     env_ids=self._ALL_ENV_INDICES_WP,
+                    is_global=True,
                 )
                 # Apply both instantaneous and permanent wrench to the simulation
                 self.root_physx_view.apply_forces_and_torques_at_position(
                     force_data=self.reshape_data_to_view(self._instantaneous_wrench_composer.composed_force_as_torch),
                     torque_data=self.reshape_data_to_view(self._instantaneous_wrench_composer.composed_torque_as_torch),
                     position_data=None,
                     indices=self._env_obj_ids_to_view_ids(self._ALL_ENV_INDICES, self._ALL_OBJ_INDICES),
-                    is_global=False,
+                    is_global=True,
                 )
             else:
                 # Apply permanent wrench to the simulation
@@ -207,7 +208,7 @@ def write_data_to_sim(self):
                     torque_data=self.reshape_data_to_view(self._permanent_wrench_composer.composed_torque_as_torch),
                     position_data=None,
                     indices=self._env_obj_ids_to_view_ids(self._ALL_ENV_INDICES, self._ALL_OBJ_INDICES),
-                    is_global=False,
+                    is_global=True,
                 )
         self._instantaneous_wrench_composer.reset()
 

source/isaaclab/isaaclab/utils/warp/kernels.py

@@ -307,26 +307,29 @@ def reshape_tiled_image(
 
 
 @wp.func
-def cast_to_link_frame(position: wp.vec3f, link_position: wp.vec3f, is_global: bool) -> wp.vec3f:
-    """Casts a position to the link frame of the body.
+def cast_position_to_mixed_frame(
+    position: wp.vec3f, link_position: wp.vec3f, link_quat: wp.quatf, is_global: bool
+) -> wp.vec3f:
+    """Casts a position to the mixed frame.
 
     Args:
         position: The position to cast.
         link_position: The link frame position.
+        link_quat: The link frame quaternion.
         is_global: Whether the position is in the global frame.
 
     Returns:
-        The position in the link frame of the body.
+        The position in the mixed frame.
     """
     if is_global:
         return position - link_position
     else:
-        return position
+        return wp.quat_rotate(link_quat, position)
 
 
 @wp.func
-def cast_force_to_link_frame(force: wp.vec3f, link_quat: wp.quatf, is_global: bool) -> wp.vec3f:
-    """Casts a force to the link frame of the body.
+def cast_force_to_mixed_frame(force: wp.vec3f, link_quat: wp.quatf, is_global: bool) -> wp.vec3f:
+    """Casts a force to the mixed frame.
 
     Args:
         force: The force to cast.
@@ -336,27 +339,27 @@ def cast_force_to_link_frame(force: wp.vec3f, link_quat: wp.quatf, is_global: bo
         The force in the link frame of the body.
     """
     if is_global:
-        return wp.quat_rotate_inv(link_quat, force)
-    else:
         return force
+    else:
+        return wp.quat_rotate(link_quat, force)
 
 
 @wp.func
-def cast_torque_to_link_frame(torque: wp.vec3f, link_quat: wp.quatf, is_global: bool) -> wp.vec3f:
-    """Casts a torque to the link frame of the body.
+def cast_torque_to_mixed_frame(torque: wp.vec3f, link_quat: wp.quatf, is_global: bool) -> wp.vec3f:
+    """Casts a torque to the mixed frame.
 
     Args:
         torque: The torque to cast.
         link_quat: The link frame quaternion.
         is_global: Whether the torque is applied in the global frame.
 
     Returns:
-        The torque in the link frame of the body.
+        The torque in the mixed frame.
     """
     if is_global:
-        return wp.quat_rotate_inv(link_quat, torque)
-    else:
         return torque
+    else:
+        return wp.quat_rotate(link_quat, torque)
 
 
 @wp.kernel
@@ -368,8 +371,8 @@ def add_forces_and_torques_at_position(
     positions: wp.array2d(dtype=wp.vec3f),
     link_positions: wp.array2d(dtype=wp.vec3f),
     link_quaternions: wp.array2d(dtype=wp.quatf),
-    composed_forces_b: wp.array2d(dtype=wp.vec3f),
-    composed_torques_b: wp.array2d(dtype=wp.vec3f),
+    composed_forces_m: wp.array2d(dtype=wp.vec3f),
+    composed_torques_m: wp.array2d(dtype=wp.vec3f),
     is_global: bool,
 ):
     """Adds forces and torques to the composed force and torque at the user-provided positions.
@@ -385,30 +388,34 @@ def add_forces_and_torques_at_position(
         positions: The positions.
         link_positions: The link frame positions.
         link_quaternions: The link frame quaternions.
-        composed_forces_b: The composed forces.
-        composed_torques_b: The composed torques.
-        is_global: Whether the forces and torques are applied in the global frame.
+        composed_forces_m: The composed forces in mixed representation (origin at link frame, orientation in global frame).
+        composed_torques_m: The composed torques in mixed representation (origin at link frame, orientation in global frame).
+        is_global: Whether the forces and torques are in the global frame.
     """
     # get the thread id
     tid_env, tid_body = wp.tid()
 
     # add the forces to the composed force, if the positions are provided, also adds a torque to the composed torque.
     if forces:
-        # add the forces to the composed force
-        composed_forces_b[env_ids[tid_env], body_ids[tid_body]] += cast_force_to_link_frame(
+        # Convert forces to mixed frame and add to composed force
+        composed_forces_m[env_ids[tid_env], body_ids[tid_body]] += cast_force_to_mixed_frame(
             forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
         )
-        # if there is a position offset, add a torque to the composed torque.
+        # If position offset provided, add torque contribution: τ = (pos - link_origin) × force
         if positions:
-            composed_torques_b[env_ids[tid_env], body_ids[tid_body]] += wp.skew(
-                cast_to_link_frame(
-                    positions[tid_env, tid_body], link_positions[env_ids[tid_env], body_ids[tid_body]], is_global
-                )
-            ) @ cast_force_to_link_frame(
-                forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
+            composed_torques_m[env_ids[tid_env], body_ids[tid_body]] += wp.cross(
+                cast_position_to_mixed_frame(
+                    positions[tid_env, tid_body],
+                    link_positions[env_ids[tid_env], body_ids[tid_body]],
+                    link_quaternions[env_ids[tid_env], body_ids[tid_body]],
+                    is_global,
+                ),
+                cast_force_to_mixed_frame(
+                    forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
+                ),
             )
     if torques:
-        composed_torques_b[env_ids[tid_env], body_ids[tid_body]] += cast_torque_to_link_frame(
+        composed_torques_m[env_ids[tid_env], body_ids[tid_body]] += cast_torque_to_mixed_frame(
             torques[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
         )
 
@@ -422,15 +429,22 @@ def set_forces_and_torques_at_position(
     positions: wp.array2d(dtype=wp.vec3f),
     link_positions: wp.array2d(dtype=wp.vec3f),
     link_quaternions: wp.array2d(dtype=wp.quatf),
-    composed_forces_b: wp.array2d(dtype=wp.vec3f),
-    composed_torques_b: wp.array2d(dtype=wp.vec3f),
+    composed_forces_m: wp.array2d(dtype=wp.vec3f),
+    composed_torques_m: wp.array2d(dtype=wp.vec3f),
     is_global: bool,
 ):
     """Sets forces and torques to the composed force and torque at the user-provided positions.
     When is_global is False, the user-provided positions are offsetting the application of the force relatively
     to the link frame of the body. When is_global is True, the user-provided positions are the global positions
     of the force application.
 
+    All forces and torques are stored in "mixed" representation: expressed in global (world) frame
+    orientation but referenced to the link origin. Positions are NOT stored - they are used to compute
+    the torque contribution from forces applied at offset positions.
+
+    When is_global is False, the user-provided positions are local offsets from the link frame origin.
+    When is_global is True, the user-provided positions are global coordinates.
+
     Args:
         env_ids: The environment ids.
         body_ids: The body ids.
@@ -439,31 +453,33 @@ def set_forces_and_torques_at_position(
         positions: The positions.
         link_positions: The link frame positions.
         link_quaternions: The link frame quaternions.
-        composed_forces_b: The composed forces.
-        composed_torques_b: The composed torques.
-        is_global: Whether the forces and torques are applied in the global frame.
+        composed_forces_m: The composed forces in mixed representation (origin at link frame, orientation in global frame).
+        composed_torques_m: The composed torques in mixed representation (origin at link frame, orientation in global frame).
+        is_global: Whether the forces and torques are in the global frame.
     """
     # get the thread id
     tid_env, tid_body = wp.tid()
 
-    # set the torques to the composed torque
+    # Add user-provided torque
     if torques:
-        composed_torques_b[env_ids[tid_env], body_ids[tid_body]] = cast_torque_to_link_frame(
+        composed_torques_m[env_ids[tid_env], body_ids[tid_body]] = cast_torque_to_mixed_frame(
             torques[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
         )
-    # set the forces to the composed force, if the positions are provided, adds a torque to the composed torque
-    # from the force at that position.
+    # Set the forces to the composed force, if the positions are provided, adds a torque to the composed torque.
     if forces:
-        # set the forces to the composed force
-        composed_forces_b[env_ids[tid_env], body_ids[tid_body]] = cast_force_to_link_frame(
+        composed_forces_m[env_ids[tid_env], body_ids[tid_body]] = cast_force_to_mixed_frame(
             forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
         )
         # if there is a position offset, set the torque from the force at that position.
         if positions:
-            composed_torques_b[env_ids[tid_env], body_ids[tid_body]] = wp.skew(
-                cast_to_link_frame(
-                    positions[tid_env, tid_body], link_positions[env_ids[tid_env], body_ids[tid_body]], is_global
-                )
-            ) @ cast_force_to_link_frame(
-                forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
+            composed_torques_m[env_ids[tid_env], body_ids[tid_body]] += wp.cross(
+                cast_position_to_mixed_frame(
+                    positions[tid_env, tid_body],
+                    link_positions[env_ids[tid_env], body_ids[tid_body]],
+                    link_quaternions[env_ids[tid_env], body_ids[tid_body]],
+                    is_global,
+                ),
+                cast_force_to_mixed_frame(
+                    forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
+                ),
             )