diff --git a/examples/run_autosim_example.py b/examples/run_autosim_example.py index 18c7e25..6b56d4f 100644 --- a/examples/run_autosim_example.py +++ b/examples/run_autosim_example.py @@ -28,8 +28,7 @@ def main(): pipeline = make_pipeline(args_cli.pipeline_id) - result = pipeline.run() - print(result) + pipeline.run() if __name__ == "__main__": diff --git a/source/autosim/autosim/core/pipeline.py b/source/autosim/autosim/core/pipeline.py index 497a85d..7cd6f88 100644 --- a/source/autosim/autosim/core/pipeline.py +++ b/source/autosim/autosim/core/pipeline.py @@ -216,6 +216,8 @@ def _build_world_state(self) -> WorldState: yaw = torch.atan2(sin_yaw, cos_yaw) robot_base_pose = torch.stack((robot_base_pose[0], robot_base_pose[1], yaw)) # [x, y, yaw] + robot_root_pose = self._robot.data.root_pose_w[self._env_id] + sim_joint_names = self._robot.data.joint_names objects_dict = dict() @@ -229,6 +231,7 @@ def _build_world_state(self) -> WorldState: robot_joint_vel=robot_joint_vel, robot_ee_pose=robot_ee_pose, robot_base_pose=robot_base_pose, + robot_root_pose=robot_root_pose, sim_joint_names=sim_joint_names, objects=objects_dict, ) diff --git a/source/autosim/autosim/core/types.py b/source/autosim/autosim/core/types.py index 824a6fb..9b3567c 100644 --- a/source/autosim/autosim/core/types.py +++ b/source/autosim/autosim/core/types.py @@ -123,6 +123,8 @@ class WorldState: """The end - effector pose of the robot in the world frame. [x, y, z, qw, qx, qy, qz]""" robot_base_pose: torch.Tensor """The base pose of the robot in the world frame. [x, y, yaw]""" + robot_root_pose: torch.Tensor + """The root pose of the robot in the world frame. [x, y, z, qw, qx, qy, qz]""" sim_joint_names: list[str] """The joint names of the robot.""" objects: dict[str, torch.Tensor] = field(default_factory=dict) diff --git a/source/autosim/autosim/skills/__init__.py b/source/autosim/autosim/skills/__init__.py index b393919..cb246da 100644 --- a/source/autosim/autosim/skills/__init__.py +++ b/source/autosim/autosim/skills/__init__.py @@ -28,4 +28,13 @@ class AutoSimSkillsExtraCfg: def get(cls, skill_name: str): """Get the skill configuration by name.""" + return getattr(cls, skill_name) + + def debug_target_pose(self): + """Debug the target pose of the skills.""" + + self.lift.extra_cfg.debug_target_pose = True + self.pull.extra_cfg.debug_target_pose = True + self.push.extra_cfg.debug_target_pose = True + self.reach.extra_cfg.debug_target_pose = True diff --git a/source/autosim/autosim/skills/base_skill.py b/source/autosim/autosim/skills/base_skill.py index 2fa7653..dcbfdf2 100644 --- a/source/autosim/autosim/skills/base_skill.py +++ b/source/autosim/autosim/skills/base_skill.py @@ -11,6 +11,7 @@ SkillOutput, WorldState, ) +from autosim.utils.debug_util import create_marker, visualize_marker @configclass @@ -84,6 +85,9 @@ class CuroboSkillExtraCfg(SkillExtraCfg): curobo_planner: CuroboPlanner | None = None """The curobo planner for the skill.""" + debug_target_pose: bool = False + """Whether to debug the target pose.""" + class CuroboSkillBase(Skill): """Base class for skills dependent on curobo.""" @@ -91,3 +95,13 @@ class CuroboSkillBase(Skill): def __init__(self, extra_cfg: CuroboSkillExtraCfg) -> None: super().__init__(extra_cfg) self._planner = extra_cfg.curobo_planner + + self._target_poses = dict() + if self.cfg.extra_cfg.debug_target_pose: + create_marker("target_pose") + + def visualize_debug_target_pose(self): + """Visualize the debug target pose.""" + + if self.cfg.extra_cfg.debug_target_pose: + visualize_marker("target_pose", self._target_poses["target_pose"]) diff --git a/source/autosim/autosim/skills/reach.py b/source/autosim/autosim/skills/reach.py index 3c260bd..4fb61de 100644 --- a/source/autosim/autosim/skills/reach.py +++ b/source/autosim/autosim/skills/reach.py @@ -56,6 +56,7 @@ def extract_goal_from_info( object_pos_in_env, object_quat_in_env = object_pose_in_env[:, :3], object_pose_in_env[:, 3:] reach_target_pose = env_extra_info.get_next_reach_target_pose(target_object) + reach_target_pose = reach_target_pose.to(env.device) reach_target_pose_in_object = reach_target_pose.unsqueeze(0) reach_target_pos_in_object, reach_target_quat_in_object = ( reach_target_pose_in_object[:, :3], @@ -67,6 +68,7 @@ def extract_goal_from_info( ) self._logger.info(f"Reach target position in environment: {reach_target_pos_in_env}") self._logger.info(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) robot_root_pose_in_env = robot.data.root_pose_w robot_root_pos_in_env, robot_root_quat_in_env = robot_root_pose_in_env[:, :3], robot_root_pose_in_env[:, 3:] @@ -146,6 +148,8 @@ def step(self, state: WorldState) -> SkillOutput: action: The action to be applied to the environment. [joint_positions with isaaclab joint order] """ + self.visualize_debug_target_pose() + traj_positions = self._trajectory.position if self._step_idx >= len(self._trajectory.position): traj_pos = traj_positions[-1] diff --git a/source/autosim/autosim/skills/relative_reach.py b/source/autosim/autosim/skills/relative_reach.py index f05493d..99caf3a 100644 --- a/source/autosim/autosim/skills/relative_reach.py +++ b/source/autosim/autosim/skills/relative_reach.py @@ -106,6 +106,19 @@ def execute_plan(self, state: WorldState, goal: SkillGoal) -> bool: f"target pos in robot root frame: {target_pos}, target quat in robot root frame: {target_quat}" ) + reach_target_pos_in_robot_root = offset_pos_in_robot_root.clone() + reach_target_quat_in_robot_root = offset_quat_in_robot_root.clone() + robot_root_pos_in_env, robot_root_quat_in_env = state.robot_root_pose[None, :3], state.robot_root_pose[None, 3:] + reach_target_pos_in_env, reach_target_quat_in_env = PoseUtils.combine_frame_transforms( + robot_root_pos_in_env, + robot_root_quat_in_env, + reach_target_pos_in_robot_root, + reach_target_quat_in_robot_root, + ) + self._logger.info(f"reach target pos in environment: {reach_target_pos_in_env}") + self._logger.info(f"reach target quat 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._trajectory = self._planner.plan_motion( target_pos, target_quat, diff --git a/source/autosim/autosim/utils/debug_util.py b/source/autosim/autosim/utils/debug_util.py index 31c2191..f628a63 100644 --- a/source/autosim/autosim/utils/debug_util.py +++ b/source/autosim/autosim/utils/debug_util.py @@ -2,16 +2,19 @@ from isaaclab.markers import VisualizationMarkers from isaaclab.markers.config import FRAME_MARKER_CFG -markers: list[VisualizationMarkers] = [] +markers: dict[str, VisualizationMarkers] = {} -def create_replay_marker(marker_id: str | int = 0): +def create_marker(marker_name: str): + if marker_name in markers.keys(): + return frame_marker_cfg = FRAME_MARKER_CFG.copy() frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) - marker_cfg = frame_marker_cfg.replace(prim_path=f"/World/Visuals/replay_marker_{marker_id}") + marker_cfg = frame_marker_cfg.replace(prim_path=f"/World/Visuals/replay_marker_{marker_name}") marker = VisualizationMarkers(marker_cfg) - markers.append(marker) + markers[marker_name] = marker -def marker_visualize(marker_idx: int, pos: torch.Tensor, quat: torch.Tensor): - markers[marker_idx].visualize(translations=pos, orientations=quat, marker_indices=[0]) +def visualize_marker(marker_name: str, pose: torch.Tensor): + pos, quat = pose[:, :3], pose[:, 3:] + markers[marker_name].visualize(translations=pos, orientations=quat, marker_indices=[0]) diff --git a/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py b/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py index 683d39b..db4967a 100644 --- a/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py +++ b/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py @@ -18,7 +18,8 @@ class FrankaCubeLiftPipelineCfg(AutoSimPipelineCfg): action_adapter: FrankaAbsAdapterCfg = FrankaAbsAdapterCfg() def __post_init__(self): - self.skills.lift.extra_cfg.lift_offset = 0.20 + self.skills.lift.extra_cfg.move_axis = "-z" + self.skills.lift.extra_cfg.lift_offset = 0.30 self.occupancy_map.floor_prim_suffix = "Table" @@ -38,7 +39,8 @@ def load_env(self) -> ManagerBasedEnv: from isaaclab_tasks.utils import parse_env_cfg env_cfg = parse_env_cfg(self._task_name, device="cuda:0", num_envs=1, use_fabric=True) - env_cfg.terminations.time_out = None + env_cfg.terminations = None + env_cfg.scene.robot.spawn.rigid_props.disable_gravity = True env = gym.make(self._task_name, cfg=env_cfg).unwrapped return env @@ -54,8 +56,7 @@ def get_env_extra_info(self) -> EnvExtraInfo: ee_link_name="panda_hand", object_reach_target_poses={ "cube": [ - # torch.tensor([0.0, 0.0, 0.1, 0.0, 1.0, 0.0, 0.0]), - torch.tensor([0.0, 0.0, 0.15, 0.0, 1.0, 0.0, 0.0]), + torch.tensor([0.0, 0.0, 0.10, 0.0, 1.0, 0.0, 0.0]), ], }, )