task decompose enhance & return link poses in robot root frame in retract skill.

examples/visualization/curobo_collision_spheres.py

@@ -35,6 +35,7 @@
 from __future__ import annotations
 
 import argparse
+import traceback
 
 import numpy as np
 import torch
@@ -405,43 +406,49 @@ def main():
     pipeline.reset_env()
     _update_visualization(pipeline, env_id, vm_spheres, vm_ee, unique_radii)
 
-    for subtask in decompose_result.subtasks:
-        for skill_info in subtask.skills:
-            skill = SkillRegistry.create(
-                skill_info.skill_type, pipeline.cfg.skills.get(skill_info.skill_type).extra_cfg
-            )
-
-            if pipeline._action_adapter.should_skip_apply(skill):
-                pipeline._logger.info(f"Skill {skill_info.skill_type} skipped.")
-                continue
-
-            goal = skill.extract_goal_from_info(skill_info, pipeline._env, pipeline._env_extra_info)
-            success, steps, episode_done = _execute_single_skill_with_viz(
-                pipeline,
-                skill,
-                goal,
-                vm_spheres,
-                vm_ee,
-                unique_radii,
-                env_id,
-                curobo_link_name=args_cli.curobo_link_name,
-                isaaclab_link_name=args_cli.isaaclab_link_name,
-            )
-
-            if not success:
-                pipeline._logger.error(f"Skill {skill_info.skill_type} failed after {steps} steps.")
-                raise ValueError(f"Skill {skill_info.skill_type} failed after {steps} steps.")
-            if episode_done:
-                pipeline._logger.info(f"Episode completed during skill {skill_info.skill_type}.({steps} steps)")
-                pipeline._logger.info("Pipeline execution completed.")
-                while simulation_app.is_running():
-                    pipeline._env.sim.render()
-                return
-            pipeline._logger.info(f"Skill {skill_info.skill_type} done ({steps} steps).")
-
-        pipeline._logger.info(f"Subtask {subtask.subtask_name} completed.")
-
-    pipeline._logger.info("Pipeline execution completed.")
+    try:
+        for subtask in decompose_result.subtasks:
+            for skill_info in subtask.skills:
+                skill = SkillRegistry.create(
+                    skill_info.skill_type, pipeline.cfg.skills.get(skill_info.skill_type).extra_cfg
+                )
+
+                if pipeline._action_adapter.should_skip_apply(skill):
+                    pipeline._logger.info(f"Skill {skill_info.skill_type} skipped.")
+                    continue
+
+                goal = skill.extract_goal_from_info(skill_info, pipeline._env, pipeline._env_extra_info)
+                success, steps, episode_done = _execute_single_skill_with_viz(
+                    pipeline,
+                    skill,
+                    goal,
+                    vm_spheres,
+                    vm_ee,
+                    unique_radii,
+                    env_id,
+                    curobo_link_name=args_cli.curobo_link_name,
+                    isaaclab_link_name=args_cli.isaaclab_link_name,
+                )
+
+                if not success:
+                    pipeline._logger.error(f"Skill {skill_info.skill_type} failed after {steps} steps.")
+                    raise ValueError(f"Skill {skill_info.skill_type} failed after {steps} steps.")
+                if episode_done:
+                    pipeline._logger.info(f"Episode completed during skill {skill_info.skill_type}.({steps} steps)")
+                    pipeline._logger.info("Pipeline execution completed.")
+                    while simulation_app.is_running():
+                        pipeline._env.sim.render()
+                    return
+                pipeline._logger.info(f"Skill {skill_info.skill_type} done ({steps} steps).")
+
+            pipeline._logger.info(f"Subtask {subtask.subtask_name} completed.")
+
+        pipeline._logger.info("Pipeline execution completed.")
+    except Exception as e:
+        print(f"Error: {e}")
+        print(traceback.format_exc())
+        while simulation_app.is_running():
+            pipeline._env.sim.render()
 
     while simulation_app.is_running():
         pipeline._env.sim.render()

source/autosim/autosim/core/types.py

@@ -190,7 +190,7 @@ class SkillInfo:
     skill_type: str
     """The type of the skill, must be one of the atomic skills"""
     target_object: str
