autosim-examples & add debug_target_pose mode (#2)

* add debug_target_pose mode in curobo related skill.

* autosim-examples finished.

Author: EverNorif

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__":

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,
         )

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)

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

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,10 +85,23 @@ 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."""
 
     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"])

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]

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,

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])

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]),
                 ],
             },
         )