Fix wrench composer to correctly update composed wrenches

[LoreMoretti]

Description

It addresses #4580.

The main change is that the force and torques of the wrench_composer are now stored in a representation which is called mixed (as used in some references, like https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf), which means orientation as the world frame and origin as the link frame.

Everything else is changed accordingly.

Type of change

• Bug fix (non-breaking change which fixes an issue)

Checklist

• I have read and understood the contribution guidelines
• I have run the pre-commit checks with ./isaaclab.sh --format
• I have made corresponding changes to the documentation
• My changes generate no new warnings
• I have added tests that prove my fix is effective or that my feature works
• I have updated the changelog and the corresponding version in the extension's config/extension.toml file
• I have added my name to the CONTRIBUTORS.md or my name already exists there

[greptile-apps]

Greptile Overview

Greptile Summary

This PR refactors the wrench composer to use "mixed" frame representation instead of pure link-frame representation. In the new approach, forces and torques are stored with global frame orientation but referenced to the link origin, which simplifies composition of forces from multiple sources.

Key changes:

• Renamed internal buffers from _composed_force_b (body frame) to _composed_force_m (mixed frame)
• Updated frame transformation functions: cast_force_to_mixed_frame, cast_torque_to_mixed_frame, cast_position_to_mixed_frame
• Changed torque calculation from wp.skew() @ to wp.cross()
• Updated is_global parameter from False to True when applying forces to PhysX simulation
• Removed _link_poses_updated caching flag - link poses now fetched fresh on each call

Critical issues found:

• Tests in test_wrench_composer.py were NOT updated to match the new mixed frame representation
• At least 6 test functions have incorrect expectations that assume the old link-frame behavior
• Tests will fail because they expect forces/torques in link frame when is_global=True, but the new implementation keeps them in global orientation

Impact:
The core logic appears sound, but the PR is incomplete without updating the test suite. The tests need to be corrected to reflect that with is_global=True, forces remain in global orientation (not rotated to link frame).

Confidence Score: 2/5

• This PR has critical issues - tests are outdated and will fail with the new implementation
• The core refactoring from link frame to mixed frame representation appears logically sound, but the test suite was not updated to match the new behavior. Multiple tests expect forces/torques to be transformed to link frame, but the new implementation stores them in global orientation (mixed representation). This will cause test failures and indicates the PR was not fully validated before submission.
• Pay close attention to source/isaaclab/test/utils/test_wrench_composer.py - at least 6 test functions need updates to match new mixed frame representation

Important Files Changed

Filename	Overview
source/isaaclab/isaaclab/utils/warp/kernels.py	Core wrench composition logic refactored from link frame to mixed frame representation (global orientation, link origin). Changed from wp.skew() @ matrix multiply to wp.cross() for torque calculation, and updated frame transformation functions.
source/isaaclab/isaaclab/utils/wrench_composer.py	Updated documentation and internal variable names from _b (body frame) to _m (mixed frame). Removed _link_poses_updated flag so link poses are fetched fresh on every call.
source/isaaclab/test/utils/test_wrench_composer.py	Tests not updated for mixed frame representation changes. Multiple test cases expect old link-frame behavior but implementation now uses mixed representation (global orientation, link origin). Tests will fail.

Sequence Diagram

sequenceDiagram
    participant User
    participant Asset
    participant WrenchComposer
    participant Kernel
    participant PhysXView
    
    User->>Asset: Apply external forces/torques
    Asset->>WrenchComposer: add_forces_and_torques(forces, torques, is_global)
    WrenchComposer->>WrenchComposer: Fetch link_positions and link_quaternions
    WrenchComposer->>Kernel: Launch add_forces_and_torques_at_position
    
    Kernel->>Kernel: cast_force_to_mixed_frame(force, quat, is_global)
    Note over Kernel: If is_global=True: keep in global frame<br/>If is_global=False: rotate to global via quat_rotate
    
    Kernel->>Kernel: cast_position_to_mixed_frame(pos, link_pos, quat, is_global)
    Note over Kernel: If is_global=True: offset = pos - link_pos<br/>If is_global=False: rotate pos via quat_rotate
    
    Kernel->>Kernel: Compute torque = cross(position_offset, force)
    Kernel->>Kernel: Add to composed_forces_m and composed_torques_m
    
    WrenchComposer-->>Asset: Returns (forces in mixed frame)
    Asset->>PhysXView: apply_forces_and_torques_at_position(force_data, torque_data, is_global=True)
    Note over PhysXView: Forces/torques are in global orientation<br/>at link origin (mixed representation)

Loading

[greptile-apps]

_{5 files reviewed, 6 comments}

_{Edit Code Review Agent Settings | Greptile}

[greptile-apps]

Additional Comments (6)

source/isaaclab/test/utils/test_wrench_composer.py
Tests need updating for new mixed frame representation. The test expects global forces to be rotated to local frame (quat_rotate_inv), but the new implementation stores forces in mixed representation (global orientation, link origin). When is_global=True, forces should remain in global frame, not be transformed to link frame.

---

source/isaaclab/test/utils/test_wrench_composer.py
Test expectations need updating. Similar to forces test, this expects global torques to be rotated to local frame, but mixed representation keeps torques in global orientation when is_global=True.

---

source/isaaclab/test/utils/test_wrench_composer.py
Test calculation needs correction. In mixed representation, forces stay in global frame and position offset remains in global frame. The torque should be cross(position_offset_global, force_global) not cross(position_offset_global, force_local).

---

source/isaaclab/test/utils/test_wrench_composer.py
Test expectations need updating. With mixed representation, both local and global forces are transformed to global orientation for storage, so the expected result should be quat_rotate(link_quat, forces_local) + forces_global, not forces_local + quat_rotate_inv(link_quat, forces_global).

---

source/isaaclab/test/utils/test_wrench_composer.py
Test needs updating for mixed representation. Local forces (is_global=False) are rotated to global orientation via quat_rotate(link_quat, force), and positions are also rotated. Expected should be rotated versions, not raw local values.

---

source/isaaclab/test/utils/test_wrench_composer.py
Test expectations need correction. Global forces should stay in global frame (not rotated to local), so expected_forces = forces_global_np not quat_rotate_inv(link_quat_np, forces_global_np).

[AntoineRichard]

Hi Lorenzo, looking into this PR I'm not sure it's working as expected. Can local forces track rotations in the case where I apply a permanent local force and rotate the body? I might be better to compose local and global forces separately and compose them into a local force just before applying them. It's a bit tedious but I feel like it would be more robust. See: gbionics#1

[LoreMoretti]

Hi Antoine, I see your point, you're right.