diff --git a/examples/visualization/curobo_collision_spheres.py b/examples/visualization/curobo_collision_spheres.py index 8f79ab5..0e90363 100644 --- a/examples/visualization/curobo_collision_spheres.py +++ b/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() diff --git a/source/autosim/autosim/core/types.py b/source/autosim/autosim/core/types.py index 5095aed..2f3b3bf 100644 --- a/source/autosim/autosim/core/types.py +++ b/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 diff --git a/source/autosim/autosim/decomposers/llm_decomposer/llm_decomposer.py b/source/autosim/autosim/decomposers/llm_decomposer/llm_decomposer.py index 94d0aed..b70e91d 100644 --- a/source/autosim/autosim/decomposers/llm_decomposer/llm_decomposer.py +++ b/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'" ) diff --git a/source/autosim/autosim/decomposers/llm_decomposer/prompts/task_decompose.jinja b/source/autosim/autosim/decomposers/llm_decomposer/prompts/task_decompose.jinja index e1ec1ee..9d43d1d 100644 --- a/source/autosim/autosim/decomposers/llm_decomposer/prompts/task_decompose.jinja +++ b/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): ``` diff --git a/source/autosim/autosim/skills/base_skill.py b/source/autosim/autosim/skills/base_skill.py index 0910149..811a19d 100644 --- a/source/autosim/autosim/skills/base_skill.py +++ b/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) diff --git a/source/autosim/autosim/skills/reach.py b/source/autosim/autosim/skills/reach.py index 1eb548b..349d9ab 100644 --- a/source/autosim/autosim/skills/reach.py +++ b/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] diff --git a/source/autosim/autosim/skills/retract.py b/source/autosim/autosim/skills/retract.py index 6aa080f..4650c96 100644 --- a/source/autosim/autosim/skills/retract.py +++ b/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: