Fixes collision synchronization in cuRobo planner

[Nyquist0]

Fixes collision synchronization in cuRobo planner

Description

This PR fixes a bug in the cuRobo motion planner where object poses were
not correctly transformed from world frame to robot base frame during
collision world synchronization.

The issue occurred in the _sync_object_poses_with_isaaclab method, which
was directly using world-frame object poses without accounting for the
robot's base position and orientation. This caused incorrect collision
detection when the robot was not located at the origin point of world coordinate, as cuRobo expects poses relative to the robot base frame.

Please include a summary of the change and which issue is fixed. Please also include relevant motivation and context.
List any dependencies that are required for this change.

Fixes # (issue)

Type of change

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

Screenshots

Please attach before and after screenshots of the change if applicable.

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

Adds coordinate transformation from world frame to robot base frame when syncing object poses with cuRobo's world model, addressing collision detection issues when the robot is not at the origin.

However, the fix is incomplete. The PR correctly transforms object poses for the world model update (lines 631-637), but fails to apply the same transformation in the collision checker update loop (lines 673-686). This means the collision checker still receives world-frame coordinates instead of robot base-frame coordinates, which will cause the same collision detection bugs the PR intended to fix.

• The world model update loop correctly uses subtract_frame_transforms() to convert poses to robot base frame
• The collision checker update loop is missing the same transformation and still uses raw world-frame poses
• Both loops must use the same coordinate frame for consistent collision detection

Confidence Score: 1/5

• This PR contains a critical bug that leaves the collision checker with incorrect coordinate frames
• The PR attempts to fix a coordinate frame bug but only applies the fix to half of the affected code. The collision checker update loop (lines 673-686) still uses world-frame coordinates, which will cause the exact same collision detection issues the PR claims to fix. This incomplete fix may give a false sense of security while leaving the core bug present in the collision checker.
• The file source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py needs immediate attention - specifically lines 673-686 require the same coordinate transformation applied to lines 631-637

Important Files Changed

File Analysis

Filename	Score	Overview
source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py	1/5	Adds coordinate transformation for world model updates but misses same fix for collision checker updates - incomplete bug fix

Sequence Diagram

sequenceDiagram
    participant IL as Isaac Lab
    participant SP as _sync_object_poses
    participant RO as Rigid Objects
    participant WM as World Model
    participant CC as Collision Checker
    
    Note over SP: Collision Synchronization Flow
    
    SP->>RO: Get object mappings
    
    loop For each dynamic object
        SP->>RO: Get world-frame pose (root_pos_w, root_quat_w)
        SP->>SP: Subtract env_origin
        SP->>SP: Transform to robot base frame (subtract_frame_transforms)
        Note over SP: ✅ Lines 631-637: Added in this PR
        SP->>SP: Convert to cuRobo device
        SP->>WM: Update object pose in world model
    end
    
    alt If objects updated > 0
        loop For each dynamic object
            SP->>RO: Get world-frame pose (root_pos_w, root_quat_w)
            SP->>SP: Subtract env_origin
            Note over SP: ❌ Lines 673-680: Missing transformation!
            SP->>SP: Convert to cuRobo device
            SP->>CC: update_obstacle_pose() with world-frame coords
            Note right of CC: Incorrect: expects robot base frame
        end
    end

Loading

[greptile-apps]

Additional Comments (1)

1. source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py, line 673-680 (link)

   logic: Missing coordinate transformation from world frame to robot base frame. The world model update loop (lines 631-637) correctly transforms object poses, but this collision checker update loop still uses world-frame coordinates.

_{1 file reviewed, 1 comment}

_{Edit Code Review Agent Settings | Greptile}