From e710b14b03a2ec042e08eeb18826422b5ee66e2b Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Thu, 12 Feb 2026 16:48:16 +0100
Subject: [PATCH 1/9] fix wrench composer to properly set and add wrenches

---
 .../assets/articulation/articulation.py       |   5 +-
 .../assets/rigid_object/rigid_object.py       |   5 +-
 .../rigid_object_collection.py                |   5 +-
 .../isaaclab/isaaclab/utils/warp/kernels.py   |  98 +++++++------
 .../isaaclab/utils/wrench_composer.py         | 129 ++++++++++--------
 5 files changed, 136 insertions(+), 106 deletions(-)

diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py
index b67ded15ac4..de19097e3c1 100644
--- a/source/isaaclab/isaaclab/assets/articulation/articulation.py
+++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py
@@ -234,6 +234,7 @@ 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(
@@ -241,7 +242,7 @@ def write_data_to_sim(self):
                     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()
 
diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py
index 6d0ec98f431..52789ff2a28 100644
--- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py
+++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py
@@ -147,6 +147,7 @@ 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(
@@ -154,7 +155,7 @@ def write_data_to_sim(self):
                     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()
 
diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py
index b458b15b403..c2e0f2fbfc3 100644
--- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py
+++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py
@@ -191,6 +191,7 @@ 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(
@@ -198,7 +199,7 @@ def write_data_to_sim(self):
                     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()
 
diff --git a/source/isaaclab/isaaclab/utils/warp/kernels.py b/source/isaaclab/isaaclab/utils/warp/kernels.py
index cf56e34ed45..01d72153918 100644
--- a/source/isaaclab/isaaclab/utils/warp/kernels.py
+++ b/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,14 +339,14 @@ 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.
@@ -351,12 +354,12 @@ def cast_torque_to_link_frame(torque: wp.vec3f, link_quat: wp.quatf, is_global:
         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,32 @@ 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
+            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
                 )
-            ) @ cast_force_to_link_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,8 +427,8 @@ 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.
@@ -431,6 +436,13 @@ def set_forces_and_torques_at_position(
     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 +451,31 @@ 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
+            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
                 )
-            ) @ cast_force_to_link_frame(
-                forces[tid_env, tid_body], link_quaternions[env_ids[tid_env], body_ids[tid_body]], is_global
             )
diff --git a/source/isaaclab/isaaclab/utils/wrench_composer.py b/source/isaaclab/isaaclab/utils/wrench_composer.py
index 8bd42f81e9e..b267d7e5eb3 100644
--- a/source/isaaclab/isaaclab/utils/wrench_composer.py
+++ b/source/isaaclab/isaaclab/utils/wrench_composer.py
@@ -21,8 +21,9 @@ class WrenchComposer:
     def __init__(self, asset: Articulation | RigidObject | RigidObjectCollection) -> None:
         """Wrench composer.
 
-        This class is used to compose forces and torques at the body's link frame.
-        It can compose global wrenches and local wrenches. The result is always in the link frame of the body.
+        This class composes forces and torques from multiple sources into a single wrench per body.
+        Forces and torques are stored in "mixed" representation: expressed in frame whose orientation is global,
+        while the origin is at the link origin. This allows for straightforward composition of forces and torques.
 
         Args:
             asset: Asset to use. Defaults to None.
@@ -47,9 +48,9 @@ def __init__(self, asset: Articulation | RigidObject | RigidObjectCollection) ->
         else:
             raise ValueError(f"Unsupported asset type: {self._asset.__class__.__name__}")
 
-        # Create buffers
-        self._composed_force_b = wp.zeros((self.num_envs, self.num_bodies), dtype=wp.vec3f, device=self.device)
-        self._composed_torque_b = wp.zeros((self.num_envs, self.num_bodies), dtype=wp.vec3f, device=self.device)
+        # Create buffers - all forces and torques are stored in mixed representation: origin at link frame, orientation in global frame.
+        self._composed_force_m = wp.zeros((self.num_envs, self.num_bodies), dtype=wp.vec3f, device=self.device)
+        self._composed_torque_m = wp.zeros((self.num_envs, self.num_bodies), dtype=wp.vec3f, device=self.device)
         self._ALL_ENV_INDICES_WP = wp.from_torch(
             torch.arange(self.num_envs, dtype=torch.int32, device=self.device), dtype=wp.int32
         )
@@ -58,13 +59,13 @@ def __init__(self, asset: Articulation | RigidObject | RigidObjectCollection) ->
         )
 
         # Pinning the composed force and torque to the torch tensor to avoid copying the data to the torch tensor
-        self._composed_force_b_torch = wp.to_torch(self._composed_force_b)
-        self._composed_torque_b_torch = wp.to_torch(self._composed_torque_b)
+        self._composed_force_m_torch = wp.to_torch(self._composed_force_m)
+        self._composed_torque_m_torch = wp.to_torch(self._composed_torque_m)
         # Pinning the environment and body indices to the torch tensor to allow for slicing.
         self._ALL_ENV_INDICES_TORCH = wp.to_torch(self._ALL_ENV_INDICES_WP)
         self._ALL_BODY_INDICES_TORCH = wp.to_torch(self._ALL_BODY_INDICES_WP)
 
-        # Flag to check if the link poses have been updated.
+        # # Flag to check if the link poses have been updated.
         self._link_poses_updated = False
 
     @property
@@ -74,51 +75,50 @@ def active(self) -> bool:
 
     @property
     def composed_force(self) -> wp.array:
-        """Composed force at the body's link frame.
+        """Composed force mixed representation: origin at link frame, orientation in global frame.
 
-        .. note:: If some of the forces are applied in the global frame, the composed force will be in the link frame
-        of the body.
+        Forces are stored in "mixed" representation: global frame orientation, applied at link origin.
+        Any position offsets provided when setting forces contribute to torque, not to this force buffer.
 
         Returns:
-            wp.array: Composed force at the body's link frame. (num_envs, num_bodies, 3)
+            wp.array: Composed force in mixed representation. Shape: (num_envs, num_bodies, 3)
         """
-        return self._composed_force_b
+        return self._composed_force_m
 
     @property
     def composed_torque(self) -> wp.array:
-        """Composed torque at the body's link frame.
+        """Composed torque in mixed representation: origin at link frame, orientation in global frame.
 
-        .. note:: If some of the torques are applied in the global frame, the composed torque will be in the link frame
-        of the body.
+        Torques are stored in "mixed" representation.
+        This includes both user-provided torques and torque contributions from forces applied at
+        offset positions (τ = (pos - link_origin) × force).
 
         Returns:
-            wp.array: Composed torque at the body's link frame. (num_envs, num_bodies, 3)
+            wp.array: Composed torque in global frame. Shape: (num_envs, num_bodies, 3)
         """
-        return self._composed_torque_b
+        return self._composed_torque_m
 
     @property
     def composed_force_as_torch(self) -> torch.Tensor:
-        """Composed force at the body's link frame as torch tensor.
+        """Composed force in mixed representation: origin at link frame, orientation in global frame, as torch tensor.
 
-        .. note:: If some of the forces are applied in the global frame, the composed force will be in the link frame
-        of the body.
+        Forces are stored in "mixed" representation.
 
         Returns:
-            torch.Tensor: Composed force at the body's link frame. (num_envs, num_bodies, 3)
+            torch.Tensor: Composed force in mixed representation. Shape: (num_envs, num_bodies, 3)
         """
-        return self._composed_force_b_torch
+        return self._composed_force_m_torch
 
     @property
     def composed_torque_as_torch(self) -> torch.Tensor:
-        """Composed torque at the body's link frame as torch tensor.
+        """Composed torque mixed representation: origin at link frame, orientation in global frame, as torch tensor.
 
-        .. note:: If some of the torques are applied in the global frame, the composed torque will be in the link frame
-        of the body.
+        Torques are stored in "mixed" representation.
 
         Returns:
-            torch.Tensor: Composed torque at the body's link frame. (num_envs, num_bodies, 3)
+            torch.Tensor: Composed torque in mixed representation. Shape: (num_envs, num_bodies, 3)
         """
-        return self._composed_torque_b_torch
+        return self._composed_torque_m_torch
 
     def add_forces_and_torques(
         self,
@@ -131,22 +131,28 @@ def add_forces_and_torques(
     ):
         """Add forces and torques to the composed force and torque.
 
-        Composed force and torque are the sum of all the forces and torques applied to the body.
-        It can compose global wrenches and local wrenches. The result is always in the link frame of the body.
+        It can compose global wrenches and local wrenches.
+        It first convert them to the mixed representation and then add them to the already composed force and torque.
 
-        The user can provide any combination of forces, torques, and positions.
+        Forces and torques are always stored in mixed representation: global frame orientation and application point at the link frame.
+        
+        Positions are NOT stored - they are used to compute torque contributions from forces applied at
+        offset positions (τ = (pos - link_origin) × force).
 
         .. note:: Users may want to call `reset` function after every simulation step to ensure no force is carried
         over to the next step. However, this may not necessary if the user calls `set_forces_and_torques` function
         instead of `add_forces_and_torques`.
 
         Args:
-            forces: Forces. (num_envs, num_bodies, 3). Defaults to None.
-            torques: Torques. (num_envs, num_bodies, 3). Defaults to None.
-            positions: Positions. (num_envs, num_bodies, 3). Defaults to None.
-            body_ids: Body ids. (num_envs, num_bodies). Defaults to None (all bodies).
-            env_ids: Environment ids. (num_envs). Defaults to None (all environments).
-            is_global: Whether the forces and torques are applied in the global frame. Defaults to False.
+            forces: Forces. Shape: (len(env_ids), len(body_ids), 3). Defaults to None.
+            torques: Torques. Shape: (len(env_ids), len(body_ids), 3). Defaults to None.
+            positions: Positions. Shape: (len(env_ids), len(body_ids), 3). Defaults to None.
+                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.
+            body_ids: Body ids. Defaults to None (all bodies).
+            env_ids: Environment ids. Defaults to None (all environments).
+            is_global: Whether forces and torques are in global frame. Defaults to False.
 
         Raises:
             ValueError: If the type of the input is not supported.
@@ -195,7 +201,7 @@ def add_forces_and_torques(
             self._link_quaternions = wp.from_torch(
                 convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf
             )
-            self._link_poses_updated = True
+            # self._link_poses_updated = True
 
         # Set the active flag to true
         self._active = True
@@ -211,8 +217,8 @@ def add_forces_and_torques(
                 positions,
                 self._link_positions,
                 self._link_quaternions,
-                self._composed_force_b,
-                self._composed_torque_b,
+                self._composed_force_m,
+                self._composed_torque_m,
                 is_global,
             ],
             device=self.device,
@@ -227,20 +233,29 @@ def set_forces_and_torques(
         env_ids: wp.array | torch.Tensor | None = None,
         is_global: bool = False,
     ):
-        """Set forces and torques to the composed force and torque.
+        """Set forces and torques to the composed force and torque (replaces existing values).
 
-        Composed force and torque are the sum of all the forces and torques applied to the body.
-        It can compose global wrenches and local wrenches. The result is always in the link frame of the body.
+        It can compose global wrenches and local wrenches.
+        It first convert them to the mixed representation and then add them to the already composed force and torque.
+        
+        Forces and torques are always stored in "mixed" representation: global frame orientation,
+        with application point at the link frame.
+        
+        Positions are NOT stored - they are used to compute torque
+        contributions from forces applied at offset positions (τ = (pos - link_origin) × force).
 
-        The user can provide any combination of forces, torques, and positions.
+        The total torque set is: user_torque + cross(position - link_origin, force).
 
         Args:
-            forces: Forces. (num_envs, num_bodies, 3). Defaults to None.
-            torques: Torques. (num_envs, num_bodies, 3). Defaults to None.
-            positions: Positions. (num_envs, num_bodies, 3). Defaults to None.
-            body_ids: Body ids. (num_envs, num_bodies). Defaults to None (all bodies).
-            env_ids: Environment ids. (num_envs). Defaults to None (all environments).
-            is_global: Whether the forces and torques are applied in the global frame. Defaults to False.
+            forces: Forces. Shape: (len(env_ids), len(body_ids), 3). Defaults to None.
+            torques: Torques. Shape: (len(env_ids), len(body_ids), 3). Defaults to None.
+            positions: Positions. Shape: (len(env_ids), len(body_ids), 3). Defaults to None.
+                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.
+            body_ids: Body ids. Defaults to None (all bodies).
+            env_ids: Environment ids. Defaults to None (all environments).
+            is_global: Whether forces and torques are in global frame. Defaults to False.
 
         Raises:
             ValueError: If the type of the input is not supported.
@@ -294,7 +309,7 @@ def set_forces_and_torques(
             self._link_quaternions = wp.from_torch(
                 convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf
             )
-            self._link_poses_updated = True
+            # self._link_poses_updated = True
 
         # Set the active flag to true
         self._active = True
@@ -310,8 +325,8 @@ def set_forces_and_torques(
                 positions,
                 self._link_positions,
                 self._link_quaternions,
-                self._composed_force_b,
-                self._composed_torque_b,
+                self._composed_force_m,
+                self._composed_torque_m,
                 is_global,
             ],
             device=self.device,
@@ -328,8 +343,8 @@ def reset(self, env_ids: wp.array | torch.Tensor | None = None):
         over to the next step.
         """
         if env_ids is None:
-            self._composed_force_b.zero_()
-            self._composed_torque_b.zero_()
+            self._composed_force_m.zero_()
+            self._composed_torque_m.zero_()
             self._active = False
         else:
             indices = env_ids
@@ -343,7 +358,7 @@ def reset(self, env_ids: wp.array | torch.Tensor | None = None):
                 else:
                     indices = env_ids
 
-            self._composed_force_b[indices].zero_()
-            self._composed_torque_b[indices].zero_()
+            self._composed_force_m[indices].zero_()
+            self._composed_torque_m[indices].zero_()
 
         self._link_poses_updated = False

From 29cbc014f6b8a541cf37c57a93270fef5fc758f7 Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Thu, 12 Feb 2026 16:49:57 +0100
Subject: [PATCH 2/9] remove not needed variable

---
 .../isaaclab/utils/wrench_composer.py         | 25 ++++++-------------
 1 file changed, 8 insertions(+), 17 deletions(-)

diff --git a/source/isaaclab/isaaclab/utils/wrench_composer.py b/source/isaaclab/isaaclab/utils/wrench_composer.py
index b267d7e5eb3..3306c8aa53c 100644
--- a/source/isaaclab/isaaclab/utils/wrench_composer.py
+++ b/source/isaaclab/isaaclab/utils/wrench_composer.py
@@ -65,9 +65,6 @@ def __init__(self, asset: Articulation | RigidObject | RigidObjectCollection) ->
         self._ALL_ENV_INDICES_TORCH = wp.to_torch(self._ALL_ENV_INDICES_WP)
         self._ALL_BODY_INDICES_TORCH = wp.to_torch(self._ALL_BODY_INDICES_WP)
 
-        # # Flag to check if the link poses have been updated.
-        self._link_poses_updated = False
-
     @property
     def active(self) -> bool:
         """Whether the wrench composer is active."""
@@ -196,12 +193,10 @@ def add_forces_and_torques(
             positions = wp.from_torch(positions, dtype=wp.vec3f)
 
         # Get the link positions and quaternions
-        if not self._link_poses_updated:
-            self._link_positions = wp.from_torch(self._get_link_position_fn().clone(), dtype=wp.vec3f)
-            self._link_quaternions = wp.from_torch(
-                convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf
-            )
-            # self._link_poses_updated = True
+        self._link_positions = wp.from_torch(self._get_link_position_fn().clone(), dtype=wp.vec3f)
+        self._link_quaternions = wp.from_torch(
+            convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf
+        )
 
         # Set the active flag to true
         self._active = True
@@ -304,12 +299,10 @@ def set_forces_and_torques(
             positions = wp.from_torch(positions, dtype=wp.vec3f)
 
         # Get the link positions and quaternions
-        if not self._link_poses_updated:
-            self._link_positions = wp.from_torch(self._get_link_position_fn().clone(), dtype=wp.vec3f)
-            self._link_quaternions = wp.from_torch(
-                convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf
-            )
-            # self._link_poses_updated = True
+        self._link_positions = wp.from_torch(self._get_link_position_fn().clone(), dtype=wp.vec3f)
+        self._link_quaternions = wp.from_torch(
+            convert_quat(self._get_link_quaternion_fn().clone(), to="xyzw"), dtype=wp.quatf
+        )
 
         # Set the active flag to true
         self._active = True
@@ -360,5 +353,3 @@ def reset(self, env_ids: wp.array | torch.Tensor | None = None):
 
             self._composed_force_m[indices].zero_()
             self._composed_torque_m[indices].zero_()
-
-        self._link_poses_updated = False

From b2b5965ca82f978320c1f0e330412bb808160fef Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Thu, 12 Feb 2026 18:16:30 +0100
Subject: [PATCH 3/9] start updating python tests fo wrench_composer

---
 .../test/utils/test_wrench_composer.py        | 114 ++++++++++++------
 1 file changed, 77 insertions(+), 37 deletions(-)

diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py
index 3cc88b3b902..02dedab0e26 100644
--- a/source/isaaclab/test/utils/test_wrench_composer.py
+++ b/source/isaaclab/test/utils/test_wrench_composer.py
@@ -109,6 +109,25 @@ def quat_rotate_inv_np(quat_wxyz: np.ndarray, vec: np.ndarray) -> np.ndarray:
     return vec + w * t + np.cross(-xyz, t, axis=-1)
 
 
+def quat_rotate_np(quat_wxyz: np.ndarray, vec: np.ndarray) -> np.ndarray:
+    """Rotate a vector by a quaternion (numpy).
+
+    Args:
+        quat_wxyz: Quaternion in (w, x, y, z) format. Shape: (..., 4)
+        vec: Vector to rotate. Shape: (..., 3)
+
+    Returns:
+        Rotated vector. Shape: (..., 3)
+    """
+    # Extract components
+    w = quat_wxyz[..., 0:1]
+    xyz = quat_wxyz[..., 1:4]
+
+    # Using the formula: v' = v + 2*w*(xyz x v) + 2*(xyz x (xyz x v))
+    t = 2.0 * np.cross(xyz, vec, axis=-1)
+    return vec + w * t + np.cross(xyz, t, axis=-1)
+
+
 def random_unit_quaternion_np(rng: np.random.Generator, shape: tuple) -> np.ndarray:
     """Generate random unit quaternions in (w, x, y, z) format.
 
@@ -416,7 +435,7 @@ def test_wrench_composer_reset(device: str, num_envs: int, num_bodies: int):
 @pytest.mark.parametrize("num_envs", [1, 10, 100])
 @pytest.mark.parametrize("num_bodies", [1, 3, 5])
 def test_global_forces_with_rotation(device: str, num_envs: int, num_bodies: int):
-    """Test that global forces are correctly rotated to the local frame."""
+    """Test that global forces stay unchanged in mixed representation (global orientation)."""
     rng = np.random.default_rng(seed=10)
 
     for _ in range(5):
@@ -435,13 +454,13 @@ def test_global_forces_with_rotation(device: str, num_envs: int, num_bodies: int
         # Apply global forces
         wrench_composer.add_forces_and_torques(forces=forces_global, is_global=True)
 
-        # Compute expected local forces by rotating global forces by inverse quaternion
-        expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np)
+        # In mixed representation, global forces stay unchanged (already in global orientation)
+        expected_forces_mixed = forces_global_np
 
         # Verify
         composed_force_np = wrench_composer.composed_force.numpy()
-        assert np.allclose(composed_force_np, expected_forces_local, atol=1e-4, rtol=1e-5), (
-            f"Global force rotation failed.\nExpected:\n{expected_forces_local}\nGot:\n{composed_force_np}"
+        assert np.allclose(composed_force_np, expected_forces_mixed, atol=1e-4, rtol=1e-5), (
+            f"Global force in mixed repr failed.\nExpected:\n{expected_forces_mixed}\nGot:\n{composed_force_np}"
         )
 
 
@@ -449,7 +468,7 @@ def test_global_forces_with_rotation(device: str, num_envs: int, num_bodies: int
 @pytest.mark.parametrize("num_envs", [1, 10, 100])
 @pytest.mark.parametrize("num_bodies", [1, 3, 5])
 def test_global_torques_with_rotation(device: str, num_envs: int, num_bodies: int):
-    """Test that global torques are correctly rotated to the local frame."""
+    """Test that global torques stay unchanged in mixed representation (global orientation)."""
     rng = np.random.default_rng(seed=11)
 
     for _ in range(5):
@@ -468,13 +487,13 @@ def test_global_torques_with_rotation(device: str, num_envs: int, num_bodies: in
         # Apply global torques
         wrench_composer.add_forces_and_torques(torques=torques_global, is_global=True)
 
-        # Compute expected local torques
-        expected_torques_local = quat_rotate_inv_np(link_quat_np, torques_global_np)
+        # In mixed representation, global torques stay unchanged (already in global orientation)
+        expected_torques_mixed = torques_global_np
 
         # Verify
         composed_torque_np = wrench_composer.composed_torque.numpy()
-        assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-4, rtol=1e-5), (
-            f"Global torque rotation failed.\nExpected:\n{expected_torques_local}\nGot:\n{composed_torque_np}"
+        assert np.allclose(composed_torque_np, expected_torques_mixed, atol=1e-4, rtol=1e-5), (
+            f"Global torque in mixed repr failed.\nExpected:\n{expected_torques_mixed}\nGot:\n{composed_torque_np}"
         )
 
 
@@ -505,32 +524,26 @@ def test_global_forces_at_global_position(device: str, num_envs: int, num_bodies
         # Apply global forces at global positions
         wrench_composer.add_forces_and_torques(forces=forces_global, positions=positions_global, is_global=True)
 
-        # Compute expected results:
-        # 1. Force in local frame = quat_rotate_inv(link_quat, global_force)
-        expected_forces_local = quat_rotate_inv_np(link_quat_np, forces_global_np)
+        # Compute expected results in mixed representation:
+        # 1. Force stays unchanged (already in global orientation)
+        expected_forces_mixed = forces_global_np
 
-        # 2. Position offset in local frame = global_position - link_position (then used for torque)
+        # 2. Position offset in global frame = global_position - link_position
         position_offset_global = positions_global_np - link_pos_np
 
-        # 3. Torque = skew(position_offset_global) @ force_global, then rotate to local
-        expected_torques_local = np.zeros((num_envs, num_bodies, 3), dtype=np.float32)
-        for i in range(num_envs):
-            for j in range(num_bodies):
-                pos_offset = position_offset_global[i, j]  # global frame offset
-                force_local = expected_forces_local[i, j]  # local frame force
-                # skew(pos_offset) @ force_local
-                expected_torques_local[i, j] = np.cross(pos_offset, force_local)
+        # 3. Torque = cross(position_offset, force) in global frame
+        expected_torques_mixed = np.cross(position_offset_global, forces_global_np)
 
         # Verify forces
         composed_force_np = wrench_composer.composed_force.numpy()
-        assert np.allclose(composed_force_np, expected_forces_local, atol=1e-3, rtol=1e-4), (
-            f"Global force at position failed.\nExpected forces:\n{expected_forces_local}\nGot:\n{composed_force_np}"
+        assert np.allclose(composed_force_np, expected_forces_mixed, atol=1e-3, rtol=1e-4), (
+            f"Global force at position failed.\nExpected forces:\n{expected_forces_mixed}\nGot:\n{composed_force_np}"
         )
 
         # Verify torques
         composed_torque_np = wrench_composer.composed_torque.numpy()
-        assert np.allclose(composed_torque_np, expected_torques_local, atol=1e-3, rtol=1e-4), (
-            f"Global force at position failed.\nExpected torques:\n{expected_torques_local}\nGot:\n{composed_torque_np}"
+        assert np.allclose(composed_torque_np, expected_torques_mixed, atol=1e-3, rtol=1e-4), (
+            f"Global force at position failed.\nExpected torques:\n{expected_torques_mixed}\nGot:\n{composed_torque_np}"
         )
 
 
@@ -592,18 +605,45 @@ def test_90_degree_rotation_global_force(device: str):
 
     wrench_composer.add_forces_and_torques(forces=force_wp, is_global=True)
 
-    # Expected: After inverse rotation (rotate by -90° around Z), X becomes -Y
-    # Actually, inverse rotation of +90° around Z applied to (1,0,0) gives (0,-1,0)
-    expected_force_local = np.array([[[0.0, -1.0, 0.0]]], dtype=np.float32)
+    # In mixed representation, global forces stay unchanged
+    expected_force_mixed = np.array([[[1.0, 0.0, 0.0]]], dtype=np.float32)
+
+    composed_force_np = wrench_composer.composed_force.numpy()
+    assert np.allclose(composed_force_np, expected_force_mixed, atol=1e-5), (
+        f"90-degree rotation test failed.\nExpected:\n{expected_force_mixed}\nGot:\n{composed_force_np}"
+    )
+
+@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
+def test_90_degree_rotation_local_force(device: str):
+    """Test local force with a known 90-degree rotation for easy verification."""
+    num_envs, num_bodies = 1, 1
+
+    # 90-degree rotation around Z-axis: (w, x, y, z) = (cos(45°), 0, 0, sin(45°))
+    # This rotates X -> Y, Y -> -X
+    angle = np.pi / 2
+    link_quat_np = np.array([[[[np.cos(angle / 2), 0, 0, np.sin(angle / 2)]]]], dtype=np.float32).reshape(1, 1, 4)
+    link_quat_torch = torch.from_numpy(link_quat_np)
+
+    mock_asset = MockRigidObject(num_envs, num_bodies, device, link_quat=link_quat_torch)
+    wrench_composer = WrenchComposer(mock_asset)
+
+    # Apply force in local +X direction
+    force_local = np.array([[[1.0, 0.0, 0.0]]], dtype=np.float32)
+    force_wp = wp.from_numpy(force_local, dtype=wp.vec3f, device=device)
+
+    wrench_composer.add_forces_and_torques(forces=force_wp, is_global=False)
+
+    # In mixed representation, local forces get rotated to global: local +X becomes global +Y
+    expected_force_mixed = np.array([[[0.0, 1.0, 0.0]]], dtype=np.float32)
 
     composed_force_np = wrench_composer.composed_force.numpy()
-    assert np.allclose(composed_force_np, expected_force_local, atol=1e-5), (
-        f"90-degree rotation test failed.\nExpected:\n{expected_force_local}\nGot:\n{composed_force_np}"
+    assert np.allclose(composed_force_np, expected_force_mixed, atol=1e-5), (
+        f"90-degree rotation test failed.\nExpected:\n{expected_force_mixed}\nGot:\n{composed_force_np}"
     )
 
 
 @pytest.mark.parametrize("device", ["cuda:0", "cpu"])
-def test_composition_mixed_local_and_global(device: str):
+def test_composition_local_and_global(device: str):
     """Test that local and global forces can be composed together correctly."""
     rng = np.random.default_rng(seed=14)
     num_envs, num_bodies = 5, 3
@@ -628,9 +668,9 @@ def test_composition_mixed_local_and_global(device: str):
     # Add global forces
     wrench_composer.add_forces_and_torques(forces=forces_global, is_global=True)
 
-    # Expected: local forces stay as-is, global forces get rotated, then sum
-    global_forces_in_local = quat_rotate_inv_np(link_quat_np, forces_global_np)
-    expected_total = forces_local_np + global_forces_in_local
+    # In mixed repr: local forces get rotated to global, global forces stay as-is
+    local_forces_in_global = quat_rotate_np(link_quat_np, forces_local_np)
+    expected_total = local_forces_in_global + forces_global_np
 
     composed_force_np = wrench_composer.composed_force.numpy()
     assert np.allclose(composed_force_np, expected_total, atol=1e-4, rtol=1e-5), (
@@ -701,8 +741,8 @@ def test_global_force_at_link_origin_no_torque(device: str):
     # Apply global forces at link origin
     wrench_composer.add_forces_and_torques(forces=forces_global, positions=positions_at_link, is_global=True)
 
-    # Expected: force rotated to local, torque = 0 (since position offset is zero)
-    expected_forces = quat_rotate_inv_np(link_quat_np, forces_global_np)
+    # In mixed repr: global forces stay unchanged, torque = 0 (since position offset is zero)
+    expected_forces = forces_global_np
     expected_torques = np.zeros((num_envs, num_bodies, 3), dtype=np.float32)
 
     composed_force_np = wrench_composer.composed_force.numpy()

From 48ab7a0866dd4f7904ece032492f6174c7a5c6dd Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Fri, 13 Feb 2026 10:14:37 +0100
Subject: [PATCH 4/9] modify tests to include also set of torques

---
 .../test/utils/test_wrench_composer.py        | 20 +++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py
index 02dedab0e26..0cc63c431b6 100644
--- a/source/isaaclab/test/utils/test_wrench_composer.py
+++ b/source/isaaclab/test/utils/test_wrench_composer.py
@@ -681,7 +681,7 @@ def test_composition_local_and_global(device: str):
 @pytest.mark.parametrize("device", ["cuda:0", "cpu"])
 @pytest.mark.parametrize("num_envs", [1, 10, 50])
 @pytest.mark.parametrize("num_bodies", [1, 3, 5])
-def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies: int):
+def test_local_forces_and_torques_at_local_position(device: str, num_envs: int, num_bodies: int):
     """Test local forces at local positions (offset from link frame)."""
     rng = np.random.default_rng(seed=15)
 
@@ -697,23 +697,27 @@ def test_local_forces_at_local_position(device: str, num_envs: int, num_bodies:
 
         # Generate random local forces and local positions (offsets)
         forces_local_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32)
+        torques_local_np = rng.uniform(-50.0, 50.0, (num_envs, num_bodies, 3)).astype(np.float32)
         positions_local_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32)
         forces_local = wp.from_numpy(forces_local_np, dtype=wp.vec3f, device=device)
+        torques_local = wp.from_numpy(torques_local_np, dtype=wp.vec3f, device=device)
         positions_local = wp.from_numpy(positions_local_np, dtype=wp.vec3f, device=device)
 
-        # Apply local forces at local positions
-        wrench_composer.add_forces_and_torques(forces=forces_local, positions=positions_local, is_global=False)
+        # Apply local forces and torques at local positions
+        wrench_composer.add_forces_and_torques(forces=forces_local, torques=torques_local, positions=positions_local, is_global=False)
 
-        # Expected: forces stay as-is, torque = cross(position, force)
-        expected_forces = forces_local_np
-        expected_torques = np.cross(positions_local_np, forces_local_np)
+        # In mixed repr: local forces get rotated to global
+        expected_forces = quat_rotate_np(link_quat_np, forces_local_np)
+        # In mixed repr: torque = cross(pos_mixed, force_mixed) + quat_rotate(torques_local)
+        positions_mixed = quat_rotate_np(link_quat_np, positions_local_np)
+        expected_torques = np.cross(positions_mixed, expected_forces) + quat_rotate_np(link_quat_np, torques_local_np)
 
         # Verify
         composed_force_np = wrench_composer.composed_force.numpy()
         composed_torque_np = wrench_composer.composed_torque.numpy()
 
-        assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5)
-        assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5)
+        assert np.allclose(composed_force_np, expected_forces, atol=1e-3, rtol=1e-5)
+        assert np.allclose(composed_torque_np, expected_torques, atol=1e-3, rtol=1e-5)
 
 
 @pytest.mark.parametrize("device", ["cuda:0", "cpu"])

From 17b3850e84292da06a1923f1ab10ec01a20b4082 Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Fri, 13 Feb 2026 10:16:14 +0100
Subject: [PATCH 5/9] add name to contributors

---
 CONTRIBUTORS.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md
index 9fbfe7f1bf3..a00d405f812 100644
--- a/CONTRIBUTORS.md
+++ b/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

From 542488c88ceabae18c02bc4f18cb6dc8250bd861 Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Fri, 13 Feb 2026 10:29:44 +0100
Subject: [PATCH 6/9] add test to verify that the bug addressed by the PR is
 fixed

---
 .../test/utils/test_wrench_composer.py        | 61 +++++++++++++++++++
 1 file changed, 61 insertions(+)

diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py
index 0cc63c431b6..664cce567da 100644
--- a/source/isaaclab/test/utils/test_wrench_composer.py
+++ b/source/isaaclab/test/utils/test_wrench_composer.py
@@ -754,3 +754,64 @@ def test_global_force_at_link_origin_no_torque(device: str):
 
     assert np.allclose(composed_force_np, expected_forces, atol=1e-4, rtol=1e-5)
     assert np.allclose(composed_torque_np, expected_torques, atol=1e-4, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
+def test_forces_unchanged_after_asset_pose_change(device: str):
+    """Test that stored forces remain unchanged after the asset's link pose is modified.
+
+    This verifies that the WrenchComposer correctly stores forces in "mixed" representation
+    (global frame orientation, local frame position) and doesn't re-transform them when the asset pose changes.
+    """
+    rng = np.random.default_rng(seed=17)
+    num_envs, num_bodies = 5, 3
+
+    # Step 1: Create mock asset with initial pose and assign to wrench composer
+    initial_link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32)
+    initial_link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
+    initial_link_pos_torch = torch.from_numpy(initial_link_pos_np)
+    initial_link_quat_torch = torch.from_numpy(initial_link_quat_np)
+
+    mock_asset = MockRigidObject(
+        num_envs, num_bodies, device,
+        link_pos=initial_link_pos_torch,
+        link_quat=initial_link_quat_torch
+    )
+    wrench_composer = WrenchComposer(mock_asset)
+
+    # Step 2: Set some global forces
+    forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32)
+    forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device)
+
+    wrench_composer.add_forces_and_torques(forces=forces_global, is_global=True)
+
+    # Store the composed forces immediately after setting
+    composed_force_before_np = wrench_composer.composed_force.numpy().copy()
+
+    # Step 3: Change the link position and orientation of the mock asset
+    new_link_pos_np = rng.uniform(-20.0, 20.0, (num_envs, num_bodies, 3)).astype(np.float32)
+    new_link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
+
+    # Update the mock asset's data directly
+    mock_asset.data.body_link_pos_w = torch.from_numpy(new_link_pos_np).to(device=device, dtype=torch.float32)
+    mock_asset.data.body_link_quat_w = torch.from_numpy(new_link_quat_np).to(device=device, dtype=torch.float32)
+
+    # Step 4: Add zero forces to trigger any internal updates in the wrench composer
+    wrench_composer.add_forces_and_torques(forces=wp.zeros_like(forces_global), is_global=True)
+
+    # Step 5: Get the forces from the wrench composer after the pose update
+    composed_force_after_np = wrench_composer.composed_force.numpy()
+
+    # Step 6: Verify forces remain unchanged (stored in mixed representation, independent of current pose)
+    assert np.allclose(composed_force_after_np, composed_force_before_np, atol=1e-6), (
+        f"Forces should remain unchanged after asset pose change.\n"
+        f"Before:\n{composed_force_before_np}\n"
+        f"After:\n{composed_force_after_np}"
+    )
+
+    # Also verify the forces match what we originally set (global forces should be unchanged in mixed representation)
+    assert np.allclose(composed_force_after_np, forces_global_np, atol=1e-4, rtol=1e-5), (
+        f"Forces should match the originally set global forces.\n"
+        f"Expected:\n{forces_global_np}\n"
+        f"Got:\n{composed_force_after_np}"
+    )

From 4dc4eb233fac2d7cca443efbac4c5ce9032a31a0 Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Fri, 13 Feb 2026 11:50:07 +0100
Subject: [PATCH 7/9] improve the test of the wrench composer when links poses
 change

---
 .../test/utils/test_wrench_composer.py        | 77 +++++++++----------
 1 file changed, 38 insertions(+), 39 deletions(-)

diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py
index 664cce567da..1cac46ad9ff 100644
--- a/source/isaaclab/test/utils/test_wrench_composer.py
+++ b/source/isaaclab/test/utils/test_wrench_composer.py
@@ -757,61 +757,60 @@ def test_global_force_at_link_origin_no_torque(device: str):
 
 
 @pytest.mark.parametrize("device", ["cuda:0", "cpu"])
-def test_forces_unchanged_after_asset_pose_change(device: str):
-    """Test that stored forces remain unchanged after the asset's link pose is modified.
+def test_forces_after_asset_pose_change(device: str):
+    """Test that stored forces accumulate correctly as asset pose changes over multiple iterations.
 
     This verifies that the WrenchComposer correctly stores forces in "mixed" representation
-    (global frame orientation, local frame position) and doesn't re-transform them when the asset pose changes.
+    (global frame orientation, local frame position) and properly transforms new forces based
+    on the current pose, while previously stored forces remain unchanged.
     """
     rng = np.random.default_rng(seed=17)
     num_envs, num_bodies = 5, 3
+    num_iterations = 10
 
-    # Step 1: Create mock asset with initial pose and assign to wrench composer
-    initial_link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32)
-    initial_link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
-    initial_link_pos_torch = torch.from_numpy(initial_link_pos_np)
-    initial_link_quat_torch = torch.from_numpy(initial_link_quat_np)
+    # Create mock asset with initial pose
+    link_pos_np = rng.uniform(-10.0, 10.0, (num_envs, num_bodies, 3)).astype(np.float32)
+    link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
+    link_pos_torch = torch.from_numpy(link_pos_np)
+    link_quat_torch = torch.from_numpy(link_quat_np)
 
     mock_asset = MockRigidObject(
         num_envs, num_bodies, device,
-        link_pos=initial_link_pos_torch,
-        link_quat=initial_link_quat_torch
+        link_pos=link_pos_torch,
+        link_quat=link_quat_torch
     )
     wrench_composer = WrenchComposer(mock_asset)
 
-    # Step 2: Set some global forces
-    forces_global_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32)
-    forces_global = wp.from_numpy(forces_global_np, dtype=wp.vec3f, device=device)
+    # Track expected accumulated force in mixed representation
+    expected_accumulated_force_np = np.zeros((num_envs, num_bodies, 3), dtype=np.float32)
 
-    wrench_composer.add_forces_and_torques(forces=forces_global, is_global=True)
+    for iteration in range(num_iterations):
+        # Randomly choose whether to apply local or global forces
+        use_global = rng.choice([True, False])
 
-    # Store the composed forces immediately after setting
-    composed_force_before_np = wrench_composer.composed_force.numpy().copy()
+        # Generate random forces
+        forces_np = rng.uniform(-100.0, 100.0, (num_envs, num_bodies, 3)).astype(np.float32)
+        forces_wp = wp.from_numpy(forces_np, dtype=wp.vec3f, device=device)
 
-    # Step 3: Change the link position and orientation of the mock asset
-    new_link_pos_np = rng.uniform(-20.0, 20.0, (num_envs, num_bodies, 3)).astype(np.float32)
-    new_link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
+        # Apply forces
+        wrench_composer.add_forces_and_torques(forces=forces_wp, is_global=use_global)
 
-    # Update the mock asset's data directly
-    mock_asset.data.body_link_pos_w = torch.from_numpy(new_link_pos_np).to(device=device, dtype=torch.float32)
-    mock_asset.data.body_link_quat_w = torch.from_numpy(new_link_quat_np).to(device=device, dtype=torch.float32)
-
-    # Step 4: Add zero forces to trigger any internal updates in the wrench composer
-    wrench_composer.add_forces_and_torques(forces=wp.zeros_like(forces_global), is_global=True)
+        # Update expected accumulated force based on current pose
+        if use_global:
+            # Global forces are stored as-is in mixed representation
+            expected_accumulated_force_np += forces_np
+        else:
+            # Local forces are rotated by current link quaternion to mixed representation
+            forces_in_mixed = quat_rotate_np(link_quat_np, forces_np)
+            expected_accumulated_force_np += forces_in_mixed
 
-    # Step 5: Get the forces from the wrench composer after the pose update
-    composed_force_after_np = wrench_composer.composed_force.numpy()
+        # Verify composed forces match expected accumulation
+        composed_force_np = wrench_composer.composed_force.numpy()
+        assert np.allclose(composed_force_np, expected_accumulated_force_np, atol=1e-3, rtol=1e-5)
 
-    # Step 6: Verify forces remain unchanged (stored in mixed representation, independent of current pose)
-    assert np.allclose(composed_force_after_np, composed_force_before_np, atol=1e-6), (
-        f"Forces should remain unchanged after asset pose change.\n"
-        f"Before:\n{composed_force_before_np}\n"
-        f"After:\n{composed_force_after_np}"
-    )
+        # Change the link position and orientation for the next iteration
+        link_pos_np = rng.uniform(-20.0, 20.0, (num_envs, num_bodies, 3)).astype(np.float32)
+        link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
 
-    # Also verify the forces match what we originally set (global forces should be unchanged in mixed representation)
-    assert np.allclose(composed_force_after_np, forces_global_np, atol=1e-4, rtol=1e-5), (
-        f"Forces should match the originally set global forces.\n"
-        f"Expected:\n{forces_global_np}\n"
-        f"Got:\n{composed_force_after_np}"
-    )
+        mock_asset.data.body_link_pos_w = torch.from_numpy(link_pos_np).to(device=device, dtype=torch.float32)
+        mock_asset.data.body_link_quat_w = torch.from_numpy(link_quat_np).to(device=device, dtype=torch.float32)
\ No newline at end of file

From 39107c5f3c03e03814ec417f6e3c3d62283a0a20 Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Fri, 13 Feb 2026 17:49:50 +0100
Subject: [PATCH 8/9] update changelog.rst and extension.toml

---
 source/isaaclab/config/extension.toml |  2 +-
 source/isaaclab/docs/CHANGELOG.rst    | 11 +++++++++++
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml
index 48c9f4d59d4..cc88c686180 100644
--- a/source/isaaclab/config/extension.toml
+++ b/source/isaaclab/config/extension.toml
@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.54.3"
+version = "0.54.4"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"
diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst
index 4b20c566ede..fd1ecc9eeed 100644
--- a/source/isaaclab/docs/CHANGELOG.rst
+++ b/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)
 ~~~~~~~~~~~~~~~~~~~
 

From 55ffb03aaa9ded0bbd6778210faca78cb13a8b2d Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <lorenzo.moretti@gbionics.ai>
Date: Fri, 13 Feb 2026 17:53:46 +0100
Subject: [PATCH 9/9] pre-commit format

---
 source/isaaclab/isaaclab/utils/warp/kernels.py   | 16 ++++++++++------
 .../isaaclab/isaaclab/utils/wrench_composer.py   |  6 +++---
 .../isaaclab/test/utils/test_wrench_composer.py  | 13 ++++++-------
 3 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/source/isaaclab/isaaclab/utils/warp/kernels.py b/source/isaaclab/isaaclab/utils/warp/kernels.py
index 01d72153918..0ad821fcf64 100644
--- a/source/isaaclab/isaaclab/utils/warp/kernels.py
+++ b/source/isaaclab/isaaclab/utils/warp/kernels.py
@@ -405,12 +405,14 @@ def add_forces_and_torques_at_position(
         if positions:
             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
+                    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_m[env_ids[tid_env], body_ids[tid_body]] += cast_torque_to_mixed_frame(
@@ -472,10 +474,12 @@ def set_forces_and_torques_at_position(
         if positions:
             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
+                    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
-                )
+                ),
             )
diff --git a/source/isaaclab/isaaclab/utils/wrench_composer.py b/source/isaaclab/isaaclab/utils/wrench_composer.py
index 3306c8aa53c..46f64098cc4 100644
--- a/source/isaaclab/isaaclab/utils/wrench_composer.py
+++ b/source/isaaclab/isaaclab/utils/wrench_composer.py
@@ -132,7 +132,7 @@ def add_forces_and_torques(
         It first convert them to the mixed representation and then add them to the already composed force and torque.
 
         Forces and torques are always stored in mixed representation: global frame orientation and application point at the link frame.
-        
+
         Positions are NOT stored - they are used to compute torque contributions from forces applied at
         offset positions (τ = (pos - link_origin) × force).
 
@@ -232,10 +232,10 @@ def set_forces_and_torques(
 
         It can compose global wrenches and local wrenches.
         It first convert them to the mixed representation and then add them to the already composed force and torque.
-        
+
         Forces and torques are always stored in "mixed" representation: global frame orientation,
         with application point at the link frame.
-        
+
         Positions are NOT stored - they are used to compute torque
         contributions from forces applied at offset positions (τ = (pos - link_origin) × force).
 
diff --git a/source/isaaclab/test/utils/test_wrench_composer.py b/source/isaaclab/test/utils/test_wrench_composer.py
index 1cac46ad9ff..8dfc56100a0 100644
--- a/source/isaaclab/test/utils/test_wrench_composer.py
+++ b/source/isaaclab/test/utils/test_wrench_composer.py
@@ -613,6 +613,7 @@ def test_90_degree_rotation_global_force(device: str):
         f"90-degree rotation test failed.\nExpected:\n{expected_force_mixed}\nGot:\n{composed_force_np}"
     )
 
+
 @pytest.mark.parametrize("device", ["cuda:0", "cpu"])
 def test_90_degree_rotation_local_force(device: str):
     """Test local force with a known 90-degree rotation for easy verification."""
@@ -704,7 +705,9 @@ def test_local_forces_and_torques_at_local_position(device: str, num_envs: int,
         positions_local = wp.from_numpy(positions_local_np, dtype=wp.vec3f, device=device)
 
         # Apply local forces and torques at local positions
-        wrench_composer.add_forces_and_torques(forces=forces_local, torques=torques_local, positions=positions_local, is_global=False)
+        wrench_composer.add_forces_and_torques(
+            forces=forces_local, torques=torques_local, positions=positions_local, is_global=False
+        )
 
         # In mixed repr: local forces get rotated to global
         expected_forces = quat_rotate_np(link_quat_np, forces_local_np)
@@ -774,11 +777,7 @@ def test_forces_after_asset_pose_change(device: str):
     link_pos_torch = torch.from_numpy(link_pos_np)
     link_quat_torch = torch.from_numpy(link_quat_np)
 
-    mock_asset = MockRigidObject(
-        num_envs, num_bodies, device,
-        link_pos=link_pos_torch,
-        link_quat=link_quat_torch
-    )
+    mock_asset = MockRigidObject(num_envs, num_bodies, device, link_pos=link_pos_torch, link_quat=link_quat_torch)
     wrench_composer = WrenchComposer(mock_asset)
 
     # Track expected accumulated force in mixed representation
@@ -813,4 +812,4 @@ def test_forces_after_asset_pose_change(device: str):
         link_quat_np = random_unit_quaternion_np(rng, (num_envs, num_bodies))
 
         mock_asset.data.body_link_pos_w = torch.from_numpy(link_pos_np).to(device=device, dtype=torch.float32)
-        mock_asset.data.body_link_quat_w = torch.from_numpy(link_quat_np).to(device=device, dtype=torch.float32)
\ No newline at end of file
+        mock_asset.data.body_link_quat_w = torch.from_numpy(link_quat_np).to(device=device, dtype=torch.float32)