From 100123d10e9f36c8f8b18ec3a2ea4851fe12ff71 Mon Sep 17 00:00:00 2001 From: Lance Li Date: Mon, 15 Dec 2025 11:47:06 +0800 Subject: [PATCH] Fixes object pose transformation in cuRobo planner collision synchronization --- .../motion_planners/curobo/curobo_planner.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py index f9c6cf69cbd..2c8ae242f96 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py +++ b/source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py @@ -27,6 +27,7 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.sim.spawners.materials import PreviewSurfaceCfg from isaaclab.sim.spawners.meshes import MeshSphereCfg, spawn_mesh_sphere +from isaaclab.utils.math import subtract_frame_transforms from isaaclab_mimic.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg from isaaclab_mimic.motion_planners.motion_planner_base import MotionPlannerBase @@ -627,6 +628,14 @@ def _sync_object_poses_with_isaaclab(self) -> None: current_pos_raw = obj.data.root_pos_w[self.env_id] - env_origin current_quat_raw = obj.data.root_quat_w[self.env_id] # (w, x, y, z) + # Transform object pose from world frame to robot base frame + robot_base_pos = self.robot.data.root_pos_w[self.env_id, :3] + robot_base_quat = self.robot.data.root_quat_w[self.env_id] # [w, x, y, z] + + current_pos_raw, current_quat_raw = subtract_frame_transforms( + robot_base_pos, robot_base_quat, current_pos_raw, current_quat_raw + ) + # Convert to cuRobo device and extract float values for pose list current_pos = self._to_curobo_device(current_pos_raw) current_quat = self._to_curobo_device(current_quat_raw)