Skip to content

fix obj_prim_prefix using in curobo_planner. #27

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

Merged
EverNorif merged 1 commit into from
May 25, 2026
Merged

fix obj_prim_prefix using in curobo_planner. #27

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
13 changes: 8 additions & 5 deletions source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
from isaaclab.assets import Articulation
from isaaclab.envs import ManagerBasedEnv
from isaaclab.utils.math import quat_apply, quat_mul, subtract_frame_transforms
from pxr import UsdGeom
from pxr import UsdGeom, UsdPhysics

from autosim.core.logger import AutoSimLogger

Expand Down Expand Up @@ -344,6 +344,9 @@ def _get_articulated_link_poses_from_usd(
if not obj_prim.IsValid():
self._logger.warning(f"Articulated object prim not found at {obj_path}")
continue
if not UsdPhysics.ArticulationRootAPI(obj_prim):
self._logger.warning(f"Prim at {obj_path} is not an articulated object, skipping")
continue

link_poses = {}
for child_prim in obj_prim.GetChildren():
Expand Down Expand Up @@ -430,15 +433,15 @@ def _build_articulated_primitive_offsets(self) -> None:
articulated_link_poses = self._get_articulated_link_poses_from_usd()

for obj_name, link_poses in articulated_link_poses.items():
obj_prim_prefix = f"{self._env_scene_prefix}/{obj_name}"
obj_prim_prefix = f"{self._env_scene_prefix}/{obj_name}/"

# Collect all leaf primitives for this articulated object
all_primitives = self._collect_primitives_by_prefix(obj_prim_prefix)

# Group primitives by link name
link_children: dict[str, list[str]] = {}
for prim_name in all_primitives:
relative_path = prim_name.split(f"{obj_prim_prefix}/")[1]
relative_path = prim_name.split(obj_prim_prefix)[1]
link_name = relative_path.split("/")[0]
if link_name not in link_children:
link_children[link_name] = []
Expand Down Expand Up @@ -495,7 +498,7 @@ def _sync_articulated_obstacles(self) -> None:
updated_count = 0

for obj_name, link_poses in articulated_link_poses.items():
obj_prim_prefix = f"{self._env_scene_prefix}/{obj_name}"
obj_prim_prefix = f"{self._env_scene_prefix}/{obj_name}/"

# Update each primitive using cached offset
for primitive_name, (link_name, offset_pos, offset_quat) in self._articulated_primitive_offsets.items():
Expand Down Expand Up @@ -544,7 +547,7 @@ def _get_object_mappings(self) -> dict[str, list[str]]:
# Map each rigid object to its primitives
mappings: dict[str, list[str]] = {}
for object_name in rigid_objects.keys():
object_prefix = f"{self._env_scene_prefix}/{object_name}"
object_prefix = f"{self._env_scene_prefix}/{object_name}/"
mappings[object_name] = [p for p in all_scene_primitives if p.startswith(object_prefix)]

self._cached_object_mappings = mappings
Expand Down
Loading