Skip to content

Fixes collision synchronization in cuRobo planner #4212

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Nyquist0 wants to merge 1 commit into
base: main
Choose a base branch
from
+9 −0
Open

Fixes collision synchronization in cuRobo planner #4212

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions source/isaaclab_mimic/isaaclab_mimic/motion_planners/curobo/curobo_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
Loading