-    """The target object of the skill."""
+    """The target object of the skill. "none" if the skill has no target object."""
     target_type: str
     """The type of the target. "object", "fixture", "interactive_element", or "position"."""
     description: str

source/autosim/autosim/decomposers/llm_decomposer/llm_decomposer.py

@@ -247,8 +247,8 @@ def _validate_result(self, result: dict, valid_objects: set | None = None) -> No
                     raise ValueError(f"Invalid skill type: {skill['skill_type']}. Must be one of {self._atomic_skills}")
                 if valid_objects is not None:
                     target = skill.get("target_object", "")
-                    if target and target not in valid_objects:
+                    if target and target not in valid_objects and target.lower() != "none":
                         raise ValueError(
                             f"Invalid target_object '{target}' for skill '{skill['skill_type']}'. "
-                            f"Must be one of: {sorted(valid_objects)}"
+                            f"Must be one of: {sorted(valid_objects)} or 'none'"
                         )

source/autosim/autosim/decomposers/llm_decomposer/prompts/task_decompose.jinja

@@ -62,11 +62,13 @@ Output the task decomposition result in JSON format with the following fields:
     ```
     moveto(object) → reach(object) → grasp(object) → lift(object)
     ```
+    You should set specific target_object for each skill.
 
 2. **Pick-and-Place Pattern** (for moving objects to specific location):
     ```
-    moveto(object) → reach(object) → grasp(object) → lift(object) → [moveto(target)](optional) → reach(target) → ungrasp(object)
+    moveto(object) → reach(object) → grasp(object) → lift(object) → [moveto(target)](optional) → reach(target) → ungrasp(object) → [retract(none)](optional)
     ```
