From 8810cc4149d86cdc2c5d2adb61b3a9552b6a481d Mon Sep 17 00:00:00 2001
From: zehao-wang <zekewang@outlook.com>
Date: Tue, 23 Sep 2025 22:53:16 +0200
Subject: [PATCH] Fix missing target pose transfer to robot coordinates in
 example

The original example code was missing the function for transferring the target pose into robot coordinates. This could cause misunderstandings when using curobo, as noted in issue #545.

This pull request adds the missing transformation step and improves the example to avoid confusion.
---
 examples/isaac_sim/helper.py             |  3 +-
 examples/isaac_sim/motion_gen_reacher.py | 46 ++++++++++++++++++++++--
 2 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/examples/isaac_sim/helper.py b/examples/isaac_sim/helper.py
index c50dee6f..7d16ea40 100644
--- a/examples/isaac_sim/helper.py
+++ b/examples/isaac_sim/helper.py
@@ -80,6 +80,7 @@ def add_robot_to_scene(
     subroot: str = "",
     robot_name: str = "robot",
     position: np.array = np.array([0, 0, 0]),
+    orientation: np.array = np.array([1, 0, 0, 0]),
     initialize_world: bool = True,
 ):
 
@@ -167,7 +168,7 @@ def add_robot_to_scene(
     robot_prim = robot_p.prim
     stage = robot_prim.GetStage()
     linkp = stage.GetPrimAtPath(robot_path)
-    set_prim_transform(linkp, [position[0], position[1], position[2], 1, 0, 0, 0])
+    set_prim_transform(linkp, [position[0], position[1], position[2], orientation[0], orientation[1], orientation[2], orientation[3]])
 
     robot = my_world.scene.add(robot_p)
     if initialize_world:
diff --git a/examples/isaac_sim/motion_gen_reacher.py b/examples/isaac_sim/motion_gen_reacher.py
index 77cc43d8..29cf696b 100644
--- a/examples/isaac_sim/motion_gen_reacher.py
+++ b/examples/isaac_sim/motion_gen_reacher.py
@@ -138,11 +138,46 @@
 
 ############################################################
 
+from scipy.spatial.transform import Rotation as R
+def _world_to_base_scipy(tar_pos, tar_quat, base_pos, base_quat, quat_format='wxyz'):
+    """Deal with issues in curobo, which always need relative pose to robot not pose in world"""
+
+    # Handle quaternion format conversion for scipy (expects xyzw)
+    if quat_format == "wxyz":
+        # Convert wxyz to xyzw for scipy
+        tar_quat_scipy = np.array([tar_quat[1], tar_quat[2], tar_quat[3], tar_quat[0]])
+        base_quat_scipy = np.array([base_quat[1], base_quat[2], base_quat[3], base_quat[0]])
+    else:
+        tar_quat_scipy = tar_quat
+        base_quat_scipy = base_quat
+    
+    # Create rotation objects
+    R_base = R.from_quat(base_quat_scipy)
+    R_target = R.from_quat(tar_quat_scipy)
+    
+    # Transform position: relative_pos = R_base^(-1) * (tar_pos - base_pos)
+    relative_pos = R_base.inv().apply(tar_pos - base_pos)
+    
+    # Transform orientation: relative_rotation = R_base^(-1) * R_target
+    R_relative = R_base.inv() * R_target
+    relative_quat_scipy = R_relative.as_quat()  # Returns xyzw
+    
+    # Convert back to requested quaternion format
+    if quat_format == "wxyz":
+        relative_quat = np.array([relative_quat_scipy[3], relative_quat_scipy[0], 
+                                 relative_quat_scipy[1], relative_quat_scipy[2]])
+    else:
+        relative_quat = relative_quat_scipy
+    
+    return relative_pos.astype(np.float32), relative_quat.astype(np.float32)
 
 ########### OV #################;;;;;
 
 
 def main():
+    robot_pos = np.array([0.0, 0.0, 0.0])
+    robot_quat = np.array([1.0, 0.0, 0.0, 0.0])  # wxyz
+
     # create a curobo motion gen instance:
     num_targets = 0
     # assuming obstacles are in objects_path:
@@ -187,7 +222,11 @@ def main():
     j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
     default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
 
-    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+    robot, robot_prim_path = add_robot_to_scene(
+        robot_cfg, my_world,
+        position=robot_pos,
+        orientation=robot_quat,
+    )
 
     articulation_controller = None
 
@@ -374,8 +413,9 @@ def main():
             and robot_static
         ):
             # Set EE teleop goals, use cube for simple non-vr init:
-            ee_translation_goal = cube_position
-            ee_orientation_teleop_goal = cube_orientation
+            ee_translation_goal, ee_orientation_teleop_goal = _world_to_base_scipy(
+                cube_position, cube_orientation, robot_pos, robot_quat, quat_format='wxyz'
+            )
 
             # compute curobo solution:
             ik_goal = Pose(