+    You should set specific target_object for each skill, except for the retract skill.
 
 3. **Press Pattern** (for pressing buttons):
     ```

source/autosim/autosim/skills/base_skill.py

@@ -123,3 +123,42 @@ def visualize_debug_target_pose(self):
 
         if self.cfg.extra_cfg.debug_target_pose:
             visualize_marker("target_pose", self._target_poses["target_pose"])
+
+    def _build_activate_joint_state(
+        self, full_sim_joint_names: list[str], full_sim_q: torch.Tensor, full_sim_qd: torch.Tensor | None = None
+    ) -> tuple[torch.Tensor, torch.Tensor | None]:
+        """Extract the planner's active joint state from full simulation joint state.
+
+        cuRobo typically plans over a subset of "active joints" (`self._planner.target_joint_names`).
+        This helper slices the full simulation joint vectors into that active subset, ordered exactly
+        as the planner expects, returning `q` and (optionally) `qd`.
+
+        Args:
+            full_sim_joint_names: Joint name list from simulation (index-aligned with `full_sim_q`/`full_sim_qd`).
+            full_sim_q: Full simulation joint positions, shape `[num_sim_joints]`.
+            full_sim_qd: Optional full simulation joint velocities, shape `[num_sim_joints]`.
+
+        Returns:
+            A tuple `(activate_q, activate_qd)` where:
+            - `activate_q` is ordered by `self._planner.target_joint_names`, shape `[num_active_joints]`.
+            - `activate_qd` is the corresponding velocities if `full_sim_qd` is provided; otherwise `None`.
+
+        Raises:
+            ValueError: If any planner target joint is missing from `full_sim_joint_names`.
+        """
+
+        activate_q, activate_qd = [], [] if full_sim_qd is not None else None
+        for joint_name in self._planner.target_joint_names:
+            if joint_name not in full_sim_joint_names:
+                raise ValueError(
+                    f"Joint {joint_name} in planner activate joints is not in the full simulation joint names."
+                )
+            sim_joint_idx = full_sim_joint_names.index(joint_name)
+            activate_q.append(full_sim_q[sim_joint_idx])
+            if full_sim_qd is not None and activate_qd is not None:
+                activate_qd.append(full_sim_qd[sim_joint_idx])
+
+        activate_q_tensor = torch.stack(activate_q, dim=0)
+        if activate_qd is None:
+            return activate_q_tensor, None
+        return activate_q_tensor, torch.stack(activate_qd, dim=0)

source/autosim/autosim/skills/reach.py

@@ -58,45 +58,6 @@ def __init__(self, extra_cfg: ReachSkillExtraCfg) -> None:
         self._saved_reach_offset = None
         self._saved_env_extra_info = None
 
-    def _build_activate_joint_state(
-        self, full_sim_joint_names: list[str], full_sim_q: torch.Tensor, full_sim_qd: torch.Tensor | None = None
-    ) -> tuple[torch.Tensor, torch.Tensor | None]:
-        """Extract the planner's active joint state from full simulation joint state.
-
-        cuRobo typically plans over a subset of "active joints" (`self._planner.target_joint_names`).
-        This helper slices the full simulation joint vectors into that active subset, ordered exactly
-        as the planner expects, returning `q` and (optionally) `qd`.
-
-        Args:
-            full_sim_joint_names: Joint name list from simulation (index-aligned with `full_sim_q`/`full_sim_qd`).
-            full_sim_q: Full simulation joint positions, shape `[num_sim_joints]`.
-            full_sim_qd: Optional full simulation joint velocities, shape `[num_sim_joints]`.
-
-        Returns:
-            A tuple `(activate_q, activate_qd)` where:
-            - `activate_q` is ordered by `self._planner.target_joint_names`, shape `[num_active_joints]`.
-            - `activate_qd` is the corresponding velocities if `full_sim_qd` is provided; otherwise `None`.
-
-        Raises:
-            ValueError: If any planner target joint is missing from `full_sim_joint_names`.
-        """
-
-        activate_q, activate_qd = [], [] if full_sim_qd is not None else None
-        for joint_name in self._planner.target_joint_names:
-            if joint_name not in full_sim_joint_names:
-                raise ValueError(
-                    f"Joint {joint_name} in planner activate joints is not in the full simulation joint names."
-                )
-            sim_joint_idx = full_sim_joint_names.index(joint_name)
-            activate_q.append(full_sim_q[sim_joint_idx])
-            if full_sim_qd is not None and activate_qd is not None:
-                activate_qd.append(full_sim_qd[sim_joint_idx])
-
-        activate_q_tensor = torch.stack(activate_q, dim=0)
-        if activate_qd is None:
-            return activate_q_tensor, None
-        return activate_q_tensor, torch.stack(activate_qd, dim=0)
-
     def _get_current_primary_and_extra_link_poses(
         self, activate_q: torch.Tensor
     ) -> tuple[tuple[torch.Tensor, torch.Tensor], dict[str, tuple[torch.Tensor, torch.Tensor]]]:
@@ -286,6 +247,7 @@ def _compute_goal_from_offset(
         self._logger.debug(f"Reach target position in environment: {reach_target_pos_in_env}")
         self._logger.debug(f"Reach target quaternion in environment: {reach_target_quat_in_env}")
         self._target_poses["target_pose"] = torch.cat((reach_target_pos_in_env, reach_target_quat_in_env), dim=-1)
+        self.visualize_debug_target_pose()
 
         robot = env.scene[env_extra_info.robot_name]
         robot_root_pos_in_env = robot.data.root_pose_w[:, :3]

source/autosim/autosim/skills/retract.py

@@ -128,11 +128,20 @@ def step(self, state: WorldState) -> SkillOutput:
             sim_idx = sim_joint_names.index(curobo_joint_name)
             joint_pos[sim_idx] = traj_pos[curobo_idx]
 
+        info = {}
+        if self.cfg.extra_cfg.return_link_poses_in_robot_root_frame:
+            activate_q, _ = self._build_activate_joint_state(state.sim_joint_names, joint_pos, None)
+            all_link_poses = self._planner.get_link_poses(activate_q, link_names=None)
+            info["link_poses_in_robot_root_frame"] = {
+                name: torch.cat([pose.position.squeeze(0), pose.quaternion.squeeze(0)])
+                for name, pose in all_link_poses.items()
+            }
+
         return SkillOutput(
             action=joint_pos,
             done=done,
             success=True,
-            info={},
+            info=info,
         )
 
     def reset(self) -> None: