From 24d510744febf56364ce3901fae290d5bbfeb800 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 11 Mar 2025 16:07:27 -0400 Subject: [PATCH 01/50] add murp --- examples/interactive_play.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 8d215ac425..e13139484e 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -80,7 +80,7 @@ # Please reach out to the paper authors to obtain this file DEFAULT_POSE_PATH = "data/humanoids/humanoid_data/walking_motion_processed.pkl" -DEFAULT_CFG = "benchmark/rearrange/play/play.yaml" +DEFAULT_CFG = "benchmark/rearrange/play/play_murp.yaml" DEFAULT_RENDER_STEPS_LIMIT = 60 SAVE_VIDEO_DIR = "./data/vids" SAVE_ACTIONS_DIR = "./data/interactive_play_replays" From 797c765e249b3a1d85d0feb1d6a99fd5dfe875dc Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 11 Mar 2025 16:07:51 -0400 Subject: [PATCH 02/50] add murp yaml --- .../benchmark/rearrange/play/play_murp.yaml | 20 +++++++++++++++++++ .../config/habitat/simulator/agents/murp.yaml | 9 +++++++++ 2 files changed, 29 insertions(+) create mode 100644 habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml create mode 100644 habitat-lab/habitat/config/habitat/simulator/agents/murp.yaml diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml new file mode 100644 index 0000000000..2842fdf1ec --- /dev/null +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -0,0 +1,20 @@ +# @package _global_ +defaults: + - play + - /habitat/task/lab_sensors: + - arm_depth_bbox_sensor + - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: spot_agent + - override /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp + - override /habitat/task/rearrange/actions: spot_base_arm_empty + - _self_ + +habitat: + task: + lab_sensors: + arm_depth_bbox_sensor: + height: 240 + width: 228 + actions: + arm_action: + center_cone_vector: [0.0, 1.0, 0.0] + auto_grasp: True diff --git a/habitat-lab/habitat/config/habitat/simulator/agents/murp.yaml b/habitat-lab/habitat/config/habitat/simulator/agents/murp.yaml new file mode 100644 index 0000000000..88b64dfb69 --- /dev/null +++ b/habitat-lab/habitat/config/habitat/simulator/agents/murp.yaml @@ -0,0 +1,9 @@ +# @package habitat.simulator.agents.spot +defaults: + - agent_base + - _self_ + +radius: 0.25 +height: 1.41 +articulated_agent_type: "MurpRobot" +articulated_agent_urdf: data/robots/hab_murp/murp_tmr_franka/murp_tmr_franka_metahand From cd487a309d295e6295a1f21f9e975eec54c2f989 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 11 Mar 2025 17:32:44 -0400 Subject: [PATCH 03/50] able to load murp in the interactive --- examples/interactive_play.py | 41 ++++++++++++++++--- .../articulated_agents/robots/murp_robot.py | 12 +++--- .../benchmark/rearrange/play/play_murp.yaml | 2 +- .../tasks/rearrange/actions/grip_actions.py | 12 ++++++ .../rearrange/articulated_agent_manager.py | 12 +++--- 5 files changed, 61 insertions(+), 18 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index e13139484e..d2b44c3e71 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -114,7 +114,7 @@ def get_input_vel_ctlr( base_action_name = f"{agent_k}humanoidjoint_action" base_key = "human_joints_trans" else: - if "spot" in cfg: + if "spot" in cfg or "murp" in cfg: base_action_name = f"{agent_k}base_velocity_non_cylinder" else: base_action_name = f"{agent_k}base_velocity" @@ -127,7 +127,7 @@ def get_input_vel_ctlr( arm_key ] arm_ctrlr = env.task.actions[arm_action_name].arm_ctrlr - base_action = None + base_action = [0, 0] elif "stretch" in cfg: arm_action_space = np.zeros(10) arm_ctrlr = None @@ -267,6 +267,33 @@ def get_input_vel_ctlr( elif keys[pygame.K_7]: arm_action[9] = -1.0 + elif arm_action_space.shape[0] == 5: + # Velocity control. A different key for each joint + if keys[pygame.K_q]: + arm_action[0] = 1.0 + elif keys[pygame.K_1]: + arm_action[0] = -1.0 + + elif keys[pygame.K_w]: + arm_action[1] = 1.0 + elif keys[pygame.K_2]: + arm_action[1] = -1.0 + + elif keys[pygame.K_e]: + arm_action[2] = 1.0 + elif keys[pygame.K_3]: + arm_action[2] = -1.0 + + elif keys[pygame.K_r]: + arm_action[3] = 1.0 + elif keys[pygame.K_4]: + arm_action[3] = -1.0 + + elif keys[pygame.K_t]: + arm_action[4] = 1.0 + elif keys[pygame.K_5]: + arm_action[4] = -1.0 + elif isinstance(arm_ctrlr, ArmEEAction): EE_FACTOR = 0.5 # End effector control @@ -371,9 +398,13 @@ def get_input_vel_ctlr( grip_key: arm_action[-1], } else: - args = {arm_key: arm_action, grip_key: magic_grasp} - - if magic_grasp is None: + if "murp" in cfg: + args = {arm_key: arm_action} + else: + args = {arm_key: arm_action, grip_key: magic_grasp} + if "murp" in cfg: + arm_action = [*arm_action] + elif magic_grasp is None: arm_action = [*arm_action, 0.0] else: arm_action = [*arm_action, magic_grasp] diff --git a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py index dfa70e7543..c00743ab55 100644 --- a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -79,15 +79,15 @@ class MurpRobot(MobileManipulator): @classmethod def _get_murp_params(cls): return MurpParams( - arm_joints=[0, 2, 4, 6, 8, 10, 12], + arm_joints=[2, 4, 6, 8, 12], # remove 0, 10 gripper_joints=[19], arm_init_params=[ - 2.6116285, + # 2.6116285, for 0 1.5283098, 1.0930868, -0.50559217, 0.48147443, - 2.628784, + # 2.628784, for 10 -1.3962275, ], gripper_init_params=[-1.56], @@ -153,9 +153,9 @@ def _get_murp_params(cls): @property def base_transformation(self): - add_rot = mn.Matrix4.rotation( - mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0) - ) + # add_rot = mn.Matrix4.rotation( + # mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0) + # ) return self.sim_obj.transformation # @ add_rot def __init__( diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml index 2842fdf1ec..e57d1f305d 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -5,7 +5,7 @@ defaults: - arm_depth_bbox_sensor - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: spot_agent - override /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp - - override /habitat/task/rearrange/actions: spot_base_arm_empty + - override /habitat/task/rearrange/actions: murp_base_arm_empty - _self_ habitat: diff --git a/habitat-lab/habitat/tasks/rearrange/actions/grip_actions.py b/habitat-lab/habitat/tasks/rearrange/actions/grip_actions.py index 0417215b9f..1e25ef3645 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/grip_actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/grip_actions.py @@ -10,6 +10,7 @@ import numpy as np from gym import spaces +from habitat.articulated_agents.robots.murp_robot import MurpRobot from habitat.articulated_agents.robots.spot_robot import SpotRobot from habitat.articulated_agents.robots.stretch_robot import StretchRobot from habitat.core.registry import registry @@ -223,6 +224,13 @@ def _determine_center_object(self): .sensor_states["head_rgb"] .position ) + elif isinstance(self.cur_articulated_agent, MurpRobot): + cam_pos = ( + self._sim.agents[0] + .get_state() + .sensor_states["articulated_agent_arm_rgb"] + .position + ) else: raise NotImplementedError( "This robot does not have GazeGraspAction." @@ -236,6 +244,10 @@ def _determine_center_object(self): panoptic_img = self._sim._sensor_suite.get_observations( self._sim.get_sensor_observations() )["head_panoptic"] + elif isinstance(self.cur_articulated_agent, MurpRobot): + panoptic_img = self._sim._sensor_suite.get_observations( + self._sim.get_sensor_observations() + )["articulated_agent_arm_panoptic"] else: raise NotImplementedError( "This robot does not have GazeGraspAction." diff --git a/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py b/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py index 5f1147e99f..46fc9dc2b3 100644 --- a/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py +++ b/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py @@ -4,7 +4,7 @@ import importlib from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, Iterator, List, Optional +from typing import TYPE_CHECKING, Any, Dict, Iterator, List, Optional import magnum as mn import numpy as np @@ -18,6 +18,7 @@ from habitat.articulated_agents.robots import ( FetchRobot, FetchRobotNoWheels, + MurpRobot, SpotRobot, StretchRobot, ) @@ -86,8 +87,8 @@ def __init__(self, cfg: "DictConfig", sim: "Simulator"): self._all_agent_data: List[ArticulatedAgentData] = [] self._is_pb_installed = is_pb_installed() self.agent_names: Dict[str, Any] = cfg.agents - self._agent_index_to_name: Dict[int, str] = {} - self._agent_name_to_index: Dict[str, int] = {} + self._agent_index_to_name: Dict[int, str] = {} # type: ignore + self._agent_name_to_index: Dict[str, int] = {} # type: ignore for agent_index in range(len(cfg.agents_order)): agent_name = cfg.agents_order[agent_index] @@ -320,8 +321,8 @@ def __init__(self, cfg, sim): use_arm_init = np.array(agent.params.arm_init_params) else: use_arm_init = np.array(agent_cfg.joint_start_override) - self._all_agent_data.append( - IsaacAgentData( + self._all_agent_data.append( # type: ignore + IsaacAgentData( # type: ignore articulated_agent=agent, cfg=agent_cfg, start_js=use_arm_init, @@ -336,7 +337,6 @@ def on_new_scene(self): pass def pre_obj_clear(self) -> None: - pass def agent(self): From c84325855869e3dcb1b13735ba3f0003fdfa7f27 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 08:42:30 -0400 Subject: [PATCH 04/50] add murp agent yaml --- .../benchmark/rearrange/play/play_murp.yaml | 2 +- .../simulator/sensor_setups/murp_agent.yaml | 37 +++++++++++++++++++ 2 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml index e57d1f305d..cc7a8f7202 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -3,7 +3,7 @@ defaults: - play - /habitat/task/lab_sensors: - arm_depth_bbox_sensor - - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: spot_agent + - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: murp_agent - override /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp - override /habitat/task/rearrange/actions: murp_base_arm_empty - _self_ diff --git a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml new file mode 100644 index 0000000000..7949624005 --- /dev/null +++ b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml @@ -0,0 +1,37 @@ +# @package habitat.simulator.agents.murp_agent +defaults: + - /habitat/simulator/sim_sensors@sim_sensors.head_rgb_sensor: head_rgb_sensor + - /habitat/simulator/sim_sensors@sim_sensors.head_depth_sensor: head_depth_sensor + - /habitat/simulator/sim_sensors@sim_sensors.arm_rgb_sensor: arm_rgb_sensor # here for cameras + - /habitat/simulator/sim_sensors@sim_sensors.arm_depth_sensor: arm_depth_sensor # here for cameras + - /habitat/simulator/sim_sensors@sim_sensors.arm_panoptic_sensor: arm_panoptic_sensor # here for cameras + - /habitat/simulator/sim_sensors@sim_sensors.head_stereo_left_depth_sensor: head_stereo_left_depth_sensor # here for cameras + - /habitat/simulator/sim_sensors@sim_sensors.head_stereo_right_depth_sensor: head_stereo_right_depth_sensor # here for cameras + +sim_sensors: + arm_rgb_sensor: + height: 480 + width: 640 + hfov: 47 + arm_depth_sensor: + height: 240 + width: 228 + hfov: 60 + min_depth: 0.0 + max_depth: 1.7 + arm_panoptic_sensor: + height: 240 + width: 228 + hfov: 60 + head_stereo_right_depth_sensor: + height: 212 + width: 120 + hfov: 58 + min_depth: 0.0 + max_depth: 3.5 + head_stereo_left_depth_sensor: + height: 212 + width: 120 + hfov: 58 + min_depth: 0.0 + max_depth: 3.5 From 8c2417ce3d867a5fea5242e10346f61fbb3fc9f6 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 08:49:14 -0400 Subject: [PATCH 05/50] motify the arm action and base action --- examples/interactive_play.py | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index d2b44c3e71..b1963e22c2 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -127,15 +127,15 @@ def get_input_vel_ctlr( arm_key ] arm_ctrlr = env.task.actions[arm_action_name].arm_ctrlr - base_action = [0, 0] + base_action = None elif "stretch" in cfg: arm_action_space = np.zeros(10) arm_ctrlr = None - base_action = [0, 0] + base_action = None else: arm_action_space = np.zeros(7) arm_ctrlr = None - base_action = [0, 0] + base_action = None if arm_action is None: arm_action = np.zeros(arm_action_space.shape[0]) @@ -398,13 +398,9 @@ def get_input_vel_ctlr( grip_key: arm_action[-1], } else: - if "murp" in cfg: - args = {arm_key: arm_action} - else: - args = {arm_key: arm_action, grip_key: magic_grasp} - if "murp" in cfg: - arm_action = [*arm_action] - elif magic_grasp is None: + args = {arm_key: arm_action, grip_key: magic_grasp} + + if magic_grasp is None: arm_action = [*arm_action, 0.0] else: arm_action = [*arm_action, magic_grasp] From 80a467f51823a48edabebfaae26f5d2b63d336e0 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 12:05:27 -0400 Subject: [PATCH 06/50] add yaml --- .../actions/murp_base_arm_empty.yaml | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml new file mode 100644 index 0000000000..75011d6a70 --- /dev/null +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml @@ -0,0 +1,28 @@ +# @package habitat.task.actions +defaults: + - /habitat/task/actions: + - base_velocity_non_cylinder + - arm_action + - empty + - _self_ +arm_action: + type: "ArmAction" + arm_controller: "ArmRelPosMaskAction" + grip_controller: "GazeGraspAction" + arm_joint_mask: [1,1,1,1,1] + arm_joint_dimensionality: 5 + grasp_thresh_dist: 0.15 + disable_grip: False + delta_pos_limit: 0.0872665 + ee_ctrl_lim: 0.015 + gaze_distance_range: [0.3, 0.75] + center_cone_angle_threshold: 20.0 +base_velocity_non_cylinder: + allow_dyn_slide: False + # There is a collision if the difference between the clamped NavMesh position and target position + # is more than than collision_threshold for any point + collision_threshold: 1e-5 + # The x and y locations of the clamped NavMesh position + navmesh_offset: [[0.0, 0.0], [0.25, 0.0], [-0.25, 0.0]] + # If we allow the robot to move laterally + enable_lateral_move: False From a42990721a91a8085f576e3fe5b5e40ad00027b1 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 17:38:34 -0400 Subject: [PATCH 07/50] make test run works on the new robot --- .../config/benchmark/rearrange/play/play.yaml | 4 ++-- .../simulator/isaac_rearrange_sim.yaml | 10 ++++++++++ .../datasets/rearrange/rearrange_dataset.py | 2 +- .../isaac_sim/_internal/murp_robot_wrapper.py | 20 ++----------------- .../actions/articulated_agent_action.py | 1 + .../tasks/rearrange/isaac_rearrange_sim.py | 10 ++++------ test_rearrange_env.py | 6 +++--- 7 files changed, 23 insertions(+), 30 deletions(-) create mode 100644 habitat-lab/habitat/config/habitat/simulator/isaac_rearrange_sim.yaml diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml index 99ab8a0724..f52c13ed31 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml @@ -2,7 +2,7 @@ defaults: - /habitat: habitat_config_base - - /habitat/simulator: rearrange_sim + - /habitat/simulator: isaac_rearrange_sim - /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: rgbd_head_rgbd_arm_agent - /habitat/simulator/agents@habitat.simulator.agents.main_agent: fetch_suction @@ -40,7 +40,7 @@ habitat: environment: max_episode_steps: 0 simulator: - type: RearrangeSim-v0 + #type: RearrangeSim-v0 seed: 100 additional_object_paths: - "data/objects/ycb/configs/" diff --git a/habitat-lab/habitat/config/habitat/simulator/isaac_rearrange_sim.yaml b/habitat-lab/habitat/config/habitat/simulator/isaac_rearrange_sim.yaml new file mode 100644 index 0000000000..32d80b7b8c --- /dev/null +++ b/habitat-lab/habitat/config/habitat/simulator/isaac_rearrange_sim.yaml @@ -0,0 +1,10 @@ +# @package habitat.simulator +type: IsaacRearrangeSim-v0 +additional_object_paths: +- data/objects/ycb/configs/ +debug_render_goal: False +concur_render: True +auto_sleep: True +habitat_sim_v0: + allow_sliding: False + enable_physics: True diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_dataset.py b/habitat-lab/habitat/datasets/rearrange/rearrange_dataset.py index 83dd500f2b..fe949991d2 100644 --- a/habitat-lab/habitat/datasets/rearrange/rearrange_dataset.py +++ b/habitat-lab/habitat/datasets/rearrange/rearrange_dataset.py @@ -63,7 +63,7 @@ def to_json(self) -> str: def __init__(self, config: Optional["DictConfig"] = None) -> None: self.config = config - if config and not self.check_config_paths_exist(config): + if config and not self.check_config_paths_exist(config) and False: logger.info( "Rearrange task assets are not downloaded locally, downloading and extracting now..." ) diff --git a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py index 4c5d498bef..1794095a2d 100644 --- a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py +++ b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py @@ -28,9 +28,8 @@ class MurpRobotWrapper: """ def __init__(self, isaac_service, instance_id=0): - self._isaac_service = isaac_service - asset_path = "./data/usd/robots/franka_with_hand_2.usda" #Lambda Machine Change + asset_path = "./data/usd/robots/franka_with_hand_2.usda" # Lambda Machine Change robot_prim_path = f"/World/env_{instance_id}/Murp" self._robot_prim_path = robot_prim_path @@ -48,7 +47,6 @@ def __init__(self, isaac_service, instance_id=0): # Traverse only the robot's prim hierarchy for prim in Usd.PrimRange(robot_prim): - if prim.HasAPI(PhysxSchema.PhysxJointAPI): joint_api = PhysxSchema.PhysxJointAPI(prim) joint_api.GetMaxJointVelocityAttr().Set(200.0) @@ -57,7 +55,6 @@ def __init__(self, isaac_service, instance_id=0): # Access the existing DriveAPI drive_api = UsdPhysics.DriveAPI(prim, "angular") if drive_api: - # Modify drive parameters drive_api.GetStiffnessAttr().Set(10.0) # Position gain drive_api.GetDampingAttr().Set(0.1) # Velocity gain @@ -65,14 +62,12 @@ def __init__(self, isaac_service, instance_id=0): drive_api = UsdPhysics.DriveAPI(prim, "linear") if drive_api: - drive_api = UsdPhysics.DriveAPI.Get(prim, "linear") drive_api.GetStiffnessAttr().Set( 1000 ) # Example for linear stiffness if prim.HasAPI(UsdPhysics.RigidBodyAPI): - # UsdPhysics.RigidBodyAPI doesn't support damping but PhysxRigidBodyAPI does if prim.HasAPI(PhysxSchema.PhysxRigidBodyAPI): physx_api = PhysxSchema.PhysxRigidBodyAPI(prim) @@ -111,7 +106,6 @@ def get_prim_path(self): return self._robot_prim_path def set_root_pose(self, pos, rot, convention="hab"): - rot = [rot.scalar] + list(rot.vector) if convention == "hab": pos = isaac_prim_utils.habitat_to_usd_position(pos) @@ -119,7 +113,6 @@ def set_root_pose(self, pos, rot, convention="hab"): self._robot.set_world_pose(pos, rot) def get_root_pose(self, convention="hab"): - pos_usd, rot_usd = self._robot.get_world_pose() if convention == "hab": pos = mn.Vector3(isaac_prim_utils.usd_to_habitat_position(pos_usd)) @@ -132,7 +125,6 @@ def get_root_pose(self, convention="hab"): return pos, rot def get_link_world_poses(self, convention="hab"): - positions = [] positions_usd, rotations_usd = self._xform_prim_view.get_world_poses() for pos in positions_usd: @@ -152,7 +144,6 @@ def get_link_world_poses(self, convention="hab"): return positions, rotations def _create_xform_prim_view(self): - root_prim_path = self._robot_prim_path root_prim = self._isaac_service.world.stage.GetPrimAtPath( root_prim_path @@ -311,7 +302,6 @@ def post_reset(self): self.reset_hand() def scale_prim_mass_and_inertia(self, path, scale): - prim = self._isaac_service.world.stage.GetPrimAtPath(path) assert prim.HasAPI(UsdPhysics.MassAPI) mass_api = UsdPhysics.MassAPI(prim) @@ -323,7 +313,6 @@ def scale_prim_mass_and_inertia(self, path, scale): def fix_base_orientation_via_angular_vel( self, step_size, base_position, base_orientation ): - curr_angular_velocity = self._robot.get_angular_velocity() # Constants @@ -390,7 +379,6 @@ def fix_base_orientation_via_angular_vel( def fix_base_height_via_linear_vel_z( self, step_size, base_position, base_orientation ): - curr_linear_velocity = self._robot.get_linear_velocity() z_target = 0.7 # todo: get from navmesh or assume ground_z==0 @@ -416,7 +404,6 @@ def fix_base_height_via_linear_vel_z( ) def drive_arm(self, step_size): - if np.array(self._target_arm_joint_positions).any(): assert len(self._target_arm_joint_positions) == len( self._arm_joint_indices @@ -429,7 +416,6 @@ def drive_arm(self, step_size): ) def drive_right_arm(self, step_size): - if np.array(self._target_right_arm_joint_positions).any(): assert len(self._target_right_arm_joint_positions) == len( self._right_arm_joint_indices @@ -442,7 +428,6 @@ def drive_right_arm(self, step_size): ) def drive_hand(self, step_size): - if np.array(self._target_hand_joint_positions).any(): assert len(self._target_hand_joint_positions) == len( self._hand_joint_indices @@ -455,7 +440,6 @@ def drive_hand(self, step_size): ) def drive_right_hand(self, step_size): - if np.array(self._target_right_hand_joint_positions).any(): assert len(self._target_right_hand_joint_positions) == len( self._right_hand_joint_indices @@ -468,7 +452,6 @@ def drive_right_hand(self, step_size): ) def fix_base(self, step_size, base_position, base_orientation): - self.fix_base_height_via_linear_vel_z( step_size, base_position, base_orientation ) @@ -546,6 +529,7 @@ def get_prim_transform(self, asset_path=None): if asset_path is None: asset_path = os.path.abspath( "data/usd/scenes/fremont_static_objects.usda" + # "data/usd/scenes/fremont_static.usda" ) prim_path = f"/World/test_scene/{asset_path}" prim = self._isaac_service.world.stage.GetPrimAtPath(prim_path) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/articulated_agent_action.py b/habitat-lab/habitat/tasks/rearrange/actions/articulated_agent_action.py index 64c9c4818e..21e99d7de2 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/articulated_agent_action.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/articulated_agent_action.py @@ -1,4 +1,5 @@ from habitat.core.embodied_task import SimulatorTaskAction +from habitat.tasks.rearrange.isaac_rearrange_sim import IsaacRearrangeSim from habitat.tasks.rearrange.rearrange_sim import RearrangeSim diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index 3815823ed1..5b044b7aa9 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -72,7 +72,6 @@ def bind_physics_material_to_hierarchy( dynamic_friction, restitution, ): - # material_path = f"/PhysicsMaterials/{material_name}" # material_prim = stage.DefinePrim(material_path, "PhysicsMaterial") # material = UsdPhysics.MaterialAPI(material_prim) @@ -114,9 +113,9 @@ def __init__(self, config: "DictConfig"): sensor_config.uuid = ( f"{agent_name}_{sensor_config.uuid}" ) - agent_cfg.sim_sensors[f"{agent_name}_{sensor_key}"] = ( - sensor_config - ) + agent_cfg.sim_sensors[ + f"{agent_name}_{sensor_key}" + ] = sensor_config super().__init__(config) from habitat.isaac_sim.isaac_app_wrapper import IsaacAppWrapper @@ -136,6 +135,7 @@ def __init__(self, config: "DictConfig"): # asset_path = "data/usd/scenes/102344193_with_stage.usda" asset_path = os.path.abspath( "data/usd/scenes/fremont_static_objects.usda" + # "data/usd/scenes/fremont_static.usda" ) print("asset_path: ", asset_path) from omni.isaac.core.utils.stage import add_reference_to_stage @@ -541,7 +541,6 @@ def set_articulated_agent_base_to_random_point( :returns: The set base position and rotation """ articulated_agent = self.get_agent_data(agent_idx).articulated_agent - for attempt_i in range(max_attempts): # start_pos = self.pathfinder.get_random_navigable_point( # island_index=self._largest_indoor_island_idx @@ -971,7 +970,6 @@ def get_agent_state(self, agent_id: int = 0) -> habitat_sim.AgentState: @add_perf_timing_func() def step(self, action: Union[str, int]) -> Observations: - rom = self.get_rigid_object_manager() self._isaac_wrapper.step(num_steps=1) diff --git a/test_rearrange_env.py b/test_rearrange_env.py index ac516560a1..6a0c06f5b8 100644 --- a/test_rearrange_env.py +++ b/test_rearrange_env.py @@ -42,7 +42,7 @@ from habitat_sim.utils import viz_utils as vut from habitat_sim.utils.settings import make_cfg -data_path = "/fsx-siro/jtruong/repos/vla-physics/habitat-lab/data/" +data_path = "//home/jmmy/research/hab_training/habitat-lab/data/" def make_sim_cfg(agent_dict): @@ -115,10 +115,10 @@ def main(): main_agent_config = AgentConfig() urdf_path = os.path.join( - data_path, "robots/hab_spot_arm/urdf/hab_spot_arm.urdf" + data_path, "robots/hab_murp/murp_tmr_franka/murp_tmr_franka_metahand.urdf" #"robots/hab_spot_arm/urdf/hab_spot_arm.urdf" ) main_agent_config.articulated_agent_urdf = urdf_path - main_agent_config.articulated_agent_type = "SpotRobot" + main_agent_config.articulated_agent_type = "MurpRobot" #"SpotRobot" # main_agent_config.ik_arm_urdf = arm_urdf_path # Define sensors that will be attached to this agent, here a third_rgb sensor and a head_rgb. From c809469b47d95c7e6eec4f2bb3c8c4f98ab77dfc Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 17:56:22 -0400 Subject: [PATCH 08/50] wip for the robot --- habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py | 2 +- habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py index d90afd8ad9..9e68a90b40 100644 --- a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py +++ b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py @@ -119,7 +119,7 @@ def reset(self) -> None: @property def arm_joint_pos(self): - assert False # todo + #assert False # todo pass @arm_joint_pos.setter diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index 5b044b7aa9..dfcc71f8d2 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -101,7 +101,7 @@ def bind_physics_material_to_hierarchy( @registry.register_simulator(name="IsaacRearrangeSim-v0") class IsaacRearrangeSim(HabitatSim): def __init__(self, config: "DictConfig"): - config.scene = "NONE" + #config.scene = "NONE" # cannot do scene none here when regiestering the env for interactive play py if len(config.agents) > 1: with read_write(config): for agent_name, agent_cfg in config.agents.items(): From 97086b8fad21a48f16df73cc80c345df220a18c8 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 18:32:02 -0400 Subject: [PATCH 09/50] wip --- habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py | 2 +- habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py index 9e68a90b40..90fc72d4cf 100644 --- a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py +++ b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py @@ -120,7 +120,7 @@ def reset(self) -> None: @property def arm_joint_pos(self): #assert False # todo - pass + return self._robot_wrapper.arm_joint_pos @arm_joint_pos.setter def arm_joint_pos(self, ctrl: List[float]): diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index 81c232697f..ebc49eb494 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -1323,6 +1323,7 @@ def get_observation(self, observations, episode, task, *args, **kwargs): ) # Check if task has the attribute of the abs_targ_idx + breakpoint() assert hasattr(task, "abs_targ_idx") # Get the target from sim, and ensure that the index is offset From d21875cf6a5a924f0ebbb3a4482b7834fe3636a5 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 12 Mar 2025 18:47:56 -0400 Subject: [PATCH 10/50] wip --- habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py | 4 ++++ habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py | 1 - 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index dfcc71f8d2..baa762d1bd 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -299,6 +299,7 @@ def _get_target_trans(self): """ # Preprocess the ep_info making necessary datatype conversions. target_trans = [] + breakpoint() rom = self.get_rigid_object_manager() for target_handle, trans in self._targets.items(): targ_idx = self._scene_obj_ids.index( @@ -433,6 +434,9 @@ def reconfigure(self, config: "DictConfig", ep_info: RearrangeEpisode): for ao in self.art_objs } + # use target + self._setup_targets(ep_info) + #breakpoint() return if is_hard_reset: with read_write(config): diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index ebc49eb494..81c232697f 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -1323,7 +1323,6 @@ def get_observation(self, observations, episode, task, *args, **kwargs): ) # Check if task has the attribute of the abs_targ_idx - breakpoint() assert hasattr(task, "abs_targ_idx") # Get the target from sim, and ensure that the index is offset From 42327fc7ca5446ada55cbc2267131cc1e9e0f9a4 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Thu, 13 Mar 2025 12:45:07 -0400 Subject: [PATCH 11/50] interactive_play is not working for linux, but it does have the sensors --- examples/interactive_play.py | 5 +- .../articulated_agents/robots/murp_robot.py | 6 +- .../config/benchmark/rearrange/play/play.yaml | 4 +- .../benchmark/rearrange/play/play_murp.yaml | 16 ++--- .../actions/murp_base_arm_empty.yaml | 6 +- .../isaac_sim/isaac_mobile_manipulator.py | 7 ++- .../tasks/rearrange/actions/actions.py | 60 +++++++++---------- .../tasks/rearrange/isaac_rearrange_sim.py | 5 +- 8 files changed, 59 insertions(+), 50 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index b1963e22c2..8d8cb5196a 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -84,7 +84,7 @@ DEFAULT_RENDER_STEPS_LIMIT = 60 SAVE_VIDEO_DIR = "./data/vids" SAVE_ACTIONS_DIR = "./data/interactive_play_replays" - +os.environ["SDL_VIDEODRIVER"] = "x11" #"dummy" # we need this for using pygame in unbuntu def step_env(env, action_name, action_args): return env.step({"action": action_name, "action_args": action_args}) @@ -500,7 +500,6 @@ def play_env(env, args, config): if not args.no_render: draw_obs = observations_to_image(obs, {}) - pygame.init() screen = pygame.display.set_mode( [draw_obs.shape[1], draw_obs.shape[0]] ) @@ -525,6 +524,7 @@ def play_env(env, args, config): humanoid_controller.reset(env._sim.articulated_agent.base_pos) while True: + print("A!") if ( args.save_actions and len(all_arm_actions) > args.save_actions_count @@ -826,5 +826,6 @@ def has_pygame(): if task_config.type == "RearrangePddlTask-v0": task_config.actions["pddl_apply_action"] = PddlApplyActionConfig() + pygame.init() # due to https://github.com/facebookresearch/habitat-lab/issues/1538#issuecomment-1902985545 with habitat.Env(config=config) as env: play_env(env, args, config) diff --git a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py index c00743ab55..24b8656e09 100644 --- a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -79,15 +79,15 @@ class MurpRobot(MobileManipulator): @classmethod def _get_murp_params(cls): return MurpParams( - arm_joints=[2, 4, 6, 8, 12], # remove 0, 10 + arm_joints=[0, 2, 4, 6, 8, 10, 12], # remove 0, 10 gripper_joints=[19], arm_init_params=[ - # 2.6116285, for 0 + 2.6116285, # for 0 1.5283098, 1.0930868, -0.50559217, 0.48147443, - # 2.628784, for 10 + 2.628784, # for 10 -1.3962275, ], gripper_init_params=[-1.56], diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml index f52c13ed31..e7c53a93d1 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml @@ -7,9 +7,9 @@ defaults: - /habitat/simulator/agents@habitat.simulator.agents.main_agent: fetch_suction - /habitat/task: task_config_base - - /habitat/task/rearrange/actions: fetch_suction_arm_base_empty + - /habitat/task/rearrange/actions: fetch_suction_arm_base_empty - /habitat/task/measurements: - - articulated_agent_force + #- articulated_agent_force # TODO: jimmy: remove this since AttributeError: 'IsaacAgentData' object has no attribute 'grasp_mgr - zero - /habitat/task/lab_sensors: - joint_sensor diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml index cc7a8f7202..83936ee9c3 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -1,8 +1,9 @@ # @package _global_ defaults: - play - - /habitat/task/lab_sensors: - - arm_depth_bbox_sensor + # - /habitat/task/lab_sensors: + # - arm_depth_bbox_sensor + # TODO: jimmy: remove this since the we are not able to use API to get the object handle - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: murp_agent - override /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp - override /habitat/task/rearrange/actions: murp_base_arm_empty @@ -10,11 +11,12 @@ defaults: habitat: task: - lab_sensors: - arm_depth_bbox_sensor: - height: 240 - width: 228 + # lab_sensors: + # arm_depth_bbox_sensor: + # height: 240 + # width: 228 + # TODO: jimmy: remove this since the we are not able to use API to get the object handle actions: arm_action: center_cone_vector: [0.0, 1.0, 0.0] - auto_grasp: True + auto_grasp: False diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml index 75011d6a70..62ab0b089d 100644 --- a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml @@ -9,10 +9,10 @@ arm_action: type: "ArmAction" arm_controller: "ArmRelPosMaskAction" grip_controller: "GazeGraspAction" - arm_joint_mask: [1,1,1,1,1] - arm_joint_dimensionality: 5 + arm_joint_mask: [1,1,1,1,1, 1, 1] + arm_joint_dimensionality: 7 grasp_thresh_dist: 0.15 - disable_grip: False + disable_grip: True delta_pos_limit: 0.0872665 ee_ctrl_lim: 0.015 gaze_distance_range: [0.3, 0.75] diff --git a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py index 90fc72d4cf..97bb876255 100644 --- a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py +++ b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py @@ -2,7 +2,7 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -from typing import Dict, List, Optional, Set +from typing import Dict, List, Optional, Set, Tuple import attr import magnum as mn @@ -155,3 +155,8 @@ def base_pos(self, position: mn.Vector3): pos_usd = isaac_prim_utils.habitat_to_usd_position(position) rw._robot.set_world_pose(pos_usd, rotation_usd) + + @property + def arm_joint_limits(self) -> Tuple[np.ndarray, np.ndarray]: + # TODO: jimmy: implement this + return \ No newline at end of file diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index b62a35632d..7c933b4969 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -229,37 +229,37 @@ def action_space(self): dtype=np.float32, ) - def _get_processed_action(self, delta_pos, simulation_mode="dynamic"): + def _get_processed_action(self, delta_pos, simulation_mode="kinematic"): """Assign the delta pos actions into a correct joint location""" processed_delta_pos = np.zeros(len(self._arm_joint_mask)) - min_limit, max_limit = self.cur_articulated_agent.arm_joint_limits - - src_idx = 0 - tgt_idx = 0 - for mask in self._arm_joint_mask: - if mask == 0: - tgt_idx += 1 - # Check if the effective size of action is the same as arm_joint_dimensionality - # The reason for this check is that we have two options to control the arm: - # option 1: if arm_joint_dimensionality is the same as arm_joint_mask, it means that - # arm_joint_dimensionality, arm_joint_mask, and _arm_joint_limit have the same length/size - # option 2: if arm_joint_dimensionality is different from arm_joint_mask, it means that - # arm_joint_dimensionality, arm_joint_mask, and _arm_joint_limit have the differet length/size - # Based on these, we increase the src_idx by 1 to correctly assign the value to the right index - if self._config.arm_joint_dimensionality == len( - self._config.arm_joint_mask - ): - src_idx += 1 - continue - processed_delta_pos[tgt_idx] = delta_pos[src_idx] - - # Set the new limits if needed - if self._arm_joint_limit is not None: - min_limit[tgt_idx] = self._arm_joint_limit[src_idx][0] - max_limit[tgt_idx] = self._arm_joint_limit[src_idx][1] - - tgt_idx += 1 - src_idx += 1 + # min_limit, max_limit = self.cur_articulated_agent.arm_joint_limits + + # src_idx = 0 + # tgt_idx = 0 + # for mask in self._arm_joint_mask: + # if mask == 0: + # tgt_idx += 1 + # # Check if the effective size of action is the same as arm_joint_dimensionality + # # The reason for this check is that we have two options to control the arm: + # # option 1: if arm_joint_dimensionality is the same as arm_joint_mask, it means that + # # arm_joint_dimensionality, arm_joint_mask, and _arm_joint_limit have the same length/size + # # option 2: if arm_joint_dimensionality is different from arm_joint_mask, it means that + # # arm_joint_dimensionality, arm_joint_mask, and _arm_joint_limit have the differet length/size + # # Based on these, we increase the src_idx by 1 to correctly assign the value to the right index + # if self._config.arm_joint_dimensionality == len( + # self._config.arm_joint_mask + # ): + # src_idx += 1 + # continue + # processed_delta_pos[tgt_idx] = delta_pos[src_idx] + + # # Set the new limits if needed + # if self._arm_joint_limit is not None: + # min_limit[tgt_idx] = self._arm_joint_limit[src_idx][0] + # max_limit[tgt_idx] = self._arm_joint_limit[src_idx][1] + + # tgt_idx += 1 + # src_idx += 1 # Clip the action. Although habitat_sim will prevent the motor from exceeding limits, # clip the motor joints first here to prevent the arm from being unstable. @@ -270,7 +270,7 @@ def _get_processed_action(self, delta_pos, simulation_mode="dynamic"): else: raise NotImplementedError target_arm_pos = processed_delta_pos + cur_arm_pos - set_arm_pos = np.clip(target_arm_pos, min_limit, max_limit) + set_arm_pos = target_arm_pos #np.clip(target_arm_pos, min_limit, max_limit) return set_arm_pos diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index baa762d1bd..06f6457a33 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -54,6 +54,7 @@ rearrange_collision, rearrange_logger, ) + from habitat_sim.logging import logger from habitat_sim.nav import NavMeshSettings from habitat_sim.physics import CollisionGroups, JointMotorSettings, MotionType @@ -300,11 +301,12 @@ def _get_target_trans(self): # Preprocess the ep_info making necessary datatype conversions. target_trans = [] breakpoint() - rom = self.get_rigid_object_manager() + rom = self.get_rigid_object_manager() for target_handle, trans in self._targets.items(): targ_idx = self._scene_obj_ids.index( rom.get_object_by_handle(target_handle).object_id ) + # self._isaac_rom target_trans.append((targ_idx, trans)) return target_trans @@ -1279,7 +1281,6 @@ def add_or_reset_rigid_objects(self): # drop_pos + offset_vec * 0.38 + up_vec * 0.0, # ), # ] - from habitat.isaac_sim.isaac_rigid_object_manager import ( IsaacRigidObjectManager, ) From 649d7f35fde33c4d00e0c2328d2fc3f786990b66 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Thu, 13 Mar 2025 16:41:53 -0400 Subject: [PATCH 12/50] wip --- examples/hitl/isaacsim_viewer/isaacsim_viewer.py | 3 ++- examples/interactive_play.py | 16 ++++++++++------ .../config/benchmark/rearrange/play/play.yaml | 2 +- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/examples/hitl/isaacsim_viewer/isaacsim_viewer.py b/examples/hitl/isaacsim_viewer/isaacsim_viewer.py index 99c553c2dd..92c15d805e 100644 --- a/examples/hitl/isaacsim_viewer/isaacsim_viewer.py +++ b/examples/hitl/isaacsim_viewer/isaacsim_viewer.py @@ -558,7 +558,8 @@ def __init__(self, app_service: AppService): ) # asset_path = "/home/eric/projects/habitat-lab/data/usd/scenes/102817140.usda" - asset_path = "/home/joanne/habitat-lab/data/usd/scenes/fremont_static_objects.usda" # YOUR_PATH + # asset_path = "/home/joanne/habitat-lab/data/usd/scenes/fremont_static_objects.usda" # YOUR_PATH + asset_path = "/home/jmmy/research/hab_training/habitat-lab/data/usd/scenes/fremont_static_objects.usda" from omni.isaac.core.utils.stage import add_reference_to_stage add_reference_to_stage( diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 8d8cb5196a..79ac83a0ef 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -84,7 +84,8 @@ DEFAULT_RENDER_STEPS_LIMIT = 60 SAVE_VIDEO_DIR = "./data/vids" SAVE_ACTIONS_DIR = "./data/interactive_play_replays" -os.environ["SDL_VIDEODRIVER"] = "x11" #"dummy" # we need this for using pygame in unbuntu +#os.environ["SDL_VIDEODRIVER"] = "x11" #"dummy" # we need this for using pygame in unbuntu +#os.environ["QT_GRAPHICSSYSTEM"] = "native" def step_env(env, action_name, action_args): return env.step({"action": action_name, "action_args": action_args}) @@ -485,7 +486,7 @@ def update(self, env, step_result, update_idx): return step_result -def play_env(env, args, config): +def play_env(env, args, config, screen): render_steps_limit = None if args.no_render: render_steps_limit = DEFAULT_RENDER_STEPS_LIMIT @@ -500,9 +501,9 @@ def play_env(env, args, config): if not args.no_render: draw_obs = observations_to_image(obs, {}) - screen = pygame.display.set_mode( - [draw_obs.shape[1], draw_obs.shape[0]] - ) + # screen = pygame.display.set_mode( + # [draw_obs.shape[1], draw_obs.shape[0]] + # ) update_idx = 0 target_fps = 60.0 @@ -827,5 +828,8 @@ def has_pygame(): task_config.actions["pddl_apply_action"] = PddlApplyActionConfig() pygame.init() # due to https://github.com/facebookresearch/habitat-lab/issues/1538#issuecomment-1902985545 + screen = pygame.display.set_mode( + [988, 512] + ) with habitat.Env(config=config) as env: - play_env(env, args, config) + play_env(env, args, config, screen) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml index e7c53a93d1..1832b806bd 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml @@ -61,6 +61,6 @@ habitat: height: 128 width: 128 habitat_sim_v0: - enable_physics: True + enable_physics: False dataset: data_path: data/datasets/replica_cad/rearrange/v1/{split}/rearrange_easy.json.gz From 60bbf2e2d2f4b23d486f9549aefb5b782bca321c Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Fri, 14 Mar 2025 11:24:37 -0400 Subject: [PATCH 13/50] make interactive play somehow work --- examples/interactive_play.py | 79 +++++++++++++------ .../config/default_structured_configs.py | 17 ++++ .../actions/murp_base_arm_empty.yaml | 14 +--- habitat-lab/habitat/core/registry.py | 1 - .../tasks/rearrange/actions/actions.py | 52 +++++++++--- 5 files changed, 114 insertions(+), 49 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 79ac83a0ef..60aa52bebc 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -78,6 +78,8 @@ except ImportError: pygame = None +import cv2 + # Please reach out to the paper authors to obtain this file DEFAULT_POSE_PATH = "data/humanoids/humanoid_data/walking_motion_processed.pkl" DEFAULT_CFG = "benchmark/rearrange/play/play_murp.yaml" @@ -87,10 +89,17 @@ #os.environ["SDL_VIDEODRIVER"] = "x11" #"dummy" # we need this for using pygame in unbuntu #os.environ["QT_GRAPHICSSYSTEM"] = "native" +NAMED_WINDOW = "Play Murp" +USE_CV2 = True + +# cv2 relative functions +def initializeWindow(): + cv2.namedWindow(NAMED_WINDOW, cv2.WINDOW_NORMAL) + + def step_env(env, action_name, action_args): return env.step({"action": action_name, "action_args": action_args}) - def get_input_vel_ctlr( skip_pygame, cfg, @@ -100,6 +109,7 @@ def get_input_vel_ctlr( agent_to_control, control_humanoid, humanoid_controller, + key=None, ): if skip_pygame: return step_env(env, "empty", {}), None, False @@ -115,8 +125,10 @@ def get_input_vel_ctlr( base_action_name = f"{agent_k}humanoidjoint_action" base_key = "human_joints_trans" else: - if "spot" in cfg or "murp" in cfg: + if "spot" in cfg: base_action_name = f"{agent_k}base_velocity_non_cylinder" + elif "murp" in cfg: + base_action_name = f"{agent_k}base_vel_isaac" else: base_action_name = f"{agent_k}base_velocity" arm_key = "arm_action" @@ -156,18 +168,21 @@ def get_input_vel_ctlr( elif keys[pygame.K_n]: env._sim.navmesh_visualization = not env._sim.navmesh_visualization + if key != -1: + print(f"key: {key}") + if not_block_input: # Base control - if keys[pygame.K_j]: + if keys[pygame.K_j] or key == ord("j"): # Left base_action = [0, 1] - elif keys[pygame.K_l]: + elif keys[pygame.K_l] or key == ord("l"): # Right base_action = [0, -1] - elif keys[pygame.K_k]: + elif keys[pygame.K_k] or key == ord("k"): # Back base_action = [-1, 0] - elif keys[pygame.K_i]: + elif keys[pygame.K_i] or key == ord("i"): # Forward base_action = [1, 0] @@ -478,15 +493,15 @@ def update(self, env, step_result, update_idx): trans = mn.Matrix4.from_( quat.to_matrix(), mn.Vector3(*self._free_xyz) ) - env._sim._sensors[ - "third_rgb" - ]._sensor_object.node.transformation = trans + # env._sim._sensors[ + # "third_rgb" + # ]._sensor_object.node.transformation = trans step_result = env._sim.get_sensor_observations() return step_result return step_result -def play_env(env, args, config, screen): +def play_env(env, args, config): render_steps_limit = None if args.no_render: render_steps_limit = DEFAULT_RENDER_STEPS_LIMIT @@ -501,10 +516,17 @@ def play_env(env, args, config, screen): if not args.no_render: draw_obs = observations_to_image(obs, {}) - # screen = pygame.display.set_mode( - # [draw_obs.shape[1], draw_obs.shape[0]] - # ) - + # draw_obs: (512, 988, 3) + if USE_CV2: + initializeWindow() + cv2.imshow(NAMED_WINDOW, draw_obs.astype(np.uint8)) + key = cv2.waitKey(1) # need to have wait key to show images + # python examples/interactive_play.py --disable-inverse-kinematics + else: + screen = pygame.display.set_mode( + [draw_obs.shape[1], draw_obs.shape[0]] + ) + update_idx = 0 target_fps = 60.0 prev_time = time.time() @@ -524,8 +546,12 @@ def play_env(env, args, config, screen): humanoid_controller = HumanoidRearrangeController(args.walk_pose_path) humanoid_controller.reset(env._sim.articulated_agent.base_pos) + env_steps = 0 while True: - print("A!") + + print(f"Step: {env_steps}") + env_steps += 1 + if ( args.save_actions and len(all_arm_actions) > args.save_actions_count @@ -558,6 +584,7 @@ def play_env(env, args, config, screen): agent_to_control, args.control_humanoid, humanoid_controller=humanoid_controller, + key=key, ) if not args.no_render and keys[pygame.K_c]: @@ -633,10 +660,15 @@ def play_env(env, args, config, screen): draw_ob = use_ob[:] if not args.no_render: - draw_ob = np.transpose(draw_ob, (1, 0, 2)) - draw_obuse_ob = pygame.surfarray.make_surface(draw_ob) - screen.blit(draw_obuse_ob, (0, 0)) - pygame.display.update() + if USE_CV2: + cv2.imshow(NAMED_WINDOW, draw_obs.astype(np.uint8)) + key = cv2.waitKey(1) # need to have wait key to show images + else: + draw_ob = np.transpose(draw_ob, (1, 0, 2)) + draw_obuse_ob = pygame.surfarray.make_surface(draw_ob) + screen.blit(draw_obuse_ob, (0, 0)) + pygame.display.update() + if args.save_obs: all_obs.append(draw_ob) # type: ignore[assignment] @@ -652,6 +684,8 @@ def play_env(env, args, config, screen): time.sleep(delay) prev_time = curr_time + print(env.sim.articulated_agent.base_transformation.translation) + if args.save_actions: if len(all_arm_actions) < args.save_actions_count: raise ValueError( @@ -786,7 +820,7 @@ def has_pygame(): agent_config = get_agent_config(sim_config=sim_config) agent_config.sim_sensors.update( { - "third_rgb_sensor": ThirdRGBSensorConfig( + "third_rgb": ThirdRGBSensorConfig( height=args.play_cam_res, width=args.play_cam_res ) } @@ -828,8 +862,5 @@ def has_pygame(): task_config.actions["pddl_apply_action"] = PddlApplyActionConfig() pygame.init() # due to https://github.com/facebookresearch/habitat-lab/issues/1538#issuecomment-1902985545 - screen = pygame.display.set_mode( - [988, 512] - ) with habitat.Env(config=config) as env: - play_env(env, args, config, screen) + play_env(env, args, config) diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index 1394fb417e..4752ac300b 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -269,6 +269,17 @@ class BaseVelocityActionConfig(ActionConfig): allow_back: bool = True +@dataclass +class BaseVelIsaacActionConfig(ActionConfig): + r""" + In Rearrangement only for the non cylinder shape of the robot. Corresponds to the base velocity. Contains two continuous actions, the first one controls forward and backward motion, the second the rotation. + """ + type: str = "BaseVelIsaacAction" + lin_speed: float = 10.0 + ang_speed: float = 10.0 + allow_dyn_slide: bool = True + allow_back: bool = True + @dataclass class BaseVelocityNonCylinderActionConfig(ActionConfig): r""" @@ -1969,6 +1980,12 @@ class HabitatConfig(HabitatBaseConfig): name="base_velocity_non_cylinder", node=BaseVelocityNonCylinderActionConfig, ) +cs.store( + package="habitat.task.actions.base_vel_isaac", + group="habitat/task/actions", + name="base_vel_isaac", + node=BaseVelIsaacActionConfig, +) cs.store( package="habitat.task.actions.humanoidjoint_action", group="habitat/task/actions", diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml index 62ab0b089d..12b4eab52d 100644 --- a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml @@ -1,7 +1,8 @@ # @package habitat.task.actions defaults: - /habitat/task/actions: - - base_velocity_non_cylinder + - base_vel_isaac # BaseVelIsaacAction + - /habitat/task/actions: - arm_action - empty - _self_ @@ -16,13 +17,4 @@ arm_action: delta_pos_limit: 0.0872665 ee_ctrl_lim: 0.015 gaze_distance_range: [0.3, 0.75] - center_cone_angle_threshold: 20.0 -base_velocity_non_cylinder: - allow_dyn_slide: False - # There is a collision if the difference between the clamped NavMesh position and target position - # is more than than collision_threshold for any point - collision_threshold: 1e-5 - # The x and y locations of the clamped NavMesh position - navmesh_offset: [[0.0, 0.0], [0.25, 0.0], [-0.25, 0.0]] - # If we allow the robot to move laterally - enable_lateral_move: False + center_cone_angle_threshold: 20.0 \ No newline at end of file diff --git a/habitat-lab/habitat/core/registry.py b/habitat-lab/habitat/core/registry.py index 55beea002d..0be89631fc 100644 --- a/habitat-lab/habitat/core/registry.py +++ b/habitat-lab/habitat/core/registry.py @@ -164,7 +164,6 @@ def register_task_action( :param name: Key with which the task action will be registered. If :py:`None` will use the name of the task action's method. """ - return cls._register_impl( "task_action", to_register, name, assert_type=Action ) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 7c933b4969..0a8f44ae07 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -644,7 +644,8 @@ def update_base(self, if_rotation): # Get the control frequency ctrl_freq = self._sim.ctrl_freq # Get the current transformation - trans = self.cur_articulated_agent.sim_obj.transformation + # TODO: jimmy: check + trans = self.cur_articulated_agent.base_transformation #self.cur_articulated_agent.sim_obj.transformation # Get the current rigid state rigid_state = habitat_sim.RigidState( mn.Quaternion.from_matrix(trans.rotation()), trans.translation @@ -668,18 +669,23 @@ def update_base(self, if_rotation): trans, target_trans, target_rigid_state, compute_sliding ) # Update the base - self.cur_articulated_agent.sim_obj.transformation = new_target_trans - - if self.cur_grasp_mgr.snap_idx is not None: - # Holding onto an object, also kinematically update the object. - # object. - self.cur_grasp_mgr.update_object_to_grasp() - - if self.cur_articulated_agent._base_type == "leg": - # Fix the leg joints - self.cur_articulated_agent.leg_joint_pos = ( - self.cur_articulated_agent.params.leg_init_params - ) + # TODO: jimmy: check + # self.cur_articulated_agent.sim_obj.transformation = new_target_trans + self.cur_articulated_agent.base_transformation = new_target_trans + + try: + if self.cur_grasp_mgr.snap_idx is not None: + # Holding onto an object, also kinematically update the object. + # object. + self.cur_grasp_mgr.update_object_to_grasp() + + if self.cur_articulated_agent._base_type == "leg": + # Fix the leg joints + self.cur_articulated_agent.leg_joint_pos = ( + self.cur_articulated_agent.params.leg_init_params + ) + except Exception: + pass def step(self, *args, **kwargs): # Check if we can apply the base action given a_selection_of_base_or_arm action. @@ -863,3 +869,23 @@ def step(self, *args, **kwargs): self.cur_articulated_agent.set_joint_transform( new_joints, new_transform_offset, new_transform_base ) + + +@registry.register_task_action +class BaseVelIsaacAction(BaseVelAction): + def step(self, *args, **kwargs): + lin_vel, ang_vel = kwargs[self._action_arg_prefix + "base_vel"] + print(f"lin_vel: {lin_vel}; ang_vel: {ang_vel}") + lin_vel = np.clip(lin_vel, -1, 1) * self._lin_speed + ang_vel = np.clip(ang_vel, -1, 1) * self._ang_speed + if not self._allow_back: + lin_vel = np.maximum(lin_vel, 0) + + self.base_vel_ctrl.linear_velocity = mn.Vector3(lin_vel, 0, 0) + self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0) + self.cur_articulated_agent._robot_wrapper._robot.set_angular_velocity( + [0, 0, ang_vel] + ) + self.cur_articulated_agent._robot_wrapper._robot.set_linear_velocity( + [lin_vel, 0, 0] + ) \ No newline at end of file From b1f7119d9160e97e2865816690d46014ba98e464 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Fri, 14 Mar 2025 12:55:37 -0400 Subject: [PATCH 14/50] interactive play --- examples/interactive_play.py | 8 ++++---- habitat-lab/habitat/core/env.py | 1 - habitat-lab/habitat/isaac_sim/actions.py | 2 ++ habitat-lab/habitat/tasks/rearrange/actions/actions.py | 1 + .../habitat/tasks/rearrange/isaac_rearrange_sim.py | 5 +++-- 5 files changed, 10 insertions(+), 7 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 60aa52bebc..4d86809427 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -493,9 +493,9 @@ def update(self, env, step_result, update_idx): trans = mn.Matrix4.from_( quat.to_matrix(), mn.Vector3(*self._free_xyz) ) - # env._sim._sensors[ - # "third_rgb" - # ]._sensor_object.node.transformation = trans + env._sim._sensors[ + "third_rgb" + ]._sensor_object.node.transformation = trans step_result = env._sim.get_sensor_observations() return step_result return step_result @@ -661,7 +661,7 @@ def play_env(env, args, config): if not args.no_render: if USE_CV2: - cv2.imshow(NAMED_WINDOW, draw_obs.astype(np.uint8)) + cv2.imshow(NAMED_WINDOW, draw_ob.astype(np.uint8)) key = cv2.waitKey(1) # need to have wait key to show images else: draw_ob = np.transpose(draw_ob, (1, 0, 2)) diff --git a/habitat-lab/habitat/core/env.py b/habitat-lab/habitat/core/env.py index 77c54dfc56..57d9950395 100644 --- a/habitat-lab/habitat/core/env.py +++ b/habitat-lab/habitat/core/env.py @@ -110,7 +110,6 @@ def __init__( self.number_of_episodes = len(self.episodes) else: self.number_of_episodes = None - self._sim = make_sim( id_sim=self._config.simulator.type, config=self._config.simulator ) diff --git a/habitat-lab/habitat/isaac_sim/actions.py b/habitat-lab/habitat/isaac_sim/actions.py index 37fceb564d..1ce3542bf4 100644 --- a/habitat-lab/habitat/isaac_sim/actions.py +++ b/habitat-lab/habitat/isaac_sim/actions.py @@ -15,6 +15,7 @@ class BaseVelIsaacAction(BaseVelAction): def step(self, *args, **kwargs): lin_vel, ang_vel = kwargs[self._action_arg_prefix + "base_vel"] + print(f"lin_vel: {lin_vel}; ang_vel: {ang_vel}") lin_vel = np.clip(lin_vel, -1, 1) * self._lin_speed ang_vel = np.clip(ang_vel, -1, 1) * self._ang_speed if not self._allow_back: @@ -22,6 +23,7 @@ def step(self, *args, **kwargs): self.base_vel_ctrl.linear_velocity = mn.Vector3(lin_vel, 0, 0) self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0) + # self.cur_articulated_agent._robot_wrapper._robot.set_angular_velocity( [0, 0, ang_vel] ) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 0a8f44ae07..05015b4463 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -883,6 +883,7 @@ def step(self, *args, **kwargs): self.base_vel_ctrl.linear_velocity = mn.Vector3(lin_vel, 0, 0) self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0) + # self.cur_articulated_agent._robot_wrapper._robot.set_angular_velocity( [0, 0, ang_vel] ) diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index 06f6457a33..ee11b62f42 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -25,7 +25,7 @@ import numpy.typing as npt import habitat_sim - +import habitat # flake8: noqa from habitat.articulated_agents.robots import FetchRobot, FetchRobotNoWheels from habitat.config import read_write @@ -102,7 +102,8 @@ def bind_physics_material_to_hierarchy( @registry.register_simulator(name="IsaacRearrangeSim-v0") class IsaacRearrangeSim(HabitatSim): def __init__(self, config: "DictConfig"): - #config.scene = "NONE" # cannot do scene none here when regiestering the env for interactive play py + with habitat.config.read_write(config): + config.scene = "NONE" # load from interactive_play is read only by default if len(config.agents) > 1: with read_write(config): for agent_name, agent_cfg in config.agents.items(): From 73cdee1ef554bbb5fc000f0a3af2c1c05ec983b8 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Fri, 14 Mar 2025 15:57:54 -0400 Subject: [PATCH 15/50] make interactive play runnable --- examples/interactive_play.py | 86 ++++++++++++++---- .../benchmark/rearrange/play/play_murp.yaml | 22 ++--- .../config/default_structured_configs.py | 17 +++- .../actions/murp_base_arm_empty.yaml | 26 +++--- .../tasks/rearrange/actions/actions.py | 87 +++++++++++++++++-- 5 files changed, 187 insertions(+), 51 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 4d86809427..f22917f9fa 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -86,12 +86,13 @@ DEFAULT_RENDER_STEPS_LIMIT = 60 SAVE_VIDEO_DIR = "./data/vids" SAVE_ACTIONS_DIR = "./data/interactive_play_replays" -#os.environ["SDL_VIDEODRIVER"] = "x11" #"dummy" # we need this for using pygame in unbuntu -#os.environ["QT_GRAPHICSSYSTEM"] = "native" +# os.environ["SDL_VIDEODRIVER"] = "x11" #"dummy" # we need this for using pygame in unbuntu +# os.environ["QT_GRAPHICSSYSTEM"] = "native" NAMED_WINDOW = "Play Murp" USE_CV2 = True + # cv2 relative functions def initializeWindow(): cv2.namedWindow(NAMED_WINDOW, cv2.WINDOW_NORMAL) @@ -100,6 +101,7 @@ def initializeWindow(): def step_env(env, action_name, action_args): return env.step({"action": action_name, "action_args": action_args}) + def get_input_vel_ctlr( skip_pygame, cfg, @@ -119,7 +121,11 @@ def get_input_vel_ctlr( agent_k = f"agent_{agent_to_control}_" else: agent_k = "" - arm_action_name = f"{agent_k}arm_action" + + if "murp" in cfg: + arm_action_name = f"{agent_k}arm_reach_ee" + else: + arm_action_name = f"{agent_k}arm_action" if control_humanoid: base_action_name = f"{agent_k}humanoidjoint_action" @@ -135,10 +141,15 @@ def get_input_vel_ctlr( grip_key = "grip_action" base_key = "base_vel" - if arm_action_name in env.action_space.spaces: + if "murp" in cfg: + arm_action_space = np.zeros(6) + arm_ctrlr = None + base_action = None + elif arm_action_name in env.action_space.spaces: arm_action_space = env.action_space.spaces[arm_action_name].spaces[ arm_key ] + # Murp does not have this arm_ctrlr = env.task.actions[arm_action_name].arm_ctrlr base_action = None elif "stretch" in cfg: @@ -223,6 +234,38 @@ def get_input_vel_ctlr( elif keys[pygame.K_7]: arm_action[6] = -1.0 + elif arm_action_space.shape[0] == 6: + # Velocity control. A different key for each joint + if keys[pygame.K_q] or key == ord("q"): + arm_action[0] = 0.25 + elif keys[pygame.K_1] or key == ord("1"): + arm_action[0] = -0.25 + + elif keys[pygame.K_w] or key == ord("w"): + arm_action[1] = 0.25 + elif keys[pygame.K_2] or key == ord("2"): + arm_action[1] = -0.25 + + elif keys[pygame.K_e] or key == ord("e"): + arm_action[2] = 0.25 + elif keys[pygame.K_3] or key == ord("3"): + arm_action[2] = -0.25 + + elif keys[pygame.K_r] or key == ord("r"): + arm_action[3] = 0.25 + elif keys[pygame.K_4] or key == ord("4"): + arm_action[3] = -0.25 + + elif keys[pygame.K_t] or key == ord("t"): + arm_action[4] = 0.25 + elif keys[pygame.K_5] or key == ord("5"): + arm_action[4] = -0.25 + + elif keys[pygame.K_y] or key == ord("y"): + arm_action[5] = 0.25 + elif keys[pygame.K_6] or key == ord("6"): + arm_action[5] = -0.25 + elif arm_action_space.shape[0] == 4: # Velocity control. A different key for each joint # This is for Spot robot which a user can only control the effective arm in the real robot @@ -414,7 +457,13 @@ def get_input_vel_ctlr( grip_key: arm_action[-1], } else: - args = {arm_key: arm_action, grip_key: magic_grasp} + if "murp" in cfg: + args = { + "target_pos": arm_action[0:3], + "target_rot": arm_action[3:], + } + else: + args = {arm_key: arm_action, grip_key: magic_grasp} if magic_grasp is None: arm_action = [*arm_action, 0.0] @@ -520,13 +569,13 @@ def play_env(env, args, config): if USE_CV2: initializeWindow() cv2.imshow(NAMED_WINDOW, draw_obs.astype(np.uint8)) - key = cv2.waitKey(1) # need to have wait key to show images + key = cv2.waitKey(1) # need to have wait key to show images # python examples/interactive_play.py --disable-inverse-kinematics else: screen = pygame.display.set_mode( [draw_obs.shape[1], draw_obs.shape[0]] - ) - + ) # type: ignore + update_idx = 0 target_fps = 60.0 prev_time = time.time() @@ -548,7 +597,6 @@ def play_env(env, args, config): env_steps = 0 while True: - print(f"Step: {env_steps}") env_steps += 1 @@ -660,15 +708,15 @@ def play_env(env, args, config): draw_ob = use_ob[:] if not args.no_render: - if USE_CV2: - cv2.imshow(NAMED_WINDOW, draw_ob.astype(np.uint8)) - key = cv2.waitKey(1) # need to have wait key to show images + if USE_CV2: + cv2.imshow(NAMED_WINDOW, draw_ob.astype(np.uint8)) + key = cv2.waitKey(1) # need to have wait key to show images else: draw_ob = np.transpose(draw_ob, (1, 0, 2)) draw_obuse_ob = pygame.surfarray.make_surface(draw_ob) screen.blit(draw_obuse_ob, (0, 0)) pygame.display.update() - + if args.save_obs: all_obs.append(draw_ob) # type: ignore[assignment] @@ -850,17 +898,17 @@ def has_pygame(): args.disable_inverse_kinematics = True if not args.disable_inverse_kinematics: - if "arm_action" not in task_config.actions: + if "arm_action" not in task_config.actions and ( + "arm_reach_ee" not in task_config.actions + ): raise ValueError( "Action space does not have any arm control so cannot add inverse kinematics. Specify the `--disable-inverse-kinematics` option" ) - sim_config.agents.main_agent.ik_arm_urdf = ( - "./data/robots/hab_fetch/robots/fetch_onlyarm.urdf" - ) - task_config.actions.arm_action.arm_controller = "ArmEEAction" + sim_config.agents.main_agent.ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_left_arm_only.urdf" + # task_config.actions.arm_action.arm_controller = "ArmEEAction" if task_config.type == "RearrangePddlTask-v0": task_config.actions["pddl_apply_action"] = PddlApplyActionConfig() - pygame.init() # due to https://github.com/facebookresearch/habitat-lab/issues/1538#issuecomment-1902985545 + pygame.init() # due to https://github.com/facebookresearch/habitat-lab/issues/1538#issuecomment-1902985545 with habitat.Env(config=config) as env: play_env(env, args, config) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml index 83936ee9c3..ec7f0ed657 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -9,14 +9,14 @@ defaults: - override /habitat/task/rearrange/actions: murp_base_arm_empty - _self_ -habitat: - task: - # lab_sensors: - # arm_depth_bbox_sensor: - # height: 240 - # width: 228 - # TODO: jimmy: remove this since the we are not able to use API to get the object handle - actions: - arm_action: - center_cone_vector: [0.0, 1.0, 0.0] - auto_grasp: False +# habitat: +# task: +# # lab_sensors: +# # arm_depth_bbox_sensor: +# # height: 240 +# # width: 228 +# # TODO: jimmy: remove this since the we are not able to use API to get the object handle +# actions: +# arm_action: +# center_cone_vector: [0.0, 1.0, 0.0] +# auto_grasp: False diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index 4752ac300b..7039b2d44c 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -279,7 +279,16 @@ class BaseVelIsaacActionConfig(ActionConfig): ang_speed: float = 10.0 allow_dyn_slide: bool = True allow_back: bool = True - + + +@dataclass +class ArmReachEEActionConfig(ActionConfig): + r""" + In Rearrangement only for the non cylinder shape of the robot. Corresponds to the base velocity. Contains two continuous actions, the first one controls forward and backward motion, the second the rotation. + """ + type: str = "ArmReachEEAction" + + @dataclass class BaseVelocityNonCylinderActionConfig(ActionConfig): r""" @@ -1986,6 +1995,12 @@ class HabitatConfig(HabitatBaseConfig): name="base_vel_isaac", node=BaseVelIsaacActionConfig, ) +cs.store( + package="habitat.task.actions.arm_reach_ee", + group="habitat/task/actions", + name="arm_reach_ee", + node=ArmReachEEActionConfig, +) cs.store( package="habitat.task.actions.humanoidjoint_action", group="habitat/task/actions", diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml index 12b4eab52d..285f44bb80 100644 --- a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml @@ -3,18 +3,18 @@ defaults: - /habitat/task/actions: - base_vel_isaac # BaseVelIsaacAction - /habitat/task/actions: - - arm_action + - arm_reach_ee - empty - _self_ -arm_action: - type: "ArmAction" - arm_controller: "ArmRelPosMaskAction" - grip_controller: "GazeGraspAction" - arm_joint_mask: [1,1,1,1,1, 1, 1] - arm_joint_dimensionality: 7 - grasp_thresh_dist: 0.15 - disable_grip: True - delta_pos_limit: 0.0872665 - ee_ctrl_lim: 0.015 - gaze_distance_range: [0.3, 0.75] - center_cone_angle_threshold: 20.0 \ No newline at end of file +# arm_action: +# type: "ArmAction" +# arm_controller: "ArmRelPosMaskAction" +# grip_controller: "GazeGraspAction" +# arm_joint_mask: [1,1,1,1,1, 1, 1] +# arm_joint_dimensionality: 7 +# grasp_thresh_dist: 0.15 +# disable_grip: True +# delta_pos_limit: 0.0872665 +# ee_ctrl_lim: 0.015 +# gaze_distance_range: [0.3, 0.75] +# center_cone_angle_threshold: 20.0 diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 05015b4463..bc5b1814cb 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -137,9 +137,9 @@ def action_space(self): + "arm_action": self.arm_ctrlr.action_space, } if self.grip_ctrlr is not None and self.grip_ctrlr.requires_action: - action_spaces[self._action_arg_prefix + "grip_action"] = ( - self.grip_ctrlr.action_space - ) + action_spaces[ + self._action_arg_prefix + "grip_action" + ] = self.grip_ctrlr.action_space return spaces.Dict(action_spaces) def step(self, *args, **kwargs): @@ -270,7 +270,9 @@ def _get_processed_action(self, delta_pos, simulation_mode="kinematic"): else: raise NotImplementedError target_arm_pos = processed_delta_pos + cur_arm_pos - set_arm_pos = target_arm_pos #np.clip(target_arm_pos, min_limit, max_limit) + set_arm_pos = ( + target_arm_pos # np.clip(target_arm_pos, min_limit, max_limit) + ) return set_arm_pos @@ -645,7 +647,9 @@ def update_base(self, if_rotation): ctrl_freq = self._sim.ctrl_freq # Get the current transformation # TODO: jimmy: check - trans = self.cur_articulated_agent.base_transformation #self.cur_articulated_agent.sim_obj.transformation + trans = ( + self.cur_articulated_agent.base_transformation + ) # self.cur_articulated_agent.sim_obj.transformation # Get the current rigid state rigid_state = habitat_sim.RigidState( mn.Quaternion.from_matrix(trans.rotation()), trans.translation @@ -792,7 +796,7 @@ def step(self, delta_ee_pos, **kwargs): self.calc_ee_target(delta_ee_pos) des_joint_pos = self.calc_desired_joints() - self.set_desired_ee_pos(des_joint_pos, "kinematic") + self.set_desired_ee_pos(des_joint_pos, "kinematic") # type: ignore if self._render_ee_target: global_pos = self._sim.articulated_agent.base_transformation.transform_point( @@ -889,4 +893,73 @@ def step(self, *args, **kwargs): ) self.cur_articulated_agent._robot_wrapper._robot.set_linear_velocity( [lin_vel, 0, 0] - ) \ No newline at end of file + ) + + +@registry.register_task_action +class ArmReachEEAction(ArmEEAction): + def __init__(self, *args, **kwargs): + super().__init__(self, *args, **kwargs) + + self._robot_wrapper = self.cur_articulated_agent._robot_wrapper + self.ee_rot_target = None + self._use_ee_rot = self._config.get("use_ee_rot", False) + + @property + def action_space(self): + return spaces.Box(shape=(6,), low=-1, high=1, dtype=np.float32) + + def reset(self, *args, **kwargs): + try: + self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( + np.array( + self._sim.articulated_agent._robot_wrapper.arm_joint_pos + ) + ) + except: + self.ee_target = None + self.ee_rot_target = None + + def calc_desired_joints(self): + joint_pos = np.array( + self._sim.articulated_agent._robot_wrapper.arm_joint_pos + ) + joint_vel = np.zeros(joint_pos.shape) + + self._ik_helper.set_arm_state(joint_pos, joint_vel) + + des_joint_pos = self._ik_helper.calc_ik( + self.ee_target, self.ee_rot_target + ) + return np.array(des_joint_pos) + + def apply_joint_limits(self, des_joint_pos): + murp_joint_limits_lower = np.deg2rad( + np.array([-157, -102, -166, -174, -160, 31, -172]) + ) + murp_joint_limits_upper = np.deg2rad( + np.array([157, 102, 166, -8, 160, 258, 172]) + ) + return np.clip( + des_joint_pos, murp_joint_limits_lower, murp_joint_limits_upper + ) + + def step(self, *args, **kwargs): + target_pos = kwargs[self._action_arg_prefix + "target_pos"] + target_rot = kwargs[self._action_arg_prefix + "target_rot"] + # base_pos, base_rot = self._robot_wrapper.get_root_pose() + + print(f"target_pos: {target_pos}; target_rot: {target_rot}") + print(f"EE: {self.ee_target} {self.ee_rot_target}") + + # def inverse_transform(pos_a, rot_b, pos_b): + # inv_pos = rot_b.inverted().transform_vector(pos_a - pos_b) + # return inv_pos + # target_rel_pos = inverse_transform(target_pos, base_rot, base_pos) + self.ee_target += np.array(target_pos) + self.ee_rot_target += np.array(target_rot) + self.apply_ee_constraints() + des_joint_pos = self.calc_desired_joints() + des_joint_pos = self.apply_joint_limits(des_joint_pos) + print(f"des_joint_pos: {des_joint_pos}") + self._robot_wrapper._target_arm_joint_positions = des_joint_pos From 6d83727cfb1aacecb6565206c291e122b581e3d3 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Fri, 14 Mar 2025 16:26:53 -0400 Subject: [PATCH 16/50] add finger control --- examples/interactive_play.py | 38 +++++++++++++++++-- .../tasks/rearrange/actions/actions.py | 10 ++++- 2 files changed, 44 insertions(+), 4 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index f22917f9fa..acbed2cf91 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -142,7 +142,7 @@ def get_input_vel_ctlr( base_key = "base_vel" if "murp" in cfg: - arm_action_space = np.zeros(6) + arm_action_space = np.zeros(12) arm_ctrlr = None base_action = None elif arm_action_name in env.action_space.spaces: @@ -234,7 +234,7 @@ def get_input_vel_ctlr( elif keys[pygame.K_7]: arm_action[6] = -1.0 - elif arm_action_space.shape[0] == 6: + elif arm_action_space.shape[0] == 12: # Velocity control. A different key for each joint if keys[pygame.K_q] or key == ord("q"): arm_action[0] = 0.25 @@ -266,6 +266,36 @@ def get_input_vel_ctlr( elif keys[pygame.K_6] or key == ord("6"): arm_action[5] = -0.25 + elif key == ord("a"): + arm_action[6] = 0.25 + elif key == ord("z"): + arm_action[6] = -0.25 + + elif key == ord("s"): + arm_action[7] = 0.25 + elif key == ord("x"): + arm_action[7] = -0.25 + + elif key == ord("d"): + arm_action[8] = 0.25 + elif key == ord("c"): + arm_action[8] = -0.25 + + elif key == ord("f"): + arm_action[9] = 0.25 + elif key == ord("v"): + arm_action[9] = -0.25 + + elif key == ord("g"): + arm_action[10] = 0.25 + elif key == ord("b"): + arm_action[10] = -0.25 + + elif key == ord("h"): + arm_action[11] = 0.25 + elif key == ord("n"): + arm_action[11] = -0.25 + elif arm_action_space.shape[0] == 4: # Velocity control. A different key for each joint # This is for Spot robot which a user can only control the effective arm in the real robot @@ -460,7 +490,8 @@ def get_input_vel_ctlr( if "murp" in cfg: args = { "target_pos": arm_action[0:3], - "target_rot": arm_action[3:], + "target_rot": arm_action[3:6], + "target_finger": arm_action[6:], } else: args = {arm_key: arm_action, grip_key: magic_grasp} @@ -686,6 +717,7 @@ def play_env(env, args, config): obs = step_result info = env.get_metrics() + reward_key = [k for k in info if "reward" in k] if len(reward_key) > 0: reward = info[reward_key[0]] diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index bc5b1814cb..fdf5ddcab9 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -907,7 +907,7 @@ def __init__(self, *args, **kwargs): @property def action_space(self): - return spaces.Box(shape=(6,), low=-1, high=1, dtype=np.float32) + return spaces.Box(shape=(12,), low=-1, high=1, dtype=np.float32) def reset(self, *args, **kwargs): try: @@ -916,9 +916,13 @@ def reset(self, *args, **kwargs): self._sim.articulated_agent._robot_wrapper.arm_joint_pos ) ) + self.target_finger = ( + self._robot_wrapper._target_hand_joint_positions + ) except: self.ee_target = None self.ee_rot_target = None + self.target_finger = None def calc_desired_joints(self): joint_pos = np.array( @@ -947,6 +951,7 @@ def apply_joint_limits(self, des_joint_pos): def step(self, *args, **kwargs): target_pos = kwargs[self._action_arg_prefix + "target_pos"] target_rot = kwargs[self._action_arg_prefix + "target_rot"] + finger = kwargs[self._action_arg_prefix + "target_finger"] # base_pos, base_rot = self._robot_wrapper.get_root_pose() print(f"target_pos: {target_pos}; target_rot: {target_rot}") @@ -958,8 +963,11 @@ def step(self, *args, **kwargs): # target_rel_pos = inverse_transform(target_pos, base_rot, base_pos) self.ee_target += np.array(target_pos) self.ee_rot_target += np.array(target_rot) + self.target_finger[0:6] += finger self.apply_ee_constraints() des_joint_pos = self.calc_desired_joints() des_joint_pos = self.apply_joint_limits(des_joint_pos) print(f"des_joint_pos: {des_joint_pos}") + print(f"self.target_finger: {self.target_finger}") self._robot_wrapper._target_arm_joint_positions = des_joint_pos + self._robot_wrapper._target_hand_joint_positions = self.target_finger From b32da3cdd0cfc72008ac87ec2669cd311be0b511 Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Sun, 16 Mar 2025 15:58:41 +0000 Subject: [PATCH 17/50] introduce the flag to switch between h200 and lambda in the interactive_play path --- examples/interactive_play.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index acbed2cf91..77a8eca2e2 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -91,6 +91,7 @@ NAMED_WINDOW = "Play Murp" USE_CV2 = True +TEST_MACHINE = "h200" # h200 / lambda # cv2 relative functions @@ -936,7 +937,18 @@ def has_pygame(): raise ValueError( "Action space does not have any arm control so cannot add inverse kinematics. Specify the `--disable-inverse-kinematics` option" ) - sim_config.agents.main_agent.ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_left_arm_only.urdf" + + ik_arm_urdf = "" + if TEST_MACHINE == "h200": + ik_arm_urdf = "/home/jimmytyyang/research/hab_training/habitat-lab/data/franka_tmr/franka_description_tmr/urdf/franka_tmr_left_arm_only.urdf" + elif TEST_MACHINE == "lambda": + ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_left_arm_only.urdf" + else: + raise ValueError( + f"Cannot recongize the TEST_MACHINE: {TEST_MACHINE}" + ) + + sim_config.agents.main_agent.ik_arm_urdf = ik_arm_urdf # task_config.actions.arm_action.arm_controller = "ArmEEAction" if task_config.type == "RearrangePddlTask-v0": task_config.actions["pddl_apply_action"] = PddlApplyActionConfig() From 40fc62918970ab85f34b55ebd20c035a967b1c10 Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Sun, 16 Mar 2025 16:13:41 +0000 Subject: [PATCH 18/50] able to run in h200 for interacive play --- examples/interactive_play.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 77a8eca2e2..d8c637ba9f 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -594,7 +594,7 @@ def play_env(env, args, config): logger.info("Loaded arm actions") obs = env.reset() - + key = None if not args.no_render: draw_obs = observations_to_image(obs, {}) # draw_obs: (512, 988, 3) From 6642516e22fc752475cfa955702f5297139b6ffc Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 11:05:13 -0400 Subject: [PATCH 19/50] add needed sensors --- examples/interactive_play.py | 4 +- .../benchmark/rearrange/play/pick_murp.yaml | 72 +++ .../benchmark/rearrange/play/play_murp.yaml | 2 +- .../habitat/isaac_sim/isaac_murp_robot.py | 37 +- .../tasks/rearrange/isaac_rearrange_sim.py | 559 ++++++++++-------- .../tasks/rearrange/rearrange_sensors.py | 5 +- .../tasks/rearrange/sub_tasks/pick_task.py | 43 +- 7 files changed, 446 insertions(+), 276 deletions(-) create mode 100644 habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml diff --git a/examples/interactive_play.py b/examples/interactive_play.py index d8c637ba9f..cbe6190769 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -91,7 +91,7 @@ NAMED_WINDOW = "Play Murp" USE_CV2 = True -TEST_MACHINE = "h200" # h200 / lambda +TEST_MACHINE = "lambda" # h200 / lambda # cv2 relative functions @@ -605,7 +605,7 @@ def play_env(env, args, config): # python examples/interactive_play.py --disable-inverse-kinematics else: screen = pygame.display.set_mode( - [draw_obs.shape[1], draw_obs.shape[0]] + [draw_obs.shape[1], draw_obs.shape[0]] # type: ignore ) # type: ignore update_idx = 0 diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml new file mode 100644 index 0000000000..ca4e966869 --- /dev/null +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -0,0 +1,72 @@ +# @package _global_ +defaults: + - /habitat: habitat_config_base + + - /habitat/simulator: isaac_rearrange_sim + - /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: rgbd_head_rgbd_arm_agent + - /habitat/simulator/agents@habitat.simulator.agents.main_agent: fetch_suction + + - /habitat/task: task_config_base + - /habitat/task/rearrange/actions: fetch_suction_arm_base_empty + - /habitat/task/measurements: + #- articulated_agent_force # TODO: jimmy: remove this since AttributeError: 'IsaacAgentData' object has no attribute 'grasp_mgr + - zero + - /habitat/task/lab_sensors: + - joint_sensor + - target_start_sensor + - end_effector_sensor + + - /habitat/dataset/rearrangement: replica_cad + - _self_ + +# Config for empty task to explore the scene. +habitat: + gym: + obs_keys: + - articulated_agent_arm_depth + - joint + task: + # Config for empty task to explore the scene. + type: RearrangePickTask-v0 + count_obj_collisions: True + desired_resting_position: [0.5, 0.0, 1.0] + reward_measure: "zero" + success_measure: "zero" + + # Reach task config + render_target: True + ee_sample_factor: 0.8 + + # In radians + base_angle_noise: 0.0 + base_noise: 0.0 + constraint_violation_ends_episode: False + + force_regenerate: True + environment: + max_episode_steps: 0 + simulator: + #type: RearrangeSim-v0 + seed: 100 + additional_object_paths: + - "data/objects/ycb/configs/" + agents: + main_agent: + radius: 0.3 + sim_sensors: + head_rgb_sensor: + height: 128 + width: 128 + head_depth_sensor: + height: 128 + width: 128 + arm_depth_sensor: + height: 128 + width: 128 + arm_rgb_sensor: + height: 128 + width: 128 + habitat_sim_v0: + enable_physics: False + dataset: + data_path: data/datasets/replica_cad/rearrange/v1/{split}/rearrange_easy.json.gz diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml index ec7f0ed657..b49ba6b443 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -1,6 +1,6 @@ # @package _global_ defaults: - - play + - pick_murp # - /habitat/task/lab_sensors: # - arm_depth_bbox_sensor # TODO: jimmy: remove this since the we are not able to use API to get the object handle diff --git a/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py index 339bc33709..ab85e3645b 100644 --- a/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py +++ b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py @@ -2,20 +2,21 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -from typing import Dict, List, Optional, Set, Tuple +from typing import Tuple import magnum as mn import numpy as np -import quaternion -from habitat.articulated_agents.mobile_manipulator import ( - ArticulatedAgentCameraParams, - MobileManipulatorParams, -) +# from habitat.articulated_agents.mobile_manipulator import ( +# ArticulatedAgentCameraParams, +# MobileManipulatorParams, +# ) from habitat.articulated_agents.robots.murp_robot import MurpRobot from habitat.isaac_sim._internal.murp_robot_wrapper import MurpRobotWrapper from habitat.isaac_sim.isaac_mobile_manipulator import IsaacMobileManipulator +# import quaternion + class IsaacMurpRobot(IsaacMobileManipulator): """Isaac-internal wrapper for a robot. @@ -40,17 +41,25 @@ def base_transformation(self, base_transformation): self._robot_wrapper.set_root_pose(base_transformation.translation, rot) # pose = mn.Matrix4.from_(base_rotation.to_matrix(), base_position + def ee_transform(self): + """ "Return the ee transformation""" + # Get the ee_trans from ee_pose + vec, rot = self._sim.articulated_agent._robot_wrapper.ee_pose() + global_T = mn.Matrix4.from_(rot.to_matrix(), vec) + return global_T + def get_link_transform(self, link_id): - link_positions, link_rotations = ( - self._robot_wrapper.get_link_world_poses() - ) + ( + link_positions, + link_rotations, + ) = self._robot_wrapper.get_link_world_poses() position, rotation = link_positions[link_id], link_rotations[link_id] # breakpoint() pose = mn.Matrix4.from_(rotation.to_matrix(), position) - add_rot = mn.Matrix4.rotation( - mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0) - ) + # add_rot = mn.Matrix4.rotation( + # mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0) + # ) return pose def get_ee_local_pose( @@ -65,8 +74,8 @@ def get_ee_local_pose( "The current manipulator does not have enough end effectors" ) - assert False # todo - return None + raise NotImplementedError("Need to implement get_ee_local_pose") + # return None # ee_transform = self.ee_transform() # base_transform = self.base_transformation diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index ee11b62f42..1d5ab286c9 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -24,8 +24,9 @@ import numpy as np import numpy.typing as npt -import habitat_sim import habitat +import habitat_sim + # flake8: noqa from habitat.articulated_agents.robots import FetchRobot, FetchRobotNoWheels from habitat.config import read_write @@ -54,7 +55,6 @@ rearrange_collision, rearrange_logger, ) - from habitat_sim.logging import logger from habitat_sim.nav import NavMeshSettings from habitat_sim.physics import CollisionGroups, JointMotorSettings, MotionType @@ -103,7 +103,9 @@ def bind_physics_material_to_hierarchy( class IsaacRearrangeSim(HabitatSim): def __init__(self, config: "DictConfig"): with habitat.config.read_write(config): - config.scene = "NONE" # load from interactive_play is read only by default + config.scene = ( + "NONE" # load from interactive_play is read only by default + ) if len(config.agents) > 1: with read_write(config): for agent_name, agent_cfg in config.agents.items(): @@ -149,7 +151,7 @@ def __init__(self, config: "DictConfig"): usd_path=asset_path, prim_path="/World/test_scene" ) - self._rigid_objects = [] + self._rigid_objects = [] # type: ignore self.add_or_reset_rigid_objects() self._pick_target_rigid_object_idx = None @@ -171,7 +173,7 @@ def __init__(self, config: "DictConfig"): self._isaac_rom.post_reset() for agent in self.agents_mgr.articulated_agents_iter: - agent._robot_wrapper.post_reset() + agent._robot_wrapper.post_reset() # type: ignore self.first_setup = True self.ep_info: Optional[RearrangeEpisode] = None @@ -301,8 +303,7 @@ def _get_target_trans(self): """ # Preprocess the ep_info making necessary datatype conversions. target_trans = [] - breakpoint() - rom = self.get_rigid_object_manager() + rom = self.get_rigid_object_manager() for target_handle, trans in self._targets.items(): targ_idx = self._scene_obj_ids.index( rom.get_object_by_handle(target_handle).object_id @@ -398,7 +399,7 @@ def reset(self): self._isaac_rom.post_reset() for agent in self.agents_mgr.articulated_agents_iter: - agent._robot_wrapper.post_reset() + agent._robot_wrapper.post_reset() # type: ignore for i in range(len(self.agents)): self.reset_agent(i) @@ -406,6 +407,7 @@ def reset(self): @add_perf_timing_func() def reconfigure(self, config: "DictConfig", ep_info: RearrangeEpisode): + # TODO: jimmy: the episode here is the wrong episode self._handle_to_goal_name = ep_info.info["object_labels"] self.ep_info = ep_info @@ -439,79 +441,83 @@ def reconfigure(self, config: "DictConfig", ep_info: RearrangeEpisode): # use target self._setup_targets(ep_info) - #breakpoint() - return - if is_hard_reset: - with read_write(config): - config["scene"] = ep_info.scene_id - t_start = time.time() - super().reconfigure(config, should_close_on_new_scene=False) - self.add_perf_timing("super_reconfigure", t_start) - # The articulated object handles have changed. - self._start_art_states = {} - - if new_scene: - self.agents_mgr.on_new_scene() - - self.prev_scene_id = ep_info.scene_id - self._viz_templates = {} - self._viz_handle_to_template = {} - - # Set the default articulated object joint state. - for ao, (set_joint_state, set_T) in self._start_art_states.items(): - ao.clear_joint_states() - ao.joint_positions = set_joint_state - if not is_hard_reset: - # [Andrew Szot 2023-08-22]: If we don't correct for this, some - # articulated objects may "drift" over time when the scene - # reset is skipped. - ao.transformation = set_T - - # Load specified articulated object states from episode config - self._set_ao_states_from_ep(ep_info) - - self.agents_mgr.post_obj_load_reconfigure() - - # add episode clutter objects additional to base scene objects - if self._load_objs: - self._add_objs(ep_info, should_add_objects, new_scene) - self._setup_targets(ep_info) - - self._add_markers(ep_info) - - # auto-sleep rigid objects as optimization - if self._auto_sleep: - self._sleep_all_objects() - - rom = self.get_rigid_object_manager() - self._obj_orig_motion_types = { - handle: ro.motion_type - for handle, ro in rom.get_objects_by_handle_substring().items() - } - - if new_scene: - self._load_navmesh(ep_info) - - # Get the starting positions of the target objects. - scene_pos = self.get_scene_pos() + # Set the target start pos self.target_start_pos = np.array( - [ - scene_pos[ - self._scene_obj_ids.index( - rom.get_object_by_handle(t_handle).object_id - ) - ] - for t_handle, _ in self._targets.items() - ] + [self._targets[key]["base"][0] for key in self._targets] ) - self._draw_bb_objs = [ - rom.get_object_by_handle(obj_handle).object_id - for obj_handle in self._targets - ] - - if self._should_setup_semantic_ids: - self._setup_semantic_ids() + return + # if is_hard_reset: + # with read_write(config): + # config["scene"] = ep_info.scene_id + # t_start = time.time() + # super().reconfigure(config, should_close_on_new_scene=False) + # self.add_perf_timing("super_reconfigure", t_start) + # # The articulated object handles have changed. + # self._start_art_states = {} + + # if new_scene: + # self.agents_mgr.on_new_scene() + + # self.prev_scene_id = ep_info.scene_id + # self._viz_templates = {} + # self._viz_handle_to_template = {} + + # # Set the default articulated object joint state. + # for ao, (set_joint_state, set_T) in self._start_art_states.items(): + # ao.clear_joint_states() + # ao.joint_positions = set_joint_state + # if not is_hard_reset: + # # [Andrew Szot 2023-08-22]: If we don't correct for this, some + # # articulated objects may "drift" over time when the scene + # # reset is skipped. + # ao.transformation = set_T + + # # Load specified articulated object states from episode config + # self._set_ao_states_from_ep(ep_info) + + # self.agents_mgr.post_obj_load_reconfigure() + + # # add episode clutter objects additional to base scene objects + # if self._load_objs: + # self._add_objs(ep_info, should_add_objects, new_scene) + # self._setup_targets(ep_info) + + # self._add_markers(ep_info) + + # # auto-sleep rigid objects as optimization + # if self._auto_sleep: + # self._sleep_all_objects() + + # rom = self.get_rigid_object_manager() + # self._obj_orig_motion_types = { + # handle: ro.motion_type + # for handle, ro in rom.get_objects_by_handle_substring().items() + # } + + # if new_scene: + # self._load_navmesh(ep_info) + + # # Get the starting positions of the target objects. + # scene_pos = self.get_scene_pos() + # self.target_start_pos = np.array( + # [ + # scene_pos[ + # self._scene_obj_ids.index( + # rom.get_object_by_handle(t_handle).object_id + # ) + # ] + # for t_handle, _ in self._targets.items() + # ] + # ) + + # self._draw_bb_objs = [ + # rom.get_object_by_handle(obj_handle).object_id + # for obj_handle in self._targets + # ] + + # if self._should_setup_semantic_ids: + # self._setup_semantic_ids() @add_perf_timing_func() def _setup_semantic_ids(self): @@ -580,12 +586,77 @@ def set_articulated_agent_base_to_random_point( ) return start_pos, start_rot + def get_poses(self, name, pose_type="base"): + """This function returns the target receptacle location, which is used to + replace the function of the ep_info""" + poses = { + # "cabinet": { + # "base_pos": np.array([1.7, 0.1, -0.2]), + # "base_rot": -90, + # "ee_pos": self.env.sim._rigid_objects[0].translation, + # "ee_rot": np.deg2rad([0, 80, -30]), + # }, + # "shelf": { + # "base_pos": np.array([-4.5, 0.1, -3.5]), + # "base_rot": 180, + # "ee_pos": self.env.sim._rigid_objects[0].translation, + # "ee_rot": np.deg2rad([0, 80, -30]), + # }, + "island": { + "base_pos": np.array([-5.3, 0.1, -1.6]), + "base_rot": 0, + "ee_pos": np.array([-4.4, 0.5, -2.0]), + "ee_rot": np.deg2rad([0, 80, -30]), + }, + "oven": { + "base_pos": np.array([-4.75, 0.1, -3.3]), + "base_rot": 180, + "ee_pos": np.array([-5.5, 1.6, -2.7]), + "ee_rot": np.deg2rad([0, 80, -30]), + }, + # "fridge": { + # "base_pos": np.array([-4.4, 0.1, 0.7]), + # "base_rot": 180, + # "ee_pos": np.array([-5.4, 1.4, 1.3]), + # "ee_rot": np.deg2rad([0, 0, 0]), + # }, + "fridge": { + "base_pos": np.array([-4.7, 0.1, 0.8]), + "base_rot": 180, + "ee_pos": np.array([-6.2, 1.2, 2.4]), + "ee_rot": np.deg2rad([120, 0, 0]), + }, + "fridge2": { + "base_pos": np.array([-4.75, 0.1, 1.1]), + "base_rot": 180, + "ee_pos": np.array([-6.3, 1.4, 2.4]), + "ee_rot": np.deg2rad([120, 0, 0]), + }, + "freezer": { + "base_pos": np.array([-4.9, 0.1, 0.7]), + "base_rot": 180, + "ee_pos": np.array([-5.7, 0.5, 1.34531]), + "ee_rot": np.deg2rad([0, 80, -30]), + }, + } + return ( + poses[name][f"{pose_type}_pos"], + poses[name][f"{pose_type}_rot"], + ) + def _setup_targets(self, ep_info): + # TODO: jimmy: a quick hack + # self._targets = {} + # for target_handle, transform in ep_info.targets.items(): + # self._targets[target_handle] = mn.Matrix4( + # [[transform[j][i] for j in range(4)] for i in range(4)] + # ) self._targets = {} - for target_handle, transform in ep_info.targets.items(): - self._targets[target_handle] = mn.Matrix4( - [[transform[j][i] for j in range(4)] for i in range(4)] - ) + for key in ["fridge2"]: + self._targets[key] = { + "base": self.get_poses(key, pose_type="base"), + "ee": self.get_poses(key, pose_type="ee"), + } @add_perf_timing_func() def _load_navmesh(self, ep_info): @@ -704,80 +775,80 @@ def _add_objs( new_scene: bool, ) -> None: return - # Load clutter objects: - rom = self.get_rigid_object_manager() - obj_counts: Dict[str, int] = defaultdict(int) - - self._handle_to_object_id = {} - if should_add_objects: - self._scene_obj_ids = [] - - # Get Object template manager - otm = self.get_object_template_manager() - - for i, (obj_handle, transform) in enumerate(ep_info.rigid_objs): - t_start = time.time() - if should_add_objects: - # Get object path - object_template = otm.get_templates_by_handle_substring( - obj_handle - ) - - # Exit if template is invalid - if not object_template: - raise ValueError( - f"Template not found for object with handle {obj_handle}" - ) - - # Get object path - object_path = list(object_template.keys())[0] - - # Get rigid object from the path - ro = rom.add_object_by_template_handle(object_path) - else: - ro = rom.get_object_by_id(self._scene_obj_ids[i]) - self.add_perf_timing("create_asset", t_start) - - # The saved matrices need to be flipped when reloading. - ro.transformation = mn.Matrix4( - [[transform[j][i] for j in range(4)] for i in range(4)] - ) - ro.angular_velocity = mn.Vector3.zero_init() - ro.linear_velocity = mn.Vector3.zero_init() - - other_obj_handle = ( - obj_handle.split(".")[0] + f"_:{obj_counts[obj_handle]:04d}" - ) - if self._kinematic_mode: - ro.motion_type = habitat_sim.physics.MotionType.KINEMATIC - ro.collidable = False - - if should_add_objects: - self._scene_obj_ids.append(ro.object_id) - rel_idx = self._scene_obj_ids.index(ro.object_id) - self._handle_to_object_id[other_obj_handle] = rel_idx - - if other_obj_handle in self._handle_to_goal_name: - ref_handle = self._handle_to_goal_name[other_obj_handle] - self._handle_to_object_id[ref_handle] = rel_idx - - obj_counts[obj_handle] += 1 - - if new_scene: - self._receptacles = self._create_recep_info( - ep_info.scene_id, list(self._handle_to_object_id.keys()) - ) - - ao_mgr = self.get_articulated_object_manager() - # Make all articulated objects (including the robots) kinematic - for aoi_handle in ao_mgr.get_object_handles(): - ao = ao_mgr.get_object_by_handle(aoi_handle) - if self._kinematic_mode: - ao.motion_type = habitat_sim.physics.MotionType.KINEMATIC - # remove any existing motors when converting to kinematic AO - for motor_id in ao.existing_joint_motor_ids: - ao.remove_joint_motor(motor_id) - self.art_objs.append(ao) + # # Load clutter objects: + # rom = self.get_rigid_object_manager() + # obj_counts: Dict[str, int] = defaultdict(int) + + # self._handle_to_object_id = {} + # if should_add_objects: + # self._scene_obj_ids = [] + + # # Get Object template manager + # otm = self.get_object_template_manager() + + # for i, (obj_handle, transform) in enumerate(ep_info.rigid_objs): + # t_start = time.time() + # if should_add_objects: + # # Get object path + # object_template = otm.get_templates_by_handle_substring( + # obj_handle + # ) + + # # Exit if template is invalid + # if not object_template: + # raise ValueError( + # f"Template not found for object with handle {obj_handle}" + # ) + + # # Get object path + # object_path = list(object_template.keys())[0] + + # # Get rigid object from the path + # ro = rom.add_object_by_template_handle(object_path) + # else: + # ro = rom.get_object_by_id(self._scene_obj_ids[i]) + # self.add_perf_timing("create_asset", t_start) + + # # The saved matrices need to be flipped when reloading. + # ro.transformation = mn.Matrix4( + # [[transform[j][i] for j in range(4)] for i in range(4)] + # ) + # ro.angular_velocity = mn.Vector3.zero_init() + # ro.linear_velocity = mn.Vector3.zero_init() + + # other_obj_handle = ( + # obj_handle.split(".")[0] + f"_:{obj_counts[obj_handle]:04d}" + # ) + # if self._kinematic_mode: + # ro.motion_type = habitat_sim.physics.MotionType.KINEMATIC + # ro.collidable = False + + # if should_add_objects: + # self._scene_obj_ids.append(ro.object_id) + # rel_idx = self._scene_obj_ids.index(ro.object_id) + # self._handle_to_object_id[other_obj_handle] = rel_idx + + # if other_obj_handle in self._handle_to_goal_name: + # ref_handle = self._handle_to_goal_name[other_obj_handle] + # self._handle_to_object_id[ref_handle] = rel_idx + + # obj_counts[obj_handle] += 1 + + # if new_scene: + # self._receptacles = self._create_recep_info( + # ep_info.scene_id, list(self._handle_to_object_id.keys()) + # ) + + # ao_mgr = self.get_articulated_object_manager() + # # Make all articulated objects (including the robots) kinematic + # for aoi_handle in ao_mgr.get_object_handles(): + # ao = ao_mgr.get_object_by_handle(aoi_handle) + # if self._kinematic_mode: + # ao.motion_type = habitat_sim.physics.MotionType.KINEMATIC + # # remove any existing motors when converting to kinematic AO + # for motor_id in ao.existing_joint_motor_ids: + # ao.remove_joint_motor(motor_id) + # self.art_objs.append(ao) def _create_recep_info( self, scene_id: str, ignore_handles: List[str] @@ -1194,94 +1265,94 @@ def add_or_reset_rigid_objects(self): do_add = len(self._rigid_objects) == 0 - # for coffee table - if False: - objects_to_add = [] - object_names = [ - "024_bowl", - "013_apple", - "011_banana", - "010_potted_meat_can", - "077_rubiks_cube", - "036_wood_block", - "004_sugar_box", - ] - next_obj_idx = 0 - sp = 0.25 - for cell_y in range(5): - for cell_x in range(3): - for cell_z in range(3): - offset_vec = mn.Vector3( - cell_x * sp - sp, cell_y * sp, cell_z * sp - sp - ) - objects_to_add.append( - ( - f"{path_to_configs}/{object_names[next_obj_idx]}.object_config.json", - drop_pos + offset_vec, - ) - ) - next_obj_idx = (next_obj_idx + 1) % len(object_names) - - if True: - objects_to_add = [ - ( - f"data/objects/fremont/other/plush4/plush4.object_config.json", - mn.Vector3(2.04542, 0.870047, 0.75122), - ), - ] - # for dining table - # objects_to_add = [ - # ( - # f"{path_to_configs}/024_bowl.object_config.json", - # drop_pos + offset_vec * 0.0 + up_vec * 0.0, - # ), - # # (f"{path_to_configs}/011_banana.object_config.json", drop_pos + offset_vec * 0.01 + up_vec * 0.05), - # ( - # f"{path_to_configs}/013_apple.object_config.json", - # drop_pos + offset_vec * -0.01 + up_vec * 0.05, - # ), - # # (f"{path_to_configs}/011_banana.object_config.json", drop_pos + offset_vec * 0.02 + up_vec * 0.12), - # ( - # f"{path_to_configs}/013_apple.object_config.json", - # drop_pos + offset_vec * 0.01 + up_vec * 0.1, - # ), - # ( - # f"{path_to_configs}/010_potted_meat_can.object_config.json", - # drop_pos + offset_vec * 0.3 + up_vec * 0.0, - # ), - # ( - # f"{path_to_configs}/077_rubiks_cube.object_config.json", - # drop_pos + offset_vec * 0.6 + up_vec * 0.1, - # ), - # ( - # f"{path_to_configs}/036_wood_block.object_config.json", - # drop_pos + offset_vec * 0.6 + up_vec * 0.0, - # ), - # ( - # f"{path_to_configs}/004_sugar_box.object_config.json", - # drop_pos + offset_vec * 0.9, - # ), - # ( - # f"{path_to_configs}/004_sugar_box.object_config.json", - # drop_pos + offset_vec * 1.0, - # ), - # ( - # f"{path_to_configs}/004_sugar_box.object_config.json", - # drop_pos + offset_vec * 1.1, - # ), - # ( - # f"{path_to_configs}/004_sugar_box.object_config.json", - # drop_pos + offset_vec * 0.8, - # ), - # ( - # f"{path_to_configs}/010_potted_meat_can.object_config.json", - # drop_pos + offset_vec * 0.22 + up_vec * 0.0, - # ), - # ( - # f"{path_to_configs}/010_potted_meat_can.object_config.json", - # drop_pos + offset_vec * 0.38 + up_vec * 0.0, - # ), - # ] + # # for coffee table + # if False: + # objects_to_add = [] + # object_names = [ + # "024_bowl", + # "013_apple", + # "011_banana", + # "010_potted_meat_can", + # "077_rubiks_cube", + # "036_wood_block", + # "004_sugar_box", + # ] + # next_obj_idx = 0 + # sp = 0.25 + # for cell_y in range(5): + # for cell_x in range(3): + # for cell_z in range(3): + # offset_vec = mn.Vector3( + # cell_x * sp - sp, cell_y * sp, cell_z * sp - sp + # ) + # objects_to_add.append( + # ( + # f"{path_to_configs}/{object_names[next_obj_idx]}.object_config.json", + # drop_pos + offset_vec, + # ) + # ) + # next_obj_idx = (next_obj_idx + 1) % len(object_names) + + # if True: + objects_to_add = [ + ( + f"data/objects/fremont/other/plush4/plush4.object_config.json", + mn.Vector3(2.04542, 0.870047, 0.75122), + ), + ] + # for dining table + # objects_to_add = [ + # ( + # f"{path_to_configs}/024_bowl.object_config.json", + # drop_pos + offset_vec * 0.0 + up_vec * 0.0, + # ), + # # (f"{path_to_configs}/011_banana.object_config.json", drop_pos + offset_vec * 0.01 + up_vec * 0.05), + # ( + # f"{path_to_configs}/013_apple.object_config.json", + # drop_pos + offset_vec * -0.01 + up_vec * 0.05, + # ), + # # (f"{path_to_configs}/011_banana.object_config.json", drop_pos + offset_vec * 0.02 + up_vec * 0.12), + # ( + # f"{path_to_configs}/013_apple.object_config.json", + # drop_pos + offset_vec * 0.01 + up_vec * 0.1, + # ), + # ( + # f"{path_to_configs}/010_potted_meat_can.object_config.json", + # drop_pos + offset_vec * 0.3 + up_vec * 0.0, + # ), + # ( + # f"{path_to_configs}/077_rubiks_cube.object_config.json", + # drop_pos + offset_vec * 0.6 + up_vec * 0.1, + # ), + # ( + # f"{path_to_configs}/036_wood_block.object_config.json", + # drop_pos + offset_vec * 0.6 + up_vec * 0.0, + # ), + # ( + # f"{path_to_configs}/004_sugar_box.object_config.json", + # drop_pos + offset_vec * 0.9, + # ), + # ( + # f"{path_to_configs}/004_sugar_box.object_config.json", + # drop_pos + offset_vec * 1.0, + # ), + # ( + # f"{path_to_configs}/004_sugar_box.object_config.json", + # drop_pos + offset_vec * 1.1, + # ), + # ( + # f"{path_to_configs}/004_sugar_box.object_config.json", + # drop_pos + offset_vec * 0.8, + # ), + # ( + # f"{path_to_configs}/010_potted_meat_can.object_config.json", + # drop_pos + offset_vec * 0.22 + up_vec * 0.0, + # ), + # ( + # f"{path_to_configs}/010_potted_meat_can.object_config.json", + # drop_pos + offset_vec * 0.38 + up_vec * 0.0, + # ), + # ] from habitat.isaac_sim.isaac_rigid_object_manager import ( IsaacRigidObjectManager, ) diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index 81c232697f..cc061d5ea6 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -76,8 +76,8 @@ def get_observation(self, observations, episode, *args, **kwargs): scene_pos = self._sim.get_scene_pos() pos = scene_pos[idxs] - for i in range(pos.shape[0]): - pos[i] = T_inv.transform_point(pos[i]) + for i in range(pos.shape[0]): # type: ignore + pos[i] = T_inv.transform_point(pos[i]) # type: ignore return pos.reshape(-1) @@ -97,6 +97,7 @@ def get_observation(self, *args, observations, episode, **kwargs): ).articulated_agent.ee_transform() T_inv = global_T.inverted() pos = self._sim.get_target_objs_start() + return batch_transform_point(pos, T_inv, np.float32).reshape(-1) diff --git a/habitat-lab/habitat/tasks/rearrange/sub_tasks/pick_task.py b/habitat-lab/habitat/tasks/rearrange/sub_tasks/pick_task.py index 547a79c7df..d4c6ef91ed 100644 --- a/habitat-lab/habitat/tasks/rearrange/sub_tasks/pick_task.py +++ b/habitat-lab/habitat/tasks/rearrange/sub_tasks/pick_task.py @@ -5,16 +5,17 @@ # LICENSE file in the root directory of this source tree. +import magnum as mn import numpy as np from habitat.core.dataset import Episode from habitat.core.registry import registry from habitat.datasets.rearrange.rearrange_dataset import RearrangeEpisode +from habitat.tasks.rearrange.isaac_rearrange_sim import IsaacRearrangeSim from habitat.tasks.rearrange.rearrange_task import RearrangeTask -from habitat.tasks.rearrange.utils import ( +from habitat.tasks.rearrange.utils import ( # set_agent_base_via_obj_trans, place_agent_at_dist_from_pos, rearrange_logger, - set_agent_base_via_obj_trans, ) @@ -88,11 +89,14 @@ def _gen_start_pos(self, sim, episode, sel_idx): return start_pos, angle_to_obj def _should_prevent_grip(self, action_args): - return ( - self._sim.grasp_mgr.is_grasped - and action_args.get("grip_action", None) is not None - and action_args["grip_action"] < 0 - ) + if type(self._sim) == IsaacRearrangeSim: + return False + else: + return ( + self._sim.grasp_mgr.is_grasped + and action_args.get("grip_action", None) is not None + and action_args["grip_action"] < 0 + ) def step(self, action, episode): action_args = action["action_args"] @@ -105,7 +109,7 @@ def step(self, action, episode): return obs def reset(self, episode: Episode, fetch_observations: bool = True): - sim = self._sim + # sim = self._sim assert isinstance( episode, RearrangeEpisode @@ -115,14 +119,27 @@ def reset(self, episode: Episode, fetch_observations: bool = True): self.prev_colls = 0 - sel_idx = self._sample_idx(sim) - start_pos, start_rot = self._gen_start_pos(sim, episode, sel_idx) + # TODO: jimmy: do not need to go through the process since we + # alreay hardcode the location of the furniture + # sel_idx = self._sample_idx(sim) + # start_pos, start_rot = self._gen_start_pos(sim, episode, sel_idx) + + # set_agent_base_via_obj_trans( + # start_pos, start_rot, sim.articulated_agent + # ) + + # Hardcode the target object name + sel_idx = "fridge2" + start_pos, start_rot = self._sim._targets[sel_idx]["base"] - set_agent_base_via_obj_trans( - start_pos, start_rot, sim.articulated_agent + position = mn.Vector3(start_pos) + rotation = mn.Quaternion.rotation( + mn.Deg(start_rot), mn.Vector3.y_axis() ) + self.base_trans = mn.Matrix4.from_(rotation.to_matrix(), position) + self._sim.articulated_agent.base_transformation = self.base_trans - self._targ_idx = sel_idx + self._targ_idx = sel_idx # type: ignore if fetch_observations: self._sim.maybe_update_articulated_agent() From 378daff1998b57ce4311dd0ec216fce995a37236 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 11:21:40 -0400 Subject: [PATCH 20/50] add measurements of end_effector_to_object_distance and num_steps --- .../config/benchmark/rearrange/play/pick_murp.yaml | 7 +++++-- .../habitat/tasks/rearrange/rearrange_sensors.py | 11 +++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index ca4e966869..6ca563c385 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -10,10 +10,12 @@ defaults: - /habitat/task/rearrange/actions: fetch_suction_arm_base_empty - /habitat/task/measurements: #- articulated_agent_force # TODO: jimmy: remove this since AttributeError: 'IsaacAgentData' object has no attribute 'grasp_mgr - - zero + #- zero + - end_effector_to_object_distance + - num_steps - /habitat/task/lab_sensors: - joint_sensor - - target_start_sensor + - target_start_sensor # Relative position from end effector to target object - end_effector_sensor - /habitat/dataset/rearrangement: replica_cad @@ -25,6 +27,7 @@ habitat: obs_keys: - articulated_agent_arm_depth - joint + - obj_start_sensor # Relative position from end effector to target object task: # Config for empty task to explore the scene. type: RearrangePickTask-v0 diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index cc061d5ea6..e374b952a9 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -15,6 +15,7 @@ from habitat.core.registry import registry from habitat.core.simulator import Sensor, SensorTypes from habitat.tasks.nav.nav import PointGoalSensor +from habitat.tasks.rearrange.isaac_rearrange_sim import IsaacRearrangeSim from habitat.tasks.rearrange.rearrange_sim import RearrangeSim from habitat.tasks.rearrange.utils import ( CollisionDetails, @@ -638,10 +639,12 @@ def update_metric(self, *args, episode, **kwargs): .articulated_agent.ee_transform() .translation ) - - idxs, _ = self._sim.get_targets() - scene_pos = self._sim.get_scene_pos() - target_pos = scene_pos[idxs] + if type(self._sim) == IsaacRearrangeSim: + target_pos = self._sim.target_start_pos + else: + idxs, _ = self._sim.get_targets() + scene_pos = self._sim.get_scene_pos() + target_pos = scene_pos[idxs] distances = np.linalg.norm(target_pos - ee_pos, ord=2, axis=-1) From 9c9e5e57abc05568c4424522382b09fb00234086 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 11:37:56 -0400 Subject: [PATCH 21/50] add hand joint sensor --- .../benchmark/rearrange/play/pick_murp.yaml | 2 ++ .../config/default_structured_configs.py | 16 +++++++++++ .../tasks/rearrange/rearrange_sensors.py | 27 +++++++++++++++++++ 3 files changed, 45 insertions(+) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index 6ca563c385..8493b35b5b 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -15,6 +15,7 @@ defaults: - num_steps - /habitat/task/lab_sensors: - joint_sensor + - hand_joint_sensor - target_start_sensor # Relative position from end effector to target object - end_effector_sensor @@ -27,6 +28,7 @@ habitat: obs_keys: - articulated_agent_arm_depth - joint + - hand_joint - obj_start_sensor # Relative position from end effector to target object task: # Config for empty task to explore the scene. diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index 7039b2d44c..b69083a6d3 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -60,6 +60,7 @@ "IsHoldingSensorConfig", "EEPositionSensorConfig", "JointSensorConfig", + "HandJointSensorConfig", "HumanoidJointSensorConfig", "TargetStartSensorConfig", "GoalSensorConfig", @@ -537,6 +538,15 @@ class JointSensorConfig(LabSensorConfig): arm_joint_mask: Optional[List[int]] = None +@dataclass +class HandJointSensorConfig(LabSensorConfig): + r""" + Rearrangement only. Returns the hand joint positions of the robot. + """ + type: str = "HandJointSensor" + dimensionality: int = 16 + + @dataclass class HumanoidJointSensorConfig(LabSensorConfig): r""" @@ -2261,6 +2271,12 @@ class HabitatConfig(HabitatBaseConfig): name="joint_sensor", node=JointSensorConfig, ) +cs.store( + package="habitat.task.lab_sensors.hand_joint_sensor", + group="habitat/task/lab_sensors", + name="hand_joint_sensor", + node=HandJointSensorConfig, +) cs.store( package="habitat.task.lab_sensors.humanoid_joint_sensor", group="habitat/task/lab_sensors", diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index e374b952a9..5967eb0356 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -245,6 +245,33 @@ def get_observation(self, observations, episode, *args, **kwargs): return np.array(joints_pos, dtype=np.float32) +@registry.register_sensor +class HandJointSensor(UsesArticulatedAgentInterface, Sensor): + def __init__(self, sim, config, *args, **kwargs): + super().__init__(config=config) + self._sim = sim + + def _get_uuid(self, *args, **kwargs): + return "hand_joint" + + def _get_sensor_type(self, *args, **kwargs): + return SensorTypes.TENSOR + + def _get_observation_space(self, *args, config, **kwargs): + return spaces.Box( + shape=(config.dimensionality,), + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + dtype=np.float32, + ) + + def get_observation(self, observations, episode, *args, **kwargs): + joints_pos = self._sim.get_agent_data( + self.agent_id + ).articulated_agent._robot_wrapper.hand_joint_pos + return np.array(joints_pos, dtype=np.float32) + + @registry.register_sensor class HumanoidJointSensor(UsesArticulatedAgentInterface, Sensor): def __init__(self, sim, config, *args, **kwargs): From 20477e711ceaa7fbe8d329c242a4e4b28674140b Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 12:51:31 -0400 Subject: [PATCH 22/50] add arj object reward and success --- .../benchmark/rearrange/play/pick_murp.yaml | 19 +++- .../config/default_structured_configs.py | 2 + .../actions/murp_base_arm_empty.yaml | 1 + .../tasks/rearrange/rearrange_sensors.py | 91 +++++++++++------- .../sub_tasks/articulated_object_sensors.py | 96 +++++++++++++++---- 5 files changed, 155 insertions(+), 54 deletions(-) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index 8493b35b5b..7b9b65c612 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -9,15 +9,24 @@ defaults: - /habitat/task: task_config_base - /habitat/task/rearrange/actions: fetch_suction_arm_base_empty - /habitat/task/measurements: - #- articulated_agent_force # TODO: jimmy: remove this since AttributeError: 'IsaacAgentData' object has no attribute 'grasp_mgr + - articulated_agent_force # TODO: jimmy: remove this since AttributeError: 'IsaacAgentData' object has no attribute 'grasp_mgr + - articulated_agent_colls # TODO: jimmy: the underlying implementation is a hack + - force_terminate # TODO: jimmy: the underlying implementation is a hack #- zero - end_effector_to_object_distance + - end_effector_to_rest_distance - num_steps + - art_obj_state + - art_obj_at_desired_state + - does_want_terminate + - art_obj_success + - art_obj_reward - /habitat/task/lab_sensors: - joint_sensor - hand_joint_sensor - target_start_sensor # Relative position from end effector to target object - end_effector_sensor + - relative_resting_pos_sensor - /habitat/dataset/rearrangement: replica_cad - _self_ @@ -30,13 +39,17 @@ habitat: - joint - hand_joint - obj_start_sensor # Relative position from end effector to target object + - relative_resting_position task: # Config for empty task to explore the scene. type: RearrangePickTask-v0 count_obj_collisions: True desired_resting_position: [0.5, 0.0, 1.0] - reward_measure: "zero" - success_measure: "zero" + # reward_measure: "zero" # default measure for the play yaml + # success_measure: "zero" # default measure for the play yaml + + reward_measure: art_obj_reward + success_measure: art_obj_success # Reach task config render_target: True diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index b69083a6d3..ed8c9663da 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -921,6 +921,7 @@ class ArtObjAtDesiredStateMeasurementConfig(MeasurementConfig): type: str = "ArtObjAtDesiredState" use_absolute_distance: bool = True success_dist_threshold: float = 0.05 + success_js_state: float = 3.14 @dataclass @@ -977,6 +978,7 @@ class ArtObjRewardMeasurementConfig(MeasurementConfig): count_coll_pen: float = -1.0 max_count_colls: int = -1 count_coll_end_pen: float = 1.0 + success_js_state: float = 3.14 @dataclass diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml index 285f44bb80..80fbd991ce 100644 --- a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml @@ -4,6 +4,7 @@ defaults: - base_vel_isaac # BaseVelIsaacAction - /habitat/task/actions: - arm_reach_ee + - rearrange_stop - empty - _self_ # arm_action: diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index 5967eb0356..e02840f462 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -833,14 +833,23 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): ) def update_metric(self, *args, episode, task, observations, **kwargs): - cur_coll_info = self._task.get_cur_collision_info(self.agent_id) - self._accum_coll_info += cur_coll_info - self._metric = { - "total_collisions": self._accum_coll_info.total_collisions, - "robot_obj_colls": self._accum_coll_info.robot_obj_colls, - "robot_scene_colls": self._accum_coll_info.robot_scene_colls, - "obj_scene_colls": self._accum_coll_info.obj_scene_colls, - } + # TODO: jimmy: temp hack + if type(task._sim) == IsaacRearrangeSim: + self._metric = { + "total_collisions": self._accum_coll_info.total_collisions, + "robot_obj_colls": self._accum_coll_info.robot_obj_colls, + "robot_scene_colls": self._accum_coll_info.robot_scene_colls, + "obj_scene_colls": self._accum_coll_info.obj_scene_colls, + } + else: + cur_coll_info = self._task.get_cur_collision_info(self.agent_id) + self._accum_coll_info += cur_coll_info + self._metric = { + "total_collisions": self._accum_coll_info.total_collisions, + "robot_obj_colls": self._accum_coll_info.robot_obj_colls, + "robot_scene_colls": self._accum_coll_info.robot_scene_colls, + "obj_scene_colls": self._accum_coll_info.obj_scene_colls, + } @registry.register_measure @@ -881,32 +890,41 @@ def add_force(self): return self._add_force def update_metric(self, *args, episode, task, observations, **kwargs): - articulated_agent_force, _, overall_force = self._task.get_coll_forces( - self.agent_id - ) - - if self._count_obj_collisions: - self._cur_force = overall_force + if type(task._sim) == IsaacRearrangeSim: + # TODO: jimmy: temp hack + self._metric = { + "accum": 0.0, + "instant": 0.0, + } else: - self._cur_force = articulated_agent_force + ( + articulated_agent_force, + _, + overall_force, + ) = self._task.get_coll_forces(self.agent_id) - if self._prev_force is not None: - self._add_force = self._cur_force - self._prev_force - if self._add_force > self._min_force: - self._accum_force += self._add_force - self._prev_force = self._cur_force - elif self._add_force < 0.0: - self._prev_force = self._cur_force + if self._count_obj_collisions: + self._cur_force = overall_force else: + self._cur_force = articulated_agent_force + + if self._prev_force is not None: + self._add_force = self._cur_force - self._prev_force + if self._add_force > self._min_force: + self._accum_force += self._add_force + self._prev_force = self._cur_force + elif self._add_force < 0.0: + self._prev_force = self._cur_force + else: + self._add_force = 0.0 + else: + self._prev_force = self._cur_force self._add_force = 0.0 - else: - self._prev_force = self._cur_force - self._add_force = 0.0 - self._metric = { - "accum": self._accum_force, - "instant": self._cur_force, - } + self._metric = { + "accum": self._accum_force, + "instant": self._cur_force, + } @registry.register_measure @@ -1073,16 +1091,23 @@ def update_metric(self, *args, episode, task, observations, **kwargs): reward = 0.0 # For force collision reward (userful for dynamic simulation) - reward += self._get_coll_reward() + # TODO: jimmy: temp check + if type(task._sim) == IsaacRearrangeSim: + reward += 0.0 + else: + reward += self._get_coll_reward() # For count-based collision reward and termination (userful for kinematic simulation) if self._want_count_coll(): reward += self._get_count_coll_reward() # For hold constraint violation - if self._sim.get_agent_data( - self.agent_id - ).grasp_mgr.is_violating_hold_constraint(): + if ( + type(task._sim) != IsaacRearrangeSim + and self._sim.get_agent_data( + self.agent_id + ).grasp_mgr.is_violating_hold_constraint() + ): reward -= self._config.constraint_violate_pen # For force termination diff --git a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py index 5833479373..9d3391a6a8 100644 --- a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py @@ -7,12 +7,15 @@ import numpy as np from gym import spaces +from scipy.spatial.transform import Rotation as R from habitat.core.embodied_task import Measure from habitat.core.registry import registry from habitat.core.simulator import Sensor, SensorTypes +from habitat.tasks.rearrange.isaac_rearrange_sim import IsaacRearrangeSim from habitat.tasks.rearrange.rearrange_sensors import ( DoesWantTerminate, + EndEffectorToObjectDistance, EndEffectorToRestDistance, RearrangeReward, ) @@ -22,6 +25,37 @@ ) +def apply_rotation(quat_door): + hab_T_door = R.from_quat(quat_door) + isaac_T_hab_list = [-90, 0, 0] + isaac_T_hab = R.from_euler("xyz", isaac_T_hab_list, degrees=True) + isaac_T_door_mat = R.from_matrix( + isaac_T_hab.as_matrix() @ hab_T_door.as_matrix() + ) + isaac_T_door_quat = isaac_T_door_mat.as_quat() + return isaac_T_door_quat + + +def get_door_quat(task): + ( + _, + door_orientation_rpy, + ) = task._sim.articulated_agent._robot_wrapper.get_prim_transform( + "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor2" + ) + # self.visualize_pos(door_trans, "door") + quat_door = door_orientation_rpy.GetQuaternion() + # Getting Quaternion Val to Array + scalar = quat_door.GetReal() + vector = quat_door.GetImaginary() + quat_door = [scalar, vector[0], vector[1], vector[2]] + isaac_T_door_quat = apply_rotation(quat_door) + door_orienation_quat_R = R.from_quat(isaac_T_door_quat) + door_orientation_rpy = door_orienation_quat_R.as_euler("xyz", degrees=True) + + return door_orientation_rpy + + @registry.register_sensor class MarkerRelPosSensor(UsesArticulatedAgentInterface, Sensor): """ @@ -143,11 +177,16 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): episode=episode, task=task, observations=observations, - **kwargs + **kwargs, ) def update_metric(self, *args, episode, task, observations, **kwargs): - self._metric = task.get_use_marker().get_targ_js() + if type(task._sim) == IsaacRearrangeSim: + rpy = get_door_quat(task) + print(f"current door rpy: {rpy}") + self._metric = rpy[0] + else: + self._metric = task.get_use_marker().get_targ_js() @registry.register_measure @@ -168,11 +207,14 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): episode=episode, task=task, observations=observations, - **kwargs + **kwargs, ) def update_metric(self, *args, episode, task, observations, **kwargs): - dist = task.success_js_state - task.get_use_marker().get_targ_js() + if type(task._sim) == IsaacRearrangeSim: + dist = self._config.success_js_state - get_door_quat(task)[0] + else: + dist = task.success_js_state - task.get_use_marker().get_targ_js() # If not absolute distance, we can have a joint state greater than the # target. @@ -205,7 +247,7 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): episode=episode, task=task, observations=observations, - **kwargs + **kwargs, ) def update_metric(self, *args, episode, task, observations, **kwargs): @@ -252,7 +294,7 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): episode=episode, task=task, observations=observations, - **kwargs + **kwargs, ) def update_metric(self, *args, task, **kwargs): @@ -298,16 +340,22 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): ArtObjState.cls_uuid ].get_metric() - dist_to_marker = task.measurements.measures[ - EndEffectorDistToMarker.cls_uuid - ].get_metric() + if type(task._sim) == IsaacRearrangeSim: + dist_to_marker = task.measurements.measures[ + EndEffectorToObjectDistance.cls_uuid + ].get_metric()["0"] + else: + dist_to_marker = task.measurements.measures[ + EndEffectorDistToMarker.cls_uuid + ].get_metric() ee_to_rest_distance = task.measurements.measures[ EndEffectorToRestDistance.cls_uuid ].get_metric() self._prev_art_state = link_state - self._any_has_grasped = task._sim.grasp_mgr.is_grasped + # TODO: jimmy: havre to implement grasping logics + self._any_has_grasped = False # task._sim.grasp_mgr.is_grasped self._prev_ee_dist_to_marker = dist_to_marker self._prev_ee_to_rest = ee_to_rest_distance self._any_at_desired_state = False @@ -316,7 +364,7 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): episode=episode, task=task, observations=observations, - **kwargs + **kwargs, ) def update_metric(self, *args, episode, task, observations, **kwargs): @@ -325,7 +373,7 @@ def update_metric(self, *args, episode, task, observations, **kwargs): episode=episode, task=task, observations=observations, - **kwargs + **kwargs, ) reward = self._metric link_state = task.measurements.measures[ @@ -340,19 +388,31 @@ def update_metric(self, *args, episode, task, observations, **kwargs): ArtObjAtDesiredState.cls_uuid ].get_metric() - cur_dist = abs(link_state - task.success_js_state) - prev_dist = abs(self._prev_art_state - task.success_js_state) + if type(task._sim) == IsaacRearrangeSim: + cur_dist = abs(link_state - self._config.success_js_state) + prev_dist = abs( + self._prev_art_state - self._config.success_js_state + ) + else: + cur_dist = abs(link_state - task.success_js_state) + prev_dist = abs(self._prev_art_state - task.success_js_state) # Dense reward to the target articulated object state. dist_diff = prev_dist - cur_dist if not is_art_obj_state_succ: reward += self._config.art_dist_reward * dist_diff - cur_has_grasped = task._sim.grasp_mgr.is_grasped + # TODO: jimmy: havre to implement grasping logics + cur_has_grasped = False # task._sim.grasp_mgr.is_grasped - cur_ee_dist_to_marker = task.measurements.measures[ - EndEffectorDistToMarker.cls_uuid - ].get_metric() + if type(task._sim) == IsaacRearrangeSim: + cur_ee_dist_to_marker = task.measurements.measures[ + EndEffectorToObjectDistance.cls_uuid + ].get_metric()["0"] + else: + cur_ee_dist_to_marker = task.measurements.measures[ + EndEffectorDistToMarker.cls_uuid + ].get_metric() if cur_has_grasped and not self._any_has_grasped: if task._sim.grasp_mgr.snapped_marker_id != task.use_marker_name: # Grasped wrong marker From 4c470629f93352f42a9a3d24e314f73a4fe97a72 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 13:42:45 -0400 Subject: [PATCH 23/50] remove empty action --- .../habitat/task/rearrange/actions/murp_base_arm_empty.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml index 80fbd991ce..6d7fd1ee13 100644 --- a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml @@ -5,7 +5,7 @@ defaults: - /habitat/task/actions: - arm_reach_ee - rearrange_stop - - empty + # - empty # remove empty action - _self_ # arm_action: # type: "ArmAction" From fa1f40554212cb45e7951f549afa03dd2973ff0b Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 14:26:19 -0400 Subject: [PATCH 24/50] fix right and left hands --- .../config/default_structured_configs.py | 5 +++ .../isaac_sim/_internal/murp_robot_wrapper.py | 44 ++++++++++++------- .../habitat/isaac_sim/isaac_murp_robot.py | 4 +- .../habitat_simulator/habitat_simulator.py | 1 + .../tasks/rearrange/actions/actions.py | 14 +++++- .../tasks/rearrange/rearrange_sensors.py | 22 +++++++--- 6 files changed, 65 insertions(+), 25 deletions(-) diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index ed8c9663da..26daa2d4b6 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -288,6 +288,7 @@ class ArmReachEEActionConfig(ActionConfig): In Rearrangement only for the non cylinder shape of the robot. Corresponds to the base velocity. Contains two continuous actions, the first one controls forward and backward motion, the second the rotation. """ type: str = "ArmReachEEAction" + right_left_hand: str = "right" @dataclass @@ -536,6 +537,7 @@ class JointSensorConfig(LabSensorConfig): type: str = "JointSensor" dimensionality: int = 7 arm_joint_mask: Optional[List[int]] = None + right_left_hand: str = "right" @dataclass @@ -545,6 +547,7 @@ class HandJointSensorConfig(LabSensorConfig): """ type: str = "HandJointSensor" dimensionality: int = 16 + right_left_hand: str = "right" @dataclass @@ -1693,6 +1696,8 @@ class AgentConfig(HabitatBaseConfig): ik_arm_urdf: Optional[str] = None # File to motion data, used to play pre-recorded motions motion_data_path: str = "" + # For murp + right_left_hand: str = "right" @dataclass diff --git a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py index 1794095a2d..169f3c9b2a 100644 --- a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py +++ b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py @@ -2,24 +2,27 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. +import os + import magnum as mn import numpy as np import omni -import omni.physx.scripts.utils as physxUtils # todo: add guard to ensure SimulatorApp is created, or give nice error message, so we don't get weird import errors here -from omni.isaac.core import World -from omni.isaac.core.objects import DynamicCuboid -from omni.isaac.core.prims.rigid_prim import RigidPrim -from omni.isaac.core.prims.rigid_prim_view import RigidPrimView +# from omni.isaac.core import World +# from omni.isaac.core.objects import DynamicCuboid +# from omni.isaac.core.prims.rigid_prim import RigidPrim +# from omni.isaac.core.prims.rigid_prim_view import RigidPrimView from omni.isaac.core.robots import Robot from omni.isaac.core.utils.stage import add_reference_to_stage from omni.isaac.core.utils.types import ArticulationAction -from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics +from pxr import PhysxSchema, Usd, UsdPhysics # Sdf UsdGeom from scipy.spatial.transform import Rotation as R from habitat.isaac_sim import isaac_prim_utils +# import omni.physx.scripts.utils as physxUtils + class MurpRobotWrapper: """Isaac-internal wrapper for a robot. @@ -27,11 +30,12 @@ class MurpRobotWrapper: The goal with this wrapper is convenience but not encapsulation. See also (public) IsaacMobileManipulator, which has the goal of exposing a minimal public interface to the rest of Habitat-lab. """ - def __init__(self, isaac_service, instance_id=0): + def __init__(self, isaac_service, instance_id=0, right_left_hand="right"): self._isaac_service = isaac_service asset_path = "./data/usd/robots/franka_with_hand_2.usda" # Lambda Machine Change robot_prim_path = f"/World/env_{instance_id}/Murp" self._robot_prim_path = robot_prim_path + self._right_left_hand = right_left_hand add_reference_to_stage(usd_path=asset_path, prim_path=robot_prim_path) self._isaac_service.usd_visualizer.on_add_reference_to_stage( @@ -288,7 +292,7 @@ def reset_hand(self): self._right_hand_joint_indices = np.array(right_hand_joint_indices) n_hand_joints = len(left_hand_joint_names) - closed_positions = np.array([3.14159] * n_hand_joints) + # closed_positions = np.array([3.14159] * n_hand_joints) open_positions = np.zeros(n_hand_joints) self._target_hand_joint_positions = open_positions self._target_right_hand_joint_positions = open_positions @@ -519,8 +523,12 @@ def ee_pose(self, convention="hab"): """Get the current ee position and rotation.""" link_poses = self.get_link_world_poses(convention=convention) - ee_pos = link_poses[0][self.ee_link_id] - ee_rot = link_poses[1][self.ee_link_id] + if self._right_left_hand == "right": + ee_pos = link_poses[0][self.right_ee_link_id] + ee_rot = link_poses[1][self.right_ee_link_id] + else: + ee_pos = link_poses[0][self.ee_link_id] + ee_rot = link_poses[1][self.ee_link_id] return ee_pos, ee_rot @@ -533,11 +541,15 @@ def get_prim_transform(self, asset_path=None): ) prim_path = f"/World/test_scene/{asset_path}" prim = self._isaac_service.world.stage.GetPrimAtPath(prim_path) - matrix: Gf.Matrix4d = omni.usd.get_world_transform_matrix(prim) - translate: Gf.Vec3d = matrix.ExtractTranslation() - rotation: Gf.Rotation = matrix.ExtractRotation() - quat_rotation: Gf.Quatd = matrix.ExtractRotationQuat() - euler_rotation = rotation.GetAngle() + matrix: Gf.Matrix4d = omni.usd.get_world_transform_matrix( # type: ignore # noqa: F821 + prim + ) # type: ignore + translate: Gf.Vec3d = ( # type: ignore # noqa: F821 + matrix.ExtractTranslation() + ) # type: ignore + rotation: Gf.Rotation = matrix.ExtractRotation() # type: ignore # noqa: F821 + # quat_rotation: Gf.Quatd = matrix.ExtractRotationQuat() # noqa: F821 + # euler_rotation = rotation.GetAngle() return list(translate), rotation @@ -550,7 +562,7 @@ def get_articulation_links(self, prim_path: str): for child_prim in prim.GetChildren(): if UsdPhysics.ArticulationRootAPI.CanApply(child_prim): - articulation_api = UsdPhysics.ArticulationRootAPI(child_prim) + # articulation_api = UsdPhysics.ArticulationRootAPI(child_prim) link_names = [] diff --git a/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py index ab85e3645b..3bb3dae183 100644 --- a/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py +++ b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py @@ -111,7 +111,9 @@ def __init__(self, agent_cfg, isaac_service, sim=None): -1.3962275, ] robot_wrapper = MurpRobotWrapper( - isaac_service=isaac_service, instance_id=0 + isaac_service=isaac_service, + instance_id=0, + right_left_hand=agent_cfg.right_left_hand, ) super().__init__( murp_params, agent_cfg, isaac_service, robot_wrapper, sim=sim diff --git a/habitat-lab/habitat/sims/habitat_simulator/habitat_simulator.py b/habitat-lab/habitat/sims/habitat_simulator/habitat_simulator.py index 1f5a863945..bbf716d574 100644 --- a/habitat-lab/habitat/sims/habitat_simulator/habitat_simulator.py +++ b/habitat-lab/habitat/sims/habitat_simulator/habitat_simulator.py @@ -348,6 +348,7 @@ def create_sim_config( "max_climb", "max_slope", "joint_start_override", + "right_left_hand", }, ) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index fdf5ddcab9..f418e214a6 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -969,5 +969,15 @@ def step(self, *args, **kwargs): des_joint_pos = self.apply_joint_limits(des_joint_pos) print(f"des_joint_pos: {des_joint_pos}") print(f"self.target_finger: {self.target_finger}") - self._robot_wrapper._target_arm_joint_positions = des_joint_pos - self._robot_wrapper._target_hand_joint_positions = self.target_finger + if self._config.right_left_hand == "right": + self._robot_wrapper._target_right_arm_joint_positions = ( + des_joint_pos + ) + self._robot_wrapper._target_right_hand_joint_positions = ( + self.target_finger + ) + else: + self._robot_wrapper._target_arm_joint_positions = des_joint_pos + self._robot_wrapper._target_hand_joint_positions = ( + self.target_finger + ) diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index e02840f462..a1c16bd5d9 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -237,9 +237,14 @@ def _get_mask_joint(self, joints_pos): return mask_joints_pos def get_observation(self, observations, episode, *args, **kwargs): - joints_pos = self._sim.get_agent_data( - self.agent_id - ).articulated_agent.arm_joint_pos + if self.config.right_left_hand == "right": + joints_pos = self._sim.get_agent_data( + self.agent_id + ).articulated_agent._robot_wrapper.right_arm_joint_pos + else: + joints_pos = self._sim.get_agent_data( + self.agent_id + ).articulated_agent._robot_wrapper.arm_joint_pos if self._arm_joint_mask is not None: joints_pos = self._get_mask_joint(joints_pos) return np.array(joints_pos, dtype=np.float32) @@ -266,9 +271,14 @@ def _get_observation_space(self, *args, config, **kwargs): ) def get_observation(self, observations, episode, *args, **kwargs): - joints_pos = self._sim.get_agent_data( - self.agent_id - ).articulated_agent._robot_wrapper.hand_joint_pos + if self.config.right_left_hand == "right": + joints_pos = self._sim.get_agent_data( + self.agent_id + ).articulated_agent._robot_wrapper.right_hand_joint_pos + else: + joints_pos = self._sim.get_agent_data( + self.agent_id + ).articulated_agent._robot_wrapper.hand_joint_pos return np.array(joints_pos, dtype=np.float32) From 38c6466e13d54cb146022d3ceb14f9e6955f54f6 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 14:53:32 -0400 Subject: [PATCH 25/50] fix uncontrol arm --- .../tasks/rearrange/actions/actions.py | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index f418e214a6..64526590f6 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -948,6 +948,54 @@ def apply_joint_limits(self, des_joint_pos): des_joint_pos, murp_joint_limits_lower, murp_joint_limits_upper ) + def get_arm_mode(self, name): + arm_joints = { + "rest": np.zeros(7), + "side": np.array( + [ + 2.6116285, + 1.5283098, + 1.0930868, + -0.50559217, + 0.48147443, + 2.628784, + -1.3962275, + ] + ), + } + return arm_joints[name] + + def get_grasp_mode(self, name): + # num_hand_joints = 10 + num_hand_joints = 16 + grasp_joints = { + "open": np.zeros(num_hand_joints), + "pre_grasp": np.concatenate( + (np.full(12, 0.7), [-0.785], np.full(3, 0.7)) + ), + "close": np.concatenate((np.full(12, 0.90), np.zeros(4))), + "close_thumb": np.concatenate( + (np.full(12, 0.90), np.full(4, 0.4)) + ), + } + return grasp_joints[name] + + def fix_arm(self, fix_right_left="left"): + if fix_right_left == "left": + self._robot_wrapper._target_arm_joint_positions = ( + self.get_arm_mode("rest") + ) + self._robot_wrapper._target_hand_joint_positions = ( + self.get_grasp_mode("open") + ) + else: + self._robot_wrapper._target_right_arm_joint_positions = ( + self.get_arm_mode("rest") + ) + self._robot_wrapper._target_right_hand_joint_positions = ( + self.get_grasp_mode("open") + ) + def step(self, *args, **kwargs): target_pos = kwargs[self._action_arg_prefix + "target_pos"] target_rot = kwargs[self._action_arg_prefix + "target_rot"] @@ -976,8 +1024,10 @@ def step(self, *args, **kwargs): self._robot_wrapper._target_right_hand_joint_positions = ( self.target_finger ) + self.fix_arm("left") else: self._robot_wrapper._target_arm_joint_positions = des_joint_pos self._robot_wrapper._target_hand_joint_positions = ( self.target_finger ) + self.fix_arm("right") From 5be7b6743965387be3b1accaa486dcb2cc632900 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 14:57:37 -0400 Subject: [PATCH 26/50] fix right left bug for the action --- .../tasks/rearrange/actions/actions.py | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 64526590f6..0f6df2fa45 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -911,23 +911,38 @@ def action_space(self): def reset(self, *args, **kwargs): try: - self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( - np.array( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos + if self._config.right_left_hand == "right": + self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( + np.array( + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + ) + ) + self.target_finger = ( + self._robot_wrapper._target_right_hand_joint_positions + ) + else: + self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( + np.array( + self._sim.articulated_agent._robot_wrapper.arm_joint_pos + ) + ) + self.target_finger = ( + self._robot_wrapper._target_hand_joint_positions ) - ) - self.target_finger = ( - self._robot_wrapper._target_hand_joint_positions - ) except: self.ee_target = None self.ee_rot_target = None self.target_finger = None def calc_desired_joints(self): - joint_pos = np.array( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos - ) + if self._config.right_left_hand == "right": + joint_pos = np.array( + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + ) + else: + joint_pos = np.array( + self._sim.articulated_agent._robot_wrapper.arm_joint_pos + ) joint_vel = np.zeros(joint_pos.shape) self._ik_helper.set_arm_state(joint_pos, joint_vel) From 0ac10c1c605aabdbf148b207076da30bcafe5bbb Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 15:10:35 -0400 Subject: [PATCH 27/50] adjust the third camera location --- .../habitat/articulated_agents/robots/murp_robot.py | 9 +++++---- habitat-lab/habitat/tasks/rearrange/actions/actions.py | 1 + 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py index 24b8656e09..e697bc1892 100644 --- a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -82,12 +82,12 @@ def _get_murp_params(cls): arm_joints=[0, 2, 4, 6, 8, 10, 12], # remove 0, 10 gripper_joints=[19], arm_init_params=[ - 2.6116285, # for 0 + 2.6116285, # for 0 1.5283098, 1.0930868, -0.50559217, 0.48147443, - 2.628784, # for 10 + 2.628784, # for 10 -1.3962275, ], gripper_init_params=[-1.56], @@ -132,8 +132,9 @@ def _get_murp_params(cls): attached_link_id=-1, ), "third": ArticulatedAgentCameraParams( - cam_offset_pos=mn.Vector3(0.5, 1.9, 0.0), - cam_look_at_pos=mn.Vector3(1, 0.0, -0.75), + cam_offset_pos=mn.Vector3(0.5, 2.5, 0.0), + # cam_look_at_pos=mn.Vector3(1, 0.0, -0.75), + cam_look_at_pos=mn.Vector3(1, 0.0, 0), attached_link_id=-1, ), }, diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 0f6df2fa45..08a1080958 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -1040,6 +1040,7 @@ def step(self, *args, **kwargs): self.target_finger ) self.fix_arm("left") + print("control right, fix left") else: self._robot_wrapper._target_arm_joint_positions = des_joint_pos self._robot_wrapper._target_hand_joint_positions = ( From b11bbfee031df1a83ccf38fe8f4478b845d59478 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 15:30:50 -0400 Subject: [PATCH 28/50] update the script for the heuriscti pick --- heuristic_expert_w_grasp.py | 163 +++++++++++++++++++----------------- 1 file changed, 86 insertions(+), 77 deletions(-) diff --git a/heuristic_expert_w_grasp.py b/heuristic_expert_w_grasp.py index 00e67b9b0d..9f3260558e 100644 --- a/heuristic_expert_w_grasp.py +++ b/heuristic_expert_w_grasp.py @@ -49,7 +49,7 @@ from habitat_sim.utils.settings import make_cfg from viz_utils import add_text_to_image -user = "joanne" +user = " " if user == "joanne": data_path = "/fsx-siro/jtruong/repos/vla-physics/habitat-lab/data/" else: @@ -131,18 +131,6 @@ def __init__(self, target_name="cabinet", skill="pick", replay=False): self.replay = replay main_agent_config = AgentConfig() -<<<<<<< HEAD - urdf_path = os.path.join( - data_path, - "franka_tmr/franka_description_tmr/urdf/franka_with_hand_2.urdf", # Lambda Change - ) - arm_urdf_path = os.path.join( - data_path, - # "hab_murp/murp_tmr_franka/murp_tmr_franka_metahand_left_arm_obj.urdf", - "franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf", # Lambda Change - # "franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf", - ) -======= if user == "joanne": urdf_path = os.path.join( data_path, @@ -155,13 +143,12 @@ def __init__(self, target_name="cabinet", skill="pick", replay=False): else: urdf_path = os.path.join( data_path, - "franka_tmr/franka_description_tmr/urdf/franka_left_arm.urdf", # Lambda Change + "franka_tmr/franka_description_tmr/urdf/franka_with_hand_2.urdf", # Lambda Change ) arm_urdf_path = os.path.join( data_path, - "franka_tmr/franka_description_tmr/allegro/allegro.urdf", # Lambda Change + "franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf", # Lambda Change ) ->>>>>>> 11b33a138 (add base movement and arm cameras) main_agent_config.articulated_agent_urdf = urdf_path main_agent_config.articulated_agent_type = "MurpRobot" main_agent_config.ik_arm_urdf = arm_urdf_path @@ -399,9 +386,9 @@ def move_base(self, base_lin_vel, base_ang_vel): ) }, } - self.env.sim.articulated_agent.base_transformation = self.base_trans + obs = self.env.step(action) - self.base_trans = self.env.sim.articulated_agent.base_transformation + self.base_trans = (self.env.sim.articulated_agent.base_transformation) im = process_obs_img(obs) im = add_text_to_image(im, "using base controller") self.writer.append_data(im) @@ -497,10 +484,10 @@ def get_poses(self, name, pose_type="base"): "ee_rot": np.deg2rad([120, 0, 0]), }, "fridge2": { - "base_pos": np.array([-4.75, 0.1, 1.1]), + "base_pos": np.array([-4.0, 0.1, 1.28]), "base_rot": 180, - "ee_pos": np.array([-6.3, 1.4, 2.4]), - "ee_rot": np.deg2rad([120,0,0]), + "ee_pos": np.array([-6.3, 1.2, 1.3]), + "ee_rot": np.deg2rad([-60, 0, 0]), }, "freezer": { "base_pos": np.array([-4.9, 0.1, 0.7]), @@ -514,7 +501,7 @@ def get_poses(self, name, pose_type="base"): poses[name][f"{pose_type}_rot"], ) - def set_targets(self, target_w_xyz, target_w_quat, target_joints): + def set_targets(self, target_w_xyz, target_w_quat, target_joints,hand="left"): self.target_w_xyz = target_w_xyz self.target_w_quat = target_w_quat target_quat = R.from_quat(target_w_quat, scalar_first=True) @@ -525,19 +512,36 @@ def set_targets(self, target_w_xyz, target_w_quat, target_joints): # XYZ self.open_xyz = target_w_xyz.copy() - self.open_xyz[2] += 0.1 - self.open_xyz[0] += 0.1 - - # Pre-Grasp Targets - OPEN_JOINTS = [1, 5, 9, 14] - # OPEN_JOINTS = [0, 4, 6, 9] - - # Grasp fingers - self.grasp_fingers = self.target_joints.copy() - self.close_fingers = self.target_joints.copy() - self.close_fingers[OPEN_JOINTS] += 0.2 + if hand=="left": + self.open_xyz[2] += 0.1 + self.open_xyz[0] += 0.1 - def get_targets(self, name="target"): + # Pre-Grasp Targets + OPEN_JOINTS = [1, 5, 9, 14] + # OPEN_JOINTS = [0, 4, 6, 9] + # Grasp fingers + self.grasp_fingers = self.target_joints.copy() + self.close_fingers = self.target_joints.copy() + self.close_fingers[OPEN_JOINTS] += 0.2 + else: + self.open_xyz[2] -= 0.05 + self.open_xyz[0] += 0.1 + SECONDARY_JOINTS = [2,6,10,15] + TERTIARY_JOINTS = [3,7,11] + OPEN_JOINTS = [1,5,9] + CURVE_JOINTS=[13] + BASE_THUMB_JOINT=[12] + self.grasp_fingers = self.target_joints.copy() + self.close_fingers = self.target_joints.copy() + self.close_fingers[BASE_THUMB_JOINT] +=1.0 + # self.close_fingers[CURVE_JOINTS] -=0.5 + self.close_fingers[SECONDARY_JOINTS] += 1.0 + self.close_fingers[TERTIARY_JOINTS] += 1.0 + self.close_fingers[OPEN_JOINTS] += 1.0 + + + + def get_targets(self, name="target", hand="right"): # Lambda Changes if name == "target": return ( @@ -556,8 +560,9 @@ def get_targets(self, name="target"): elif name == "open": self.open_xyz = self.get_curr_ee_pose()[0] - self.open_xyz[2] += 0.1 - self.open_xyz[0] += 0.1 + if hand == "right": + self.open_xyz[2] -= 0.05 + self.open_xyz[0] += 0.1 return ( torch.tensor(self.close_fingers, device="cuda:0"), @@ -685,23 +690,23 @@ def grasp_obj(self, name): curr_xyz, curr_ori = self.get_curr_ee_pose() print(f"Curr XYZ {curr_xyz}, Rot {curr_ori}") target_rot_rpy = self.target_ee_rot - if name == "open" and self.step > 10: - self.move_base_ee_and_hand( - -0.1, - 0.0, - act["tar_xyz"], - target_rot_rpy, - act["tar_fingers"], - timeout=10, - ) - # self.move_base( - # -0.1, - # 0.0, - # ) - else: - self.move_ee_and_hand( - act["tar_xyz"], target_rot_rpy, act["tar_fingers"], timeout=10 - ) + # if name == "open" and self.step > 10: + # self.move_base_ee_and_hand( + # -0.1, + # 0.0, + # act["tar_xyz"], + # target_rot_rpy, + # act["tar_fingers"], + # timeout=10, + # ) + # # self.move_base( + # # -0.1, + # # 0.0, + # # ) + # else: + self.move_ee_and_hand( + act["tar_xyz"], target_rot_rpy, act["tar_fingers"], timeout=10 + ) self.current_target_fingers = act["tar_fingers"] self.current_target_xyz = act["tar_xyz"] _current_target_rotmat = R.from_matrix(act["tar_rot"]) @@ -731,23 +736,15 @@ def replay_grasp_obj(self): ) print("saved_act tar_xyz: ", saved_act["tar_xyz"][0, :], idx) self.move_hand_joints(saved_act["tar_fingers"][0, :], timeout=5) - - def run_expert_w_grasp(self): - self.reset_robot(self.target_name) - - self.target_ee_pos, self.target_ee_rot = self.get_poses( - self.target_name, pose_type="ee" - ) - self.visualize_pos(self.target_ee_pos) - + + def execute_grasp_sequence(self, hand, grip_iters, open_iters, move_iters=None): self.move_to_ee( self.target_ee_pos, self.target_ee_rot, - grasp="pre_grasp", - timeout=300, + grasp="pre_grasp" if hand == "left" else "open", + timeout=300 if hand == "left" else 200, ) - # grasp object self.step = 0 self.current_target_fingers = ( self.env.sim.articulated_agent._robot_wrapper.right_hand_joint_pos @@ -764,25 +761,37 @@ def run_expert_w_grasp(self): target_w_xyz=target_xyz, target_w_quat=target_quaternion, target_joints=target_joints, + hand=hand ) + if move_iters: + for _ in range(move_iters): + self.move_base(1.0, 0.0) + + # Grasp and open object if self.replay: self.replay_grasp_obj() else: - for _ in range(20): + for _ in range(grip_iters): self.grasp_obj(name="target_grip") - self.step = 0 - for _ in range(30): + for _ in range(open_iters): self.grasp_obj(name="open") - door_orientation_quat = self.get_door_quat() - door_orienation_quat_R = R.from_quat(door_orientation_quat) - door_orientation_rpy = door_orienation_quat_R.as_euler( - "xyz", degrees=True + #Move robot back + for _ in range(10): + self.move_base(-1.0, 0.0) + + def run_expert_w_grasp(self, hand="left"): + self.reset_robot(self.target_name) + + self.target_ee_pos, self.target_ee_rot = self.get_poses( + self.target_name, pose_type="ee" ) - print("final door orientation: ", door_orientation_rpy) - self.writer.close() - print(f"saved file to: {self.save_path}") + self.visualize_pos(self.target_ee_pos) + if hand == "left": + self.execute_grasp_sequence(hand, grip_iters=30, open_iters=30) + elif hand == "right": + self.execute_grasp_sequence(hand, grip_iters=40, open_iters=30, move_iters=19) if __name__ == "__main__": @@ -790,7 +799,7 @@ def run_expert_w_grasp(self): # Add arguments parser.add_argument( - "--target-name", default="fridge", help="target object name" + "--target-name", default="fridge2", help="target object name" ) parser.add_argument("--skill", default="open", help="open, pick") parser.add_argument("--replay", action="store_true") @@ -798,4 +807,4 @@ def run_expert_w_grasp(self): args = parser.parse_args() datagen = ExpertDatagen(args.target_name, args.skill, args.replay) - datagen.run_expert_w_grasp() + datagen.run_expert_w_grasp(hand="right") \ No newline at end of file From dac086d62df20dbc2c1b6ca4f145dee0b98984c5 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 16:17:53 -0400 Subject: [PATCH 29/50] fix right and left arm bug --- examples/interactive_play.py | 5 +++-- .../habitat/tasks/rearrange/actions/actions.py | 15 +++++++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index cbe6190769..b12d65d978 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -939,10 +939,11 @@ def has_pygame(): ) ik_arm_urdf = "" + # Make sure we use the correct urdf for the arm control to compute IK if TEST_MACHINE == "h200": - ik_arm_urdf = "/home/jimmytyyang/research/hab_training/habitat-lab/data/franka_tmr/franka_description_tmr/urdf/franka_tmr_left_arm_only.urdf" + ik_arm_urdf = "/home/jimmytyyang/research/hab_training/habitat-lab/data/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" elif TEST_MACHINE == "lambda": - ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_left_arm_only.urdf" + ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" else: raise ValueError( f"Cannot recongize the TEST_MACHINE: {TEST_MACHINE}" diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 08a1080958..46678f4d37 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -977,6 +977,17 @@ def get_arm_mode(self, name): -1.3962275, ] ), + "retract": np.array( + [ + 2.6116285, + 1.5283098, + 1.5708, + -0.50559217, + -1.5708, + 1.5708, + -1.3962275, + ] + ), } return arm_joints[name] @@ -998,14 +1009,14 @@ def get_grasp_mode(self, name): def fix_arm(self, fix_right_left="left"): if fix_right_left == "left": self._robot_wrapper._target_arm_joint_positions = ( - self.get_arm_mode("rest") + self.get_arm_mode("retract") ) self._robot_wrapper._target_hand_joint_positions = ( self.get_grasp_mode("open") ) else: self._robot_wrapper._target_right_arm_joint_positions = ( - self.get_arm_mode("rest") + self.get_arm_mode("retract") ) self._robot_wrapper._target_right_hand_joint_positions = ( self.get_grasp_mode("open") From a649411162109274117603012a8d860fbb47cb7b Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 16:18:23 -0400 Subject: [PATCH 30/50] set path of grasp.py --- heuristic_expert_w_grasp.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/heuristic_expert_w_grasp.py b/heuristic_expert_w_grasp.py index 9f3260558e..6d7da7859d 100644 --- a/heuristic_expert_w_grasp.py +++ b/heuristic_expert_w_grasp.py @@ -53,7 +53,7 @@ if user == "joanne": data_path = "/fsx-siro/jtruong/repos/vla-physics/habitat-lab/data/" else: - data_path = "/home/joanne/habitat-lab/data/" + data_path = "home/jmmy/research/hab_training/habitat-lab/data/" def make_sim_cfg(agent_dict): @@ -143,11 +143,9 @@ def __init__(self, target_name="cabinet", skill="pick", replay=False): else: urdf_path = os.path.join( data_path, - "franka_tmr/franka_description_tmr/urdf/franka_with_hand_2.urdf", # Lambda Change + "robots/hab_murp/murp_tmr_franka/murp_tmr_franka_metahand.urdf", # Lambda Change ) - arm_urdf_path = os.path.join( - data_path, - "franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf", # Lambda Change + arm_urdf_path = os.path.join("/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" ) main_agent_config.articulated_agent_urdf = urdf_path main_agent_config.articulated_agent_type = "MurpRobot" From 2a023902323afb4132f4e15b3911e950f399e5b8 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 17:59:48 -0400 Subject: [PATCH 31/50] debug wip --- .../habitat/isaac_sim/_internal/murp_robot_wrapper.py | 8 ++++---- habitat-lab/habitat/isaac_sim/actions.py | 6 +++--- heuristic_expert_w_grasp.py | 6 ++---- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py index 169f3c9b2a..541bd57c88 100644 --- a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py +++ b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py @@ -32,7 +32,7 @@ class MurpRobotWrapper: def __init__(self, isaac_service, instance_id=0, right_left_hand="right"): self._isaac_service = isaac_service - asset_path = "./data/usd/robots/franka_with_hand_2.usda" # Lambda Machine Change + asset_path = "./data/usd/robots/franka_with_hand_2 (1).usda" # Lambda Machine Change robot_prim_path = f"/World/env_{instance_id}/Murp" self._robot_prim_path = robot_prim_path self._right_left_hand = right_left_hand @@ -202,7 +202,7 @@ def reset_arm(self): ] self.ee_link_name = left_arm_joint_names[-1].replace("joint", "link") - self.right_ee_link_name = left_arm_joint_names[-1].replace( + self.right_ee_link_name = right_arm_joint_names[-1].replace( "joint", "link" ) @@ -469,9 +469,9 @@ def physics_callback(self, step_size): "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor1" ) self.fix_base(step_size, base_position, base_orientation) - self.drive_arm(step_size) + #self.drive_arm(step_size) self.drive_right_arm(step_size) - self.drive_hand(step_size) + #self.drive_hand(step_size) self.drive_right_hand(step_size) self._step_count += 1 diff --git a/habitat-lab/habitat/isaac_sim/actions.py b/habitat-lab/habitat/isaac_sim/actions.py index 1ce3542bf4..1f6c328595 100644 --- a/habitat-lab/habitat/isaac_sim/actions.py +++ b/habitat-lab/habitat/isaac_sim/actions.py @@ -73,7 +73,7 @@ def reset(self, *args, **kwargs): try: self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( np.array( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos ) ) except: @@ -82,7 +82,7 @@ def reset(self, *args, **kwargs): def calc_desired_joints(self): joint_pos = np.array( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos ) joint_vel = np.zeros(joint_pos.shape) @@ -119,7 +119,7 @@ def inverse_transform(pos_a, rot_b, pos_b): des_joint_pos = self.calc_desired_joints() des_joint_pos = self.apply_joint_limits(des_joint_pos) - self._robot_wrapper._target_arm_joint_positions = des_joint_pos + self._robot_wrapper._target_right_arm_joint_positions = des_joint_pos @registry.register_task_action diff --git a/heuristic_expert_w_grasp.py b/heuristic_expert_w_grasp.py index 6d7da7859d..d4b050a359 100644 --- a/heuristic_expert_w_grasp.py +++ b/heuristic_expert_w_grasp.py @@ -142,11 +142,9 @@ def __init__(self, target_name="cabinet", skill="pick", replay=False): ) else: urdf_path = os.path.join( - data_path, - "robots/hab_murp/murp_tmr_franka/murp_tmr_franka_metahand.urdf", # Lambda Change - ) - arm_urdf_path = os.path.join("/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" + "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_with_hand_2.urdf", # Lambda Change ) + arm_urdf_path = os.path.join("/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf") main_agent_config.articulated_agent_urdf = urdf_path main_agent_config.articulated_agent_type = "MurpRobot" main_agent_config.ik_arm_urdf = arm_urdf_path From a7896f5cb1dcd0760e8d2ffe8fa4dda7efe010fc Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 17 Mar 2025 18:40:30 -0400 Subject: [PATCH 32/50] add --- habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py | 2 +- habitat-lab/habitat/isaac_sim/actions.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py index 541bd57c88..d4d4b4846b 100644 --- a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py +++ b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py @@ -385,7 +385,7 @@ def fix_base_height_via_linear_vel_z( ): curr_linear_velocity = self._robot.get_linear_velocity() - z_target = 0.7 # todo: get from navmesh or assume ground_z==0 + z_target = 0.1 # todo: get from navmesh or assume ground_z==0 max_linear_vel = 3.0 # Extract the vertical position and velocity diff --git a/habitat-lab/habitat/isaac_sim/actions.py b/habitat-lab/habitat/isaac_sim/actions.py index 1f6c328595..5da3e41109 100644 --- a/habitat-lab/habitat/isaac_sim/actions.py +++ b/habitat-lab/habitat/isaac_sim/actions.py @@ -73,7 +73,7 @@ def reset(self, *args, **kwargs): try: self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( np.array( - self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + self._sim.articulated_agent._robot_wrapper.arm_joint_pos ) ) except: @@ -82,7 +82,7 @@ def reset(self, *args, **kwargs): def calc_desired_joints(self): joint_pos = np.array( - self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + self._sim.articulated_agent._robot_wrapper.arm_joint_pos ) joint_vel = np.zeros(joint_pos.shape) From ec7ee3a95d23f59200c1f954e81ae70979926dc7 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 18 Mar 2025 09:29:00 -0400 Subject: [PATCH 33/50] apply the fix from 1cc236a --- habitat-lab/habitat/isaac_sim/actions.py | 30 +++---- heuristic_expert_w_grasp.py | 99 ++++++++++++++---------- 2 files changed, 71 insertions(+), 58 deletions(-) diff --git a/habitat-lab/habitat/isaac_sim/actions.py b/habitat-lab/habitat/isaac_sim/actions.py index 5da3e41109..d6ad40316e 100644 --- a/habitat-lab/habitat/isaac_sim/actions.py +++ b/habitat-lab/habitat/isaac_sim/actions.py @@ -1,6 +1,5 @@ import magnum as mn import numpy as np -from gym import spaces import habitat_sim from habitat.core.registry import registry @@ -10,6 +9,8 @@ BaseVelAction, ) +# from gym import spaces + @registry.register_task_action class BaseVelIsaacAction(BaseVelAction): @@ -46,18 +47,18 @@ def __init__(self, *args, **kwargs): ) def step(self, *args, **kwargs): - target_pos = kwargs[self._action_arg_prefix + "target_pos"] - base_pos, base_rot = self._robot_wrapper.get_root_pose() - - def inverse_transform(pos_a, rot_b, pos_b): - inv_pos = rot_b.inverted().transform_vector(pos_a - pos_b) - return inv_pos + pass + # target_pos = kwargs[self._action_arg_prefix + "target_pos"] + # base_pos, base_rot = self._robot_wrapper.get_root_pose() - target_rel_pos = inverse_transform(target_pos, base_rot, base_pos) - dt = 0.5 - new_arm_joints = self._spot_pick_helper.update(dt, target_rel_pos) - self._robot_wrapper._target_right_arm_joint_positions = des_joint_pos + # def inverse_transform(pos_a, rot_b, pos_b): + # inv_pos = rot_b.inverted().transform_vector(pos_a - pos_b) + # return inv_pos + # # target_rel_pos = inverse_transform(target_pos, base_rot, base_pos) + # # dt = 0.5 + # # new_arm_joints = self._spot_pick_helper.update(dt, target_rel_pos) + # # self._robot_wrapper._target_right_arm_joint_positions = des_joint_pos @registry.register_task_action @@ -73,16 +74,16 @@ def reset(self, *args, **kwargs): try: self.ee_target, self.ee_rot_target = self._ik_helper.calc_fk( np.array( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos ) ) - except: + except Exception: self.ee_target = None self.ee_rot_target = None def calc_desired_joints(self): joint_pos = np.array( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos ) joint_vel = np.zeros(joint_pos.shape) @@ -124,7 +125,6 @@ def inverse_transform(pos_a, rot_b, pos_b): @registry.register_task_action class BaseVelKinematicIsaacAction(BaseVelAction): - def update_base(self): ctrl_freq = self._sim.ctrl_freq trans = self.cur_articulated_agent.base_transformation diff --git a/heuristic_expert_w_grasp.py b/heuristic_expert_w_grasp.py index d4b050a359..ca37d79693 100644 --- a/heuristic_expert_w_grasp.py +++ b/heuristic_expert_w_grasp.py @@ -1,4 +1,3 @@ -import time import warnings import magnum as mn @@ -10,7 +9,6 @@ import argparse import math import os -import random import imageio import numpy as np @@ -144,7 +142,9 @@ def __init__(self, target_name="cabinet", skill="pick", replay=False): urdf_path = os.path.join( "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_with_hand_2.urdf", # Lambda Change ) - arm_urdf_path = os.path.join("/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf") + arm_urdf_path = os.path.join( + "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf" + ) main_agent_config.articulated_agent_urdf = urdf_path main_agent_config.articulated_agent_type = "MurpRobot" main_agent_config.ik_arm_urdf = arm_urdf_path @@ -230,6 +230,7 @@ def pin_right_arm(self): self.env.sim.articulated_agent._robot_wrapper._target_right_hand_joint_positions = self.get_grasp_mode( "open" ) + def pin_left_arm(self): self.env.sim.articulated_agent._robot_wrapper._target_arm_joint_positions = self.get_arm_mode( "rest" @@ -239,9 +240,10 @@ def pin_left_arm(self): ) def get_curr_ee_pose(self): - curr_ee_pos_vec, curr_ee_rot = ( - self.env.sim.articulated_agent._robot_wrapper.ee_pose() - ) + ( + curr_ee_pos_vec, + curr_ee_rot, + ) = self.env.sim.articulated_agent._robot_wrapper.ee_pose() curr_ee_rot_quat = R.from_quat( [*curr_ee_rot.vector, curr_ee_rot.scalar] @@ -254,13 +256,17 @@ def get_curr_joint_pose(self, arm="right"): if arm == "left": return self.env.sim.articulated_agent._robot_wrapper.arm_joint_pos elif arm == "right": - return self.env.sim.articulated_agent._robot_wrapper.right_arm_joint_pos + return ( + self.env.sim.articulated_agent._robot_wrapper.right_arm_joint_pos + ) def get_curr_hand_pose(self, arm="right"): if arm == "left": return self.env.sim.articulated_agent._robot_wrapper.hand_joint_pos elif arm == "right": - return self.env.sim.articulated_agent._robot_wrapper.right_hand_joint_pos + return ( + self.env.sim.articulated_agent._robot_wrapper.right_hand_joint_pos + ) def move_to_ee( self, target_ee_pos, target_ee_rot=None, grasp=None, timeout=1000 @@ -382,9 +388,9 @@ def move_base(self, base_lin_vel, base_ang_vel): ) }, } - + obs = self.env.step(action) - self.base_trans = (self.env.sim.articulated_agent.base_transformation) + self.base_trans = self.env.sim.articulated_agent.base_transformation im = process_obs_img(obs) im = add_text_to_image(im, "using base controller") self.writer.append_data(im) @@ -497,7 +503,9 @@ def get_poses(self, name, pose_type="base"): poses[name][f"{pose_type}_rot"], ) - def set_targets(self, target_w_xyz, target_w_quat, target_joints,hand="left"): + def set_targets( + self, target_w_xyz, target_w_quat, target_joints, hand="left" + ): self.target_w_xyz = target_w_xyz self.target_w_quat = target_w_quat target_quat = R.from_quat(target_w_quat, scalar_first=True) @@ -508,8 +516,8 @@ def set_targets(self, target_w_xyz, target_w_quat, target_joints,hand="left"): # XYZ self.open_xyz = target_w_xyz.copy() - if hand=="left": - self.open_xyz[2] += 0.1 + if hand == "left": + self.open_xyz[2] -= 0.1 self.open_xyz[0] += 0.1 # Pre-Grasp Targets @@ -520,22 +528,20 @@ def set_targets(self, target_w_xyz, target_w_quat, target_joints,hand="left"): self.close_fingers = self.target_joints.copy() self.close_fingers[OPEN_JOINTS] += 0.2 else: - self.open_xyz[2] -= 0.05 + self.open_xyz[2] -= 0.1 self.open_xyz[0] += 0.1 - SECONDARY_JOINTS = [2,6,10,15] - TERTIARY_JOINTS = [3,7,11] - OPEN_JOINTS = [1,5,9] - CURVE_JOINTS=[13] - BASE_THUMB_JOINT=[12] + SECONDARY_JOINTS = [2, 6, 10, 15] + TERTIARY_JOINTS = [3, 7, 11] + OPEN_JOINTS = [1, 5, 9] + CURVE_JOINTS = [13] + BASE_THUMB_JOINT = [12] self.grasp_fingers = self.target_joints.copy() self.close_fingers = self.target_joints.copy() - self.close_fingers[BASE_THUMB_JOINT] +=1.0 + self.close_fingers[BASE_THUMB_JOINT] += 1.1 # self.close_fingers[CURVE_JOINTS] -=0.5 - self.close_fingers[SECONDARY_JOINTS] += 1.0 + self.close_fingers[SECONDARY_JOINTS] += 0.7 self.close_fingers[TERTIARY_JOINTS] += 1.0 - self.close_fingers[OPEN_JOINTS] += 1.0 - - + self.close_fingers[OPEN_JOINTS] += 0.7 def get_targets(self, name="target", hand="right"): # Lambda Changes @@ -557,7 +563,7 @@ def get_targets(self, name="target", hand="right"): elif name == "open": self.open_xyz = self.get_curr_ee_pose()[0] if hand == "right": - self.open_xyz[2] -= 0.05 + self.open_xyz[1] -= 0.1 self.open_xyz[0] += 0.1 return ( @@ -603,7 +609,7 @@ def generate_action(self, cur_obs, name): door_rot = rotation_conversions.quaternion_to_matrix(door_orientation) rot_y = rotation_conversions.euler_angles_to_matrix( - torch.tensor([0.0, -math.pi, 0.0], device="cuda:0"), "XYZ" + torch.tensor([math.pi, -math.pi, 0.0], device="cuda:0"), "XYZ" ) target_rot = torch.einsum("ij,jk->ik", door_rot, rot_y) tar_rot = target_rot @@ -627,10 +633,11 @@ def apply_rotation(self, quat_door): return isaac_T_door_quat def get_door_quat(self): - door_trans, door_orientation_rpy = ( - self.env.sim.articulated_agent._robot_wrapper.get_prim_transform( - "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor2" - ) + ( + door_trans, + door_orientation_rpy, + ) = self.env.sim.articulated_agent._robot_wrapper.get_prim_transform( + "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor2" ) self.visualize_pos(door_trans, "door") quat_door = door_orientation_rpy.GetQuaternion() @@ -661,14 +668,16 @@ def grasp_obj(self, name): base_T_hand = act["tar_xyz"] - ee_pos, ee_rot = ( - self.env.sim.articulated_agent._robot_wrapper.hand_pose() - ) + ( + ee_pos, + ee_rot, + ) = self.env.sim.articulated_agent._robot_wrapper.hand_pose() base_T_ee = self.create_T_matrix(ee_pos, ee_rot) - hand_pos, hand_rot = ( - self.env.sim.articulated_agent._robot_wrapper.hand_pose() - ) + ( + hand_pos, + hand_rot, + ) = self.env.sim.articulated_agent._robot_wrapper.hand_pose() base_T_hand = self.create_T_matrix(hand_pos, hand_rot) ee_T_hand = np.linalg.inv(base_T_ee) @ base_T_hand @@ -732,8 +741,10 @@ def replay_grasp_obj(self): ) print("saved_act tar_xyz: ", saved_act["tar_xyz"][0, :], idx) self.move_hand_joints(saved_act["tar_fingers"][0, :], timeout=5) - - def execute_grasp_sequence(self, hand, grip_iters, open_iters, move_iters=None): + + def execute_grasp_sequence( + self, hand, grip_iters, open_iters, move_iters=None + ): self.move_to_ee( self.target_ee_pos, self.target_ee_rot, @@ -747,7 +758,7 @@ def execute_grasp_sequence(self, hand, grip_iters, open_iters, move_iters=None): ) self.current_target_xyz = self.target_ee_pos target_xyz, target_ori = self.get_curr_ee_pose() - target_xyz[1] -= 0.51 + target_xyz[1] -= 0.52 target_ori_rpy = R.from_euler("xyz", target_ori, degrees=True) target_quaternion = target_ori_rpy.as_quat(scalar_first=True) # wxzy target_joints = ( @@ -757,7 +768,7 @@ def execute_grasp_sequence(self, hand, grip_iters, open_iters, move_iters=None): target_w_xyz=target_xyz, target_w_quat=target_quaternion, target_joints=target_joints, - hand=hand + hand=hand, ) if move_iters: for _ in range(move_iters): @@ -773,7 +784,7 @@ def execute_grasp_sequence(self, hand, grip_iters, open_iters, move_iters=None): for _ in range(open_iters): self.grasp_obj(name="open") - #Move robot back + # Move robot back for _ in range(10): self.move_base(-1.0, 0.0) @@ -787,7 +798,9 @@ def run_expert_w_grasp(self, hand="left"): if hand == "left": self.execute_grasp_sequence(hand, grip_iters=30, open_iters=30) elif hand == "right": - self.execute_grasp_sequence(hand, grip_iters=40, open_iters=30, move_iters=19) + self.execute_grasp_sequence( + hand, grip_iters=30, open_iters=30, move_iters=19 + ) if __name__ == "__main__": @@ -803,4 +816,4 @@ def run_expert_w_grasp(self, hand="left"): args = parser.parse_args() datagen = ExpertDatagen(args.target_name, args.skill, args.replay) - datagen.run_expert_w_grasp(hand="right") \ No newline at end of file + datagen.run_expert_w_grasp(hand="right") From 162df955d92e9be7f14de78ac0379b570c5880b4 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 18 Mar 2025 11:04:47 -0400 Subject: [PATCH 34/50] fix hand arm moving bugs --- examples/interactive_play.py | 5 --- .../isaac_sim/_internal/murp_robot_wrapper.py | 34 ++++++++--------- .../tasks/rearrange/actions/actions.py | 37 +++++++++++-------- .../sub_tasks/articulated_object_sensors.py | 1 - 4 files changed, 38 insertions(+), 39 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index b12d65d978..0995d541f4 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -180,9 +180,6 @@ def get_input_vel_ctlr( elif keys[pygame.K_n]: env._sim.navmesh_visualization = not env._sim.navmesh_visualization - if key != -1: - print(f"key: {key}") - if not_block_input: # Base control if keys[pygame.K_j] or key == ord("j"): @@ -765,8 +762,6 @@ def play_env(env, args, config): time.sleep(delay) prev_time = curr_time - print(env.sim.articulated_agent.base_transformation.translation) - if args.save_actions: if len(all_arm_actions) < args.save_actions_count: raise ValueError( diff --git a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py index d4d4b4846b..62126fbec5 100644 --- a/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py +++ b/habitat-lab/habitat/isaac_sim/_internal/murp_robot_wrapper.py @@ -221,17 +221,17 @@ def reset_arm(self): self._arm_joint_indices = np.array(left_arm_joint_indices) self._right_arm_joint_indices = np.array(right_arm_joint_indices) - rest_positions = [ - 2.6116285, - 1.5283098, - 1.0930868, - -0.50559217, - 0.48147443, - 2.628784, - -1.3962275, - ] - self._target_arm_joint_positions = rest_positions - self._target_right_arm_joint_positions = rest_positions + # rest_positions = [ + # 2.6116285, + # 1.5283098, + # 1.0930868, + # -0.50559217, + # 0.48147443, + # 2.628784, + # -1.3962275, + # ] + self._target_arm_joint_positions = self.arm_joint_pos + self._target_right_arm_joint_positions = self.right_arm_joint_pos def get_link_id(self, link_str): return [ @@ -291,11 +291,11 @@ def reset_hand(self): self._hand_joint_indices = np.array(left_hand_joint_indices) self._right_hand_joint_indices = np.array(right_hand_joint_indices) - n_hand_joints = len(left_hand_joint_names) + # n_hand_joints = len(left_hand_joint_names) # closed_positions = np.array([3.14159] * n_hand_joints) - open_positions = np.zeros(n_hand_joints) - self._target_hand_joint_positions = open_positions - self._target_right_hand_joint_positions = open_positions + # open_positions = np.zeros(n_hand_joints) + self._target_hand_joint_positions = self.hand_joint_pos + self._target_right_hand_joint_positions = self.right_hand_joint_pos def post_reset(self): # todo: just do a single callback @@ -469,9 +469,9 @@ def physics_callback(self, step_size): "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor1" ) self.fix_base(step_size, base_position, base_orientation) - #self.drive_arm(step_size) + # self.drive_arm(step_size) self.drive_right_arm(step_size) - #self.drive_hand(step_size) + # self.drive_hand(step_size) self.drive_right_hand(step_size) self._step_count += 1 diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 46678f4d37..d7da551075 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -1023,26 +1023,34 @@ def fix_arm(self, fix_right_left="left"): ) def step(self, *args, **kwargs): - target_pos = kwargs[self._action_arg_prefix + "target_pos"] - target_rot = kwargs[self._action_arg_prefix + "target_rot"] + delta_pos = kwargs[self._action_arg_prefix + "target_pos"] + delta_rot = kwargs[self._action_arg_prefix + "target_rot"] finger = kwargs[self._action_arg_prefix + "target_finger"] - # base_pos, base_rot = self._robot_wrapper.get_root_pose() - print(f"target_pos: {target_pos}; target_rot: {target_rot}") - print(f"EE: {self.ee_target} {self.ee_rot_target}") + self.ee_target += np.array(delta_pos) + self.ee_rot_target += np.array(delta_rot) - # def inverse_transform(pos_a, rot_b, pos_b): - # inv_pos = rot_b.inverted().transform_vector(pos_a - pos_b) - # return inv_pos - # target_rel_pos = inverse_transform(target_pos, base_rot, base_pos) - self.ee_target += np.array(target_pos) - self.ee_rot_target += np.array(target_rot) self.target_finger[0:6] += finger + self.apply_ee_constraints() + des_joint_pos = self.calc_desired_joints() des_joint_pos = self.apply_joint_limits(des_joint_pos) - print(f"des_joint_pos: {des_joint_pos}") - print(f"self.target_finger: {self.target_finger}") + + if not np.any(delta_pos) and not np.any(delta_rot): + # Fix the arm and hands if there is no input + self._robot_wrapper._target_right_arm_joint_positions = ( + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + ) + self._robot_wrapper._target_arm_joint_positions = ( + self._sim.articulated_agent._robot_wrapper.arm_joint_pos + ) + self._robot_wrapper._target_right_hand_joint_positions = ( + self._sim.articulated_agent._robot_wrapper.right_hand_joint_pos + ) + self._robot_wrapper._target_hand_joint_positions = ( + self._sim.articulated_agent._robot_wrapper.hand_joint_pos + ) if self._config.right_left_hand == "right": self._robot_wrapper._target_right_arm_joint_positions = ( des_joint_pos @@ -1050,11 +1058,8 @@ def step(self, *args, **kwargs): self._robot_wrapper._target_right_hand_joint_positions = ( self.target_finger ) - self.fix_arm("left") - print("control right, fix left") else: self._robot_wrapper._target_arm_joint_positions = des_joint_pos self._robot_wrapper._target_hand_joint_positions = ( self.target_finger ) - self.fix_arm("right") diff --git a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py index 9d3391a6a8..ec73fcf9a7 100644 --- a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py @@ -183,7 +183,6 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): def update_metric(self, *args, episode, task, observations, **kwargs): if type(task._sim) == IsaacRearrangeSim: rpy = get_door_quat(task) - print(f"current door rpy: {rpy}") self._metric = rpy[0] else: self._metric = task.get_use_marker().get_targ_js() From 40e5dd989ab729694b15b16f65109de403e7898f Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 18 Mar 2025 11:44:34 -0400 Subject: [PATCH 35/50] fix action --- .../tasks/rearrange/actions/actions.py | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index d7da551075..8bc0a6a0ee 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -9,6 +9,7 @@ import magnum as mn import numpy as np from gym import spaces +from scipy.spatial.transform import Rotation as R import habitat_sim from habitat.core.embodied_task import SimulatorTaskAction @@ -1022,16 +1023,29 @@ def fix_arm(self, fix_right_left="left"): self.get_grasp_mode("open") ) + def get_curr_ee_pose(self): + ( + curr_ee_pos_vec, + curr_ee_rot, + ) = self._sim.articulated_agent._robot_wrapper.ee_pose() + + curr_ee_rot_quat = R.from_quat( + [*curr_ee_rot.vector, curr_ee_rot.scalar] + ) + curr_ee_rot_rpy = curr_ee_rot_quat.as_euler("xyz", degrees=True) + curr_ee_pos = np.array([*curr_ee_pos_vec]) + return curr_ee_pos, curr_ee_rot_rpy + def step(self, *args, **kwargs): delta_pos = kwargs[self._action_arg_prefix + "target_pos"] delta_rot = kwargs[self._action_arg_prefix + "target_rot"] finger = kwargs[self._action_arg_prefix + "target_finger"] - self.ee_target += np.array(delta_pos) self.ee_rot_target += np.array(delta_rot) self.target_finger[0:6] += finger + # Constrain the ee location self.apply_ee_constraints() des_joint_pos = self.calc_desired_joints() @@ -1051,7 +1065,7 @@ def step(self, *args, **kwargs): self._robot_wrapper._target_hand_joint_positions = ( self._sim.articulated_agent._robot_wrapper.hand_joint_pos ) - if self._config.right_left_hand == "right": + elif self._config.right_left_hand == "right": self._robot_wrapper._target_right_arm_joint_positions = ( des_joint_pos ) @@ -1063,3 +1077,17 @@ def step(self, *args, **kwargs): self._robot_wrapper._target_hand_joint_positions = ( self.target_finger ) + + print(f"target local ee xyz: {self.ee_target}") + print(f"get_curr_ee_pose (global): {self.get_curr_ee_pose()}") + ee_pos = ( + self._sim.get_agent_data(0) + .articulated_agent.ee_transform() + .translation + ) + print(f"ee_transform (global): {ee_pos}") + trans = self._sim.get_agent_data( + 0 + ).articulated_agent.base_transformation + local_ee_pos = trans.inverted().transform_point(ee_pos) + print(f"local_ee_pos (local): {local_ee_pos}") From c1204e7af0e6dafa1c5652aae66018978a207bda Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 18 Mar 2025 18:11:28 -0400 Subject: [PATCH 36/50] fix BaseVelIsaacAction frame --- .../tasks/rearrange/actions/actions.py | 79 ++++++++++++------- 1 file changed, 51 insertions(+), 28 deletions(-) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 8bc0a6a0ee..ddd2078c0a 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -14,6 +14,7 @@ import habitat_sim from habitat.core.embodied_task import SimulatorTaskAction from habitat.core.registry import registry +from habitat.isaac_sim import isaac_prim_utils from habitat.sims.habitat_simulator.actions import HabitatSimActions from habitat.tasks.rearrange.actions.articulated_agent_action import ( ArticulatedAgentAction, @@ -892,8 +893,22 @@ def step(self, *args, **kwargs): self.cur_articulated_agent._robot_wrapper._robot.set_angular_velocity( [0, 0, ang_vel] ) + # self.cur_articulated_agent._robot_wrapper._robot.set_linear_velocity( + # [lin_vel, 0, 0] + # ) + robot_forward = isaac_prim_utils.get_forward( + self.cur_articulated_agent._robot_wrapper._robot + ) + cur_linear_vel_usd = ( + self.cur_articulated_agent._robot_wrapper._robot.get_linear_velocity() + ) + linear_vel = robot_forward * lin_vel + linear_vel_usd = isaac_prim_utils.habitat_to_usd_position( + [linear_vel.x, linear_vel.y, linear_vel.z] + ) + linear_vel_usd[2] = cur_linear_vel_usd[2] self.cur_articulated_agent._robot_wrapper._robot.set_linear_velocity( - [lin_vel, 0, 0] + linear_vel_usd ) @@ -1051,21 +1066,22 @@ def step(self, *args, **kwargs): des_joint_pos = self.calc_desired_joints() des_joint_pos = self.apply_joint_limits(des_joint_pos) - if not np.any(delta_pos) and not np.any(delta_rot): - # Fix the arm and hands if there is no input - self._robot_wrapper._target_right_arm_joint_positions = ( - self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos - ) - self._robot_wrapper._target_arm_joint_positions = ( - self._sim.articulated_agent._robot_wrapper.arm_joint_pos - ) - self._robot_wrapper._target_right_hand_joint_positions = ( - self._sim.articulated_agent._robot_wrapper.right_hand_joint_pos - ) - self._robot_wrapper._target_hand_joint_positions = ( - self._sim.articulated_agent._robot_wrapper.hand_joint_pos - ) - elif self._config.right_left_hand == "right": + # if not np.any(delta_pos) and not np.any(delta_rot): + # # Fix the arm and hands if there is no input + # self._robot_wrapper._target_right_arm_joint_positions = ( + # self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + # ) + # self._robot_wrapper._target_arm_joint_positions = ( + # self._sim.articulated_agent._robot_wrapper.arm_joint_pos + # ) + # self._robot_wrapper._target_right_hand_joint_positions = ( + # self._sim.articulated_agent._robot_wrapper.right_hand_joint_pos + # ) + # self._robot_wrapper._target_hand_joint_positions = ( + # self._sim.articulated_agent._robot_wrapper.hand_joint_pos + # ) + + if self._config.right_left_hand == "right": self._robot_wrapper._target_right_arm_joint_positions = ( des_joint_pos ) @@ -1078,16 +1094,23 @@ def step(self, *args, **kwargs): self.target_finger ) - print(f"target local ee xyz: {self.ee_target}") - print(f"get_curr_ee_pose (global): {self.get_curr_ee_pose()}") - ee_pos = ( - self._sim.get_agent_data(0) - .articulated_agent.ee_transform() - .translation + # #print(f"target local ee xyz: {self.ee_target}") + # #print(f"get_curr_ee_pose (global): {self.get_curr_ee_pose()}") + # ee_pos = ( + # self._sim.get_agent_data(0) + # .articulated_agent.ee_transform() + # .translation + # ) + # trans = self._sim.get_agent_data( + # 0 + # ).articulated_agent.base_transformation + # local_ee_pos = trans.inverted().transform_point(ee_pos) + # #print(f"local_ee_pos (local): {local_ee_pos}") + + ee_pos_from_ik, _ = self._ik_helper.calc_fk( + np.array( + self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + ) ) - print(f"ee_transform (global): {ee_pos}") - trans = self._sim.get_agent_data( - 0 - ).articulated_agent.base_transformation - local_ee_pos = trans.inverted().transform_point(ee_pos) - print(f"local_ee_pos (local): {local_ee_pos}") + print(f"self.ee_target: {self.ee_target}") + print(f"ee_pos_from_ik: {ee_pos_from_ik}") From 7fbb1e3f433dc2bcaf81d1f2572d90c9802eb5ac Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 18 Mar 2025 19:38:04 -0400 Subject: [PATCH 37/50] remove print --- .../tasks/rearrange/actions/actions.py | 45 +++---------------- 1 file changed, 6 insertions(+), 39 deletions(-) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index ddd2078c0a..8bbdfcac57 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -881,7 +881,6 @@ def step(self, *args, **kwargs): class BaseVelIsaacAction(BaseVelAction): def step(self, *args, **kwargs): lin_vel, ang_vel = kwargs[self._action_arg_prefix + "base_vel"] - print(f"lin_vel: {lin_vel}; ang_vel: {ang_vel}") lin_vel = np.clip(lin_vel, -1, 1) * self._lin_speed ang_vel = np.clip(ang_vel, -1, 1) * self._ang_speed if not self._allow_back: @@ -889,13 +888,11 @@ def step(self, *args, **kwargs): self.base_vel_ctrl.linear_velocity = mn.Vector3(lin_vel, 0, 0) self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0) - # + self.cur_articulated_agent._robot_wrapper._robot.set_angular_velocity( [0, 0, ang_vel] ) - # self.cur_articulated_agent._robot_wrapper._robot.set_linear_velocity( - # [lin_vel, 0, 0] - # ) + robot_forward = isaac_prim_utils.get_forward( self.cur_articulated_agent._robot_wrapper._robot ) @@ -1066,21 +1063,6 @@ def step(self, *args, **kwargs): des_joint_pos = self.calc_desired_joints() des_joint_pos = self.apply_joint_limits(des_joint_pos) - # if not np.any(delta_pos) and not np.any(delta_rot): - # # Fix the arm and hands if there is no input - # self._robot_wrapper._target_right_arm_joint_positions = ( - # self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos - # ) - # self._robot_wrapper._target_arm_joint_positions = ( - # self._sim.articulated_agent._robot_wrapper.arm_joint_pos - # ) - # self._robot_wrapper._target_right_hand_joint_positions = ( - # self._sim.articulated_agent._robot_wrapper.right_hand_joint_pos - # ) - # self._robot_wrapper._target_hand_joint_positions = ( - # self._sim.articulated_agent._robot_wrapper.hand_joint_pos - # ) - if self._config.right_left_hand == "right": self._robot_wrapper._target_right_arm_joint_positions = ( des_joint_pos @@ -1094,23 +1076,8 @@ def step(self, *args, **kwargs): self.target_finger ) - # #print(f"target local ee xyz: {self.ee_target}") - # #print(f"get_curr_ee_pose (global): {self.get_curr_ee_pose()}") - # ee_pos = ( - # self._sim.get_agent_data(0) - # .articulated_agent.ee_transform() - # .translation + # ee_pos_from_ik, _ = self._ik_helper.calc_fk( + # np.array( + # self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos + # ) # ) - # trans = self._sim.get_agent_data( - # 0 - # ).articulated_agent.base_transformation - # local_ee_pos = trans.inverted().transform_point(ee_pos) - # #print(f"local_ee_pos (local): {local_ee_pos}") - - ee_pos_from_ik, _ = self._ik_helper.calc_fk( - np.array( - self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos - ) - ) - print(f"self.ee_target: {self.ee_target}") - print(f"ee_pos_from_ik: {ee_pos_from_ik}") From 5e48e005f2794e9b8ff8ca052f7e55bc27337205 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 18 Mar 2025 19:54:30 -0400 Subject: [PATCH 38/50] add sensor of door --- .../benchmark/rearrange/play/pick_murp.yaml | 2 + .../config/default_structured_configs.py | 16 ++++++ .../tasks/rearrange/rearrange_sensors.py | 56 +++++++++++++++++++ 3 files changed, 74 insertions(+) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index 7b9b65c612..2c07c73095 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -27,6 +27,7 @@ defaults: - target_start_sensor # Relative position from end effector to target object - end_effector_sensor - relative_resting_pos_sensor + - door_orientation_sensor - /habitat/dataset/rearrangement: replica_cad - _self_ @@ -40,6 +41,7 @@ habitat: - hand_joint - obj_start_sensor # Relative position from end effector to target object - relative_resting_position + - door_orientation task: # Config for empty task to explore the scene. type: RearrangePickTask-v0 diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index 26daa2d4b6..95ae55c7c1 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -61,6 +61,7 @@ "EEPositionSensorConfig", "JointSensorConfig", "HandJointSensorConfig", + "DoorOrientationSensorConfig", "HumanoidJointSensorConfig", "TargetStartSensorConfig", "GoalSensorConfig", @@ -550,6 +551,15 @@ class HandJointSensorConfig(LabSensorConfig): right_left_hand: str = "right" +@dataclass +class DoorOrientationSensorConfig(LabSensorConfig): + r""" + The door roll + """ + type: str = "DoorOrientationSensor" + dimensionality: int = 1 + + @dataclass class HumanoidJointSensorConfig(LabSensorConfig): r""" @@ -2284,6 +2294,12 @@ class HabitatConfig(HabitatBaseConfig): name="hand_joint_sensor", node=HandJointSensorConfig, ) +cs.store( + package="habitat.task.lab_sensors.door_orientation_sensor", + group="habitat/task/lab_sensors", + name="door_orientation_sensor", + node=DoorOrientationSensorConfig, +) cs.store( package="habitat.task.lab_sensors.humanoid_joint_sensor", group="habitat/task/lab_sensors", diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index a1c16bd5d9..9b41a96e8a 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -9,6 +9,7 @@ import numpy as np from gym import spaces +from scipy.spatial.transform import Rotation as R from habitat.articulated_agents.humanoids import KinematicHumanoid from habitat.core.embodied_task import Measure @@ -29,6 +30,37 @@ from habitat.tasks.utils import cartesian_to_polar +def apply_rotation(quat_door): + hab_T_door = R.from_quat(quat_door) + isaac_T_hab_list = [-90, 0, 0] + isaac_T_hab = R.from_euler("xyz", isaac_T_hab_list, degrees=True) + isaac_T_door_mat = R.from_matrix( + isaac_T_hab.as_matrix() @ hab_T_door.as_matrix() + ) + isaac_T_door_quat = isaac_T_door_mat.as_quat() + return isaac_T_door_quat + + +def get_door_quat(task): + ( + _, + door_orientation_rpy, + ) = task._sim.articulated_agent._robot_wrapper.get_prim_transform( + "_urdf_kitchen_FREMONT_KITCHENSET_FREMONT_KITCHENSET_CLEANED_urdf/kitchenset_fridgedoor2" + ) + # self.visualize_pos(door_trans, "door") + quat_door = door_orientation_rpy.GetQuaternion() + # Getting Quaternion Val to Array + scalar = quat_door.GetReal() + vector = quat_door.GetImaginary() + quat_door = [scalar, vector[0], vector[1], vector[2]] + isaac_T_door_quat = apply_rotation(quat_door) + door_orienation_quat_R = R.from_quat(isaac_T_door_quat) + door_orientation_rpy = door_orienation_quat_R.as_euler("xyz", degrees=True) + + return door_orientation_rpy + + class MultiObjSensor(PointGoalSensor): """ Abstract parent class for a sensor that specifies the locations of all targets. @@ -282,6 +314,30 @@ def get_observation(self, observations, episode, *args, **kwargs): return np.array(joints_pos, dtype=np.float32) +@registry.register_sensor +class DoorOrientationSensor(UsesArticulatedAgentInterface, Sensor): + def __init__(self, sim, config, *args, **kwargs): + super().__init__(config=config) + self._sim = sim + + def _get_uuid(self, *args, **kwargs): + return "door_orientation" + + def _get_sensor_type(self, *args, **kwargs): + return SensorTypes.TENSOR + + def _get_observation_space(self, *args, config, **kwargs): + return spaces.Box( + shape=(config.dimensionality,), + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + dtype=np.float32, + ) + + def get_observation(self, observations, task, episode, *args, **kwargs): + return np.array([get_door_quat(task)[0]], dtype=np.float32) + + @registry.register_sensor class HumanoidJointSensor(UsesArticulatedAgentInterface, Sensor): def __init__(self, sim, config, *args, **kwargs): From 24a5b1be2b15b0c0d957efe2ed71723f01ac4bab Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 10:00:24 -0400 Subject: [PATCH 39/50] refine the action space of the hand --- examples/interactive_play.py | 181 +++++++++++------- .../articulated_agents/robots/murp_robot.py | 8 +- .../tasks/rearrange/actions/actions.py | 13 +- 3 files changed, 129 insertions(+), 73 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 0995d541f4..2e85590b46 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -113,6 +113,7 @@ def get_input_vel_ctlr( control_humanoid, humanoid_controller, key=None, + ee_hand_control="ee", ): if skip_pygame: return step_env(env, "empty", {}), None, False @@ -143,7 +144,7 @@ def get_input_vel_ctlr( base_key = "base_vel" if "murp" in cfg: - arm_action_space = np.zeros(12) + arm_action_space = np.zeros(22) arm_ctrlr = None base_action = None elif arm_action_name in env.action_space.spaces: @@ -182,16 +183,16 @@ def get_input_vel_ctlr( if not_block_input: # Base control - if keys[pygame.K_j] or key == ord("j"): + if keys[pygame.K_j] or key == ord("l"): # Left base_action = [0, 1] - elif keys[pygame.K_l] or key == ord("l"): + elif keys[pygame.K_l] or key == ord("'"): # Right base_action = [0, -1] - elif keys[pygame.K_k] or key == ord("k"): + elif keys[pygame.K_k] or key == ord(";"): # Back base_action = [-1, 0] - elif keys[pygame.K_i] or key == ord("i"): + elif keys[pygame.K_i] or key == ord("p"): # Forward base_action = [1, 0] @@ -232,67 +233,100 @@ def get_input_vel_ctlr( elif keys[pygame.K_7]: arm_action[6] = -1.0 - elif arm_action_space.shape[0] == 12: - # Velocity control. A different key for each joint - if keys[pygame.K_q] or key == ord("q"): - arm_action[0] = 0.25 - elif keys[pygame.K_1] or key == ord("1"): - arm_action[0] = -0.25 - - elif keys[pygame.K_w] or key == ord("w"): - arm_action[1] = 0.25 - elif keys[pygame.K_2] or key == ord("2"): - arm_action[1] = -0.25 - - elif keys[pygame.K_e] or key == ord("e"): - arm_action[2] = 0.25 - elif keys[pygame.K_3] or key == ord("3"): - arm_action[2] = -0.25 - - elif keys[pygame.K_r] or key == ord("r"): - arm_action[3] = 0.25 - elif keys[pygame.K_4] or key == ord("4"): - arm_action[3] = -0.25 - - elif keys[pygame.K_t] or key == ord("t"): - arm_action[4] = 0.25 - elif keys[pygame.K_5] or key == ord("5"): - arm_action[4] = -0.25 - - elif keys[pygame.K_y] or key == ord("y"): - arm_action[5] = 0.25 - elif keys[pygame.K_6] or key == ord("6"): - arm_action[5] = -0.25 - - elif key == ord("a"): - arm_action[6] = 0.25 - elif key == ord("z"): - arm_action[6] = -0.25 - - elif key == ord("s"): - arm_action[7] = 0.25 - elif key == ord("x"): - arm_action[7] = -0.25 - - elif key == ord("d"): - arm_action[8] = 0.25 - elif key == ord("c"): - arm_action[8] = -0.25 - - elif key == ord("f"): - arm_action[9] = 0.25 - elif key == ord("v"): - arm_action[9] = -0.25 - - elif key == ord("g"): - arm_action[10] = 0.25 - elif key == ord("b"): - arm_action[10] = -0.25 - - elif key == ord("h"): - arm_action[11] = 0.25 - elif key == ord("n"): - arm_action[11] = -0.25 + elif arm_action_space.shape[0] == 22: + # For the ee location of the arm + + if ee_hand_control == "ee": + # x, y, z, roll, pitch, yaw of ee + if keys[pygame.K_q] or key == ord("q"): + arm_action[0] = -0.1 + elif keys[pygame.K_1] or key == ord("1"): + arm_action[0] = 0.1 + elif keys[pygame.K_w] or key == ord("w"): + arm_action[1] = -0.1 + elif keys[pygame.K_2] or key == ord("2"): + arm_action[1] = 0.1 + elif keys[pygame.K_e] or key == ord("e"): + arm_action[2] = -0.1 + elif keys[pygame.K_3] or key == ord("3"): + arm_action[2] = 0.1 + elif keys[pygame.K_r] or key == ord("r"): + arm_action[3] = -0.1 + elif keys[pygame.K_4] or key == ord("4"): + arm_action[3] = 0.1 + elif keys[pygame.K_t] or key == ord("t"): + arm_action[4] = -0.1 + elif keys[pygame.K_5] or key == ord("5"): + arm_action[4] = 0.1 + elif keys[pygame.K_y] or key == ord("y"): + arm_action[5] = -0.1 + elif keys[pygame.K_6] or key == ord("6"): + arm_action[5] = 0.1 + else: + if keys[pygame.K_q] or key == ord("1"): + arm_action[6] = 0.1 + elif keys[pygame.K_1] or key == ord("q"): + arm_action[6] = -0.1 + elif keys[pygame.K_q] or key == ord("2"): + arm_action[7] = 0.1 + elif keys[pygame.K_1] or key == ord("w"): + arm_action[7] = -0.1 + elif keys[pygame.K_q] or key == ord("3"): + arm_action[8] = 0.1 + elif keys[pygame.K_1] or key == ord("e"): + arm_action[8] = -0.1 + elif keys[pygame.K_q] or key == ord("4"): + arm_action[9] = 0.1 + elif keys[pygame.K_1] or key == ord("r"): + arm_action[9] = -0.1 + elif keys[pygame.K_q] or key == ord("5"): + arm_action[10] = 0.1 + elif keys[pygame.K_1] or key == ord("t"): + arm_action[10] = -0.1 + elif keys[pygame.K_q] or key == ord("6"): + arm_action[11] = 0.1 + elif keys[pygame.K_1] or key == ord("y"): + arm_action[11] = -0.1 + elif keys[pygame.K_q] or key == ord("7"): + arm_action[12] = 0.1 + elif keys[pygame.K_1] or key == ord("u"): + arm_action[12] = -0.1 + elif keys[pygame.K_q] or key == ord("8"): + arm_action[13] = 0.1 + elif keys[pygame.K_1] or key == ord("i"): + arm_action[13] = -0.1 + if keys[pygame.K_q] or key == ord("a"): + arm_action[14] = 0.1 + elif keys[pygame.K_1] or key == ord("z"): + arm_action[14] = -0.1 + elif keys[pygame.K_q] or key == ord("s"): + arm_action[15] = 0.1 + elif keys[pygame.K_1] or key == ord("x"): + arm_action[15] = -0.1 + elif keys[pygame.K_q] or key == ord("d"): + arm_action[16] = 0.1 + elif keys[pygame.K_1] or key == ord("c"): + arm_action[16] = -0.1 + elif keys[pygame.K_q] or key == ord("f"): + arm_action[17] = 0.1 + elif keys[pygame.K_1] or key == ord("v"): + arm_action[17] = -0.1 + elif keys[pygame.K_q] or key == ord("g"): + arm_action[18] = 0.1 + elif keys[pygame.K_1] or key == ord("b"): + arm_action[18] = -0.1 + elif keys[pygame.K_q] or key == ord("h"): + arm_action[19] = 0.1 + elif keys[pygame.K_1] or key == ord("n"): + arm_action[19] = -0.1 + elif keys[pygame.K_q] or key == ord("j"): + arm_action[20] = 0.1 + elif keys[pygame.K_1] or key == ord("m"): + arm_action[20] = -0.1 + elif keys[pygame.K_q] or key == ord("k"): + arm_action[21] = 0.1 + elif keys[pygame.K_1] or key == ord(","): + arm_action[21] = -0.1 elif arm_action_space.shape[0] == 4: # Velocity control. A different key for each joint @@ -579,6 +613,16 @@ def update(self, env, step_result, update_idx): return step_result +def switch_ee_hand_control(key, ee_hand_control): + # To switch between arm or finger control + if key == ord("0"): + if ee_hand_control == "ee": + ee_hand_control = "hand" + else: + ee_hand_control = "ee" + return ee_hand_control + + def play_env(env, args, config): render_steps_limit = None if args.no_render: @@ -625,10 +669,14 @@ def play_env(env, args, config): humanoid_controller.reset(env._sim.articulated_agent.base_pos) env_steps = 0 + + ee_hand_control = "ee" while True: print(f"Step: {env_steps}") env_steps += 1 + ee_hand_control = switch_ee_hand_control(key, ee_hand_control) + if ( args.save_actions and len(all_arm_actions) > args.save_actions_count @@ -662,6 +710,7 @@ def play_env(env, args, config): args.control_humanoid, humanoid_controller=humanoid_controller, key=key, + ee_hand_control=ee_hand_control, ) if not args.no_render and keys[pygame.K_c]: diff --git a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py index e697bc1892..d60e3af18a 100644 --- a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -79,21 +79,21 @@ class MurpRobot(MobileManipulator): @classmethod def _get_murp_params(cls): return MurpParams( - arm_joints=[0, 2, 4, 6, 8, 10, 12], # remove 0, 10 + arm_joints=[0, 2, 4, 6, 8, 10, 12], gripper_joints=[19], arm_init_params=[ - 2.6116285, # for 0 + 2.6116285, 1.5283098, 1.0930868, -0.50559217, 0.48147443, - 2.628784, # for 10 + 2.628784, -1.3962275, ], gripper_init_params=[-1.56], ee_offset=[mn.Vector3(0.08, 0, 0)], ee_links=[7], - ee_constraint=np.array([[[0.4, 1.2], [-0.7, 0.7], [0.25, 1.5]]]), + ee_constraint=np.array([[[0.1, 1.5], [-1.0, 1.0], [0.25, 1.5]]]), cameras={ "articulated_agent_arm_depth": ArticulatedAgentCameraParams( cam_offset_pos=mn.Vector3(0.166, 0.0, 0.018), diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 8bbdfcac57..9569c92d55 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -920,7 +920,8 @@ def __init__(self, *args, **kwargs): @property def action_space(self): - return spaces.Box(shape=(12,), low=-1, high=1, dtype=np.float32) + # 6-dim for the ee, x, y, z, roll, pitch, yaw, and 16-dim for the hand + return spaces.Box(shape=(22,), low=-1, high=1, dtype=np.float32) def reset(self, *args, **kwargs): try: @@ -1052,14 +1053,17 @@ def step(self, *args, **kwargs): delta_pos = kwargs[self._action_arg_prefix + "target_pos"] delta_rot = kwargs[self._action_arg_prefix + "target_rot"] finger = kwargs[self._action_arg_prefix + "target_finger"] + + # Update the target joint location self.ee_target += np.array(delta_pos) self.ee_rot_target += np.array(delta_rot) - - self.target_finger[0:6] += finger + self.target_finger += np.array(finger) # Constrain the ee location self.apply_ee_constraints() + # TODO: jimmy: missing finger joint limit + des_joint_pos = self.calc_desired_joints() des_joint_pos = self.apply_joint_limits(des_joint_pos) @@ -1081,3 +1085,6 @@ def step(self, *args, **kwargs): # self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos # ) # ) + print(f"self.ee_target: {self.ee_target}") + print(f"self.ee_rot_target: {self.ee_rot_target}") + print(f"self.target_finger: {self.target_finger}") From 99c405dd53a3db79be86c2fa9565147e0a08ca2d Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 10:20:23 -0400 Subject: [PATCH 40/50] refine action space delta joint --- .../benchmark/rearrange/play/pick_murp.yaml | 41 ++++++++----------- .../benchmark/rearrange/play/play_murp.yaml | 17 +------- .../config/default_structured_configs.py | 3 ++ .../simulator/sensor_setups/murp_agent.yaml | 27 ++---------- ...base_arm_empty.yaml => murp_base_arm.yaml} | 1 - .../tasks/rearrange/actions/actions.py | 9 ++++ 6 files changed, 34 insertions(+), 64 deletions(-) rename habitat-lab/habitat/config/habitat/task/rearrange/actions/{murp_base_arm_empty.yaml => murp_base_arm.yaml} (93%) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index 2c07c73095..87714363dd 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -4,15 +4,13 @@ defaults: - /habitat/simulator: isaac_rearrange_sim - /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: rgbd_head_rgbd_arm_agent - - /habitat/simulator/agents@habitat.simulator.agents.main_agent: fetch_suction - + - /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp_agent - /habitat/task: task_config_base - - /habitat/task/rearrange/actions: fetch_suction_arm_base_empty + - /habitat/task/rearrange/actions: murp_base_arm - /habitat/task/measurements: - articulated_agent_force # TODO: jimmy: remove this since AttributeError: 'IsaacAgentData' object has no attribute 'grasp_mgr - articulated_agent_colls # TODO: jimmy: the underlying implementation is a hack - force_terminate # TODO: jimmy: the underlying implementation is a hack - #- zero - end_effector_to_object_distance - end_effector_to_rest_distance - num_steps @@ -36,20 +34,16 @@ defaults: habitat: gym: obs_keys: - - articulated_agent_arm_depth - - joint - - hand_joint - - obj_start_sensor # Relative position from end effector to target object - - relative_resting_position - - door_orientation + - articulated_agent_arm_depth # depth image of the arm: 224 by 224 + - joint # joint angle of the arm: 7-dim + - hand_joint # joint angle of the hand: 16-dim + - obj_start_sensor # relative position from ee to target object: 3-dim + - relative_resting_position # relative resting x,y,z location for the ee: 3-dim + - door_orientation # door yaw: 1-dim task: - # Config for empty task to explore the scene. type: RearrangePickTask-v0 count_obj_collisions: True desired_resting_position: [0.5, 0.0, 1.0] - # reward_measure: "zero" # default measure for the play yaml - # success_measure: "zero" # default measure for the play yaml - reward_measure: art_obj_reward success_measure: art_obj_success @@ -61,12 +55,11 @@ habitat: base_angle_noise: 0.0 base_noise: 0.0 constraint_violation_ends_episode: False - force_regenerate: True + environment: max_episode_steps: 0 simulator: - #type: RearrangeSim-v0 seed: 100 additional_object_paths: - "data/objects/ycb/configs/" @@ -75,17 +68,17 @@ habitat: radius: 0.3 sim_sensors: head_rgb_sensor: - height: 128 - width: 128 + height: 224 + width: 224 head_depth_sensor: - height: 128 - width: 128 + height: 224 + width: 224 arm_depth_sensor: - height: 128 - width: 128 + height: 224 + width: 224 arm_rgb_sensor: - height: 128 - width: 128 + height: 224 + width: 224 habitat_sim_v0: enable_physics: False dataset: diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml index b49ba6b443..67b87cfdc3 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml @@ -1,22 +1,7 @@ # @package _global_ defaults: - pick_murp - # - /habitat/task/lab_sensors: - # - arm_depth_bbox_sensor - # TODO: jimmy: remove this since the we are not able to use API to get the object handle - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: murp_agent - override /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp - - override /habitat/task/rearrange/actions: murp_base_arm_empty + - override /habitat/task/rearrange/actions: murp_base_arm - _self_ - -# habitat: -# task: -# # lab_sensors: -# # arm_depth_bbox_sensor: -# # height: 240 -# # width: 228 -# # TODO: jimmy: remove this since the we are not able to use API to get the object handle -# actions: -# arm_action: -# center_cone_vector: [0.0, 1.0, 0.0] -# auto_grasp: False diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index 95ae55c7c1..5dbf31dafc 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -290,6 +290,9 @@ class ArmReachEEActionConfig(ActionConfig): """ type: str = "ArmReachEEAction" right_left_hand: str = "right" + max_ee_xyz_movement: float = 0.1 + max_ee_rpy_movement: float = 0.52 # 30 degree + max_finger_movement: float = 0.26 # 15 degree @dataclass diff --git a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml index 7949624005..ab0b32a5d5 100644 --- a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml +++ b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml @@ -4,34 +4,15 @@ defaults: - /habitat/simulator/sim_sensors@sim_sensors.head_depth_sensor: head_depth_sensor - /habitat/simulator/sim_sensors@sim_sensors.arm_rgb_sensor: arm_rgb_sensor # here for cameras - /habitat/simulator/sim_sensors@sim_sensors.arm_depth_sensor: arm_depth_sensor # here for cameras - - /habitat/simulator/sim_sensors@sim_sensors.arm_panoptic_sensor: arm_panoptic_sensor # here for cameras - - /habitat/simulator/sim_sensors@sim_sensors.head_stereo_left_depth_sensor: head_stereo_left_depth_sensor # here for cameras - - /habitat/simulator/sim_sensors@sim_sensors.head_stereo_right_depth_sensor: head_stereo_right_depth_sensor # here for cameras sim_sensors: arm_rgb_sensor: - height: 480 - width: 640 + height: 224 + width: 224 hfov: 47 arm_depth_sensor: - height: 240 - width: 228 + height: 224 + width: 224 hfov: 60 min_depth: 0.0 max_depth: 1.7 - arm_panoptic_sensor: - height: 240 - width: 228 - hfov: 60 - head_stereo_right_depth_sensor: - height: 212 - width: 120 - hfov: 58 - min_depth: 0.0 - max_depth: 3.5 - head_stereo_left_depth_sensor: - height: 212 - width: 120 - hfov: 58 - min_depth: 0.0 - max_depth: 3.5 diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm.yaml similarity index 93% rename from habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml rename to habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm.yaml index 6d7fd1ee13..aff2b09908 100644 --- a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm_empty.yaml +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm.yaml @@ -5,7 +5,6 @@ defaults: - /habitat/task/actions: - arm_reach_ee - rearrange_stop - # - empty # remove empty action - _self_ # arm_action: # type: "ArmAction" diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 9569c92d55..5d53954ae9 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -1054,6 +1054,15 @@ def step(self, *args, **kwargs): delta_rot = kwargs[self._action_arg_prefix + "target_rot"] finger = kwargs[self._action_arg_prefix + "target_finger"] + # Cap the joints + delta_pos = ( + np.clip(delta_pos, -1, 1) * self._config.max_ee_xyz_movement + ) + delta_rot = ( + np.clip(delta_rot, -1, 1) * self._config.max_ee_rpy_movement + ) + finger = np.clip(finger, -1, 1) * self._config.max_finger_movement + # Update the target joint location self.ee_target += np.array(delta_pos) self.ee_rot_target += np.array(delta_rot) From 8d14482f74781ef844c9c8cae9e3df2c927893b5 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 10:32:13 -0400 Subject: [PATCH 41/50] make the door orientation correct --- habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py | 7 ++++++- .../rearrange/sub_tasks/articulated_object_sensors.py | 6 +++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py index 9b41a96e8a..6de9c96cc1 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -335,7 +335,12 @@ def _get_observation_space(self, *args, config, **kwargs): ) def get_observation(self, observations, task, episode, *args, **kwargs): - return np.array([get_door_quat(task)[0]], dtype=np.float32) + # Fully close, the value is -180 degree + # when opening, the value is 90 degree + # it can be opened to 45 degree + # We use absolute value here to indicate openning or not + angle = get_door_quat(task)[0] + return np.deg2rad(np.array([abs(angle)], dtype=np.float32)) @registry.register_sensor diff --git a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py index ec73fcf9a7..23b2b6270c 100644 --- a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py @@ -182,7 +182,11 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): def update_metric(self, *args, episode, task, observations, **kwargs): if type(task._sim) == IsaacRearrangeSim: - rpy = get_door_quat(task) + # Fully close, the value is -180 degree + # when opening, the value is 90 degree + # it can be opened to 45 degree + # We use absolute value here to indicate openning or not + rpy = np.deg2rad(abs(get_door_quat(task))) self._metric = rpy[0] else: self._metric = task.get_use_marker().get_targ_js() From 74e25b9f23de2f0bb046a7a2af132908ae2f554d Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 11:03:40 -0400 Subject: [PATCH 42/50] adjust head images --- .../articulated_agents/robots/murp_robot.py | 16 +++++++++++++--- .../benchmark/rearrange/play/pick_murp.yaml | 1 + .../simulator/sensor_setups/murp_agent.yaml | 2 +- 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py index d60e3af18a..a5acdab7fc 100644 --- a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -98,13 +98,13 @@ def _get_murp_params(cls): "articulated_agent_arm_depth": ArticulatedAgentCameraParams( cam_offset_pos=mn.Vector3(0.166, 0.0, 0.018), cam_orientation=mn.Vector3(0.0, -1.571, 0.0), - attached_link_id=36, + attached_link_id=67, # 36 for left hand; 67 for the right hand relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)), ), "articulated_agent_arm_rgb": ArticulatedAgentCameraParams( cam_offset_pos=mn.Vector3(0.166, 0.023, 0.03), cam_orientation=mn.Vector3(0, -1.571, 0.0), - attached_link_id=36, + attached_link_id=67, relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)), ), "articulated_agent_arm_panoptic": ArticulatedAgentCameraParams( @@ -131,9 +131,19 @@ def _get_murp_params(cls): ), attached_link_id=-1, ), + "head_depth": ArticulatedAgentCameraParams( + # x: forward; y: up; z: left + cam_offset_pos=mn.Vector3(0.4, 0.75, 0), + cam_orientation=mn.Vector3(0.0, -1.571, 0.0), + attached_link_id=-1, + ), + "head_rgb": ArticulatedAgentCameraParams( + cam_offset_pos=mn.Vector3(0.4, 0.75, 0), + cam_orientation=mn.Vector3(0.0, -1.571, 0.0), + attached_link_id=-1, + ), "third": ArticulatedAgentCameraParams( cam_offset_pos=mn.Vector3(0.5, 2.5, 0.0), - # cam_look_at_pos=mn.Vector3(1, 0.0, -0.75), cam_look_at_pos=mn.Vector3(1, 0.0, 0), attached_link_id=-1, ), diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index 87714363dd..e554a385d5 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -35,6 +35,7 @@ habitat: gym: obs_keys: - articulated_agent_arm_depth # depth image of the arm: 224 by 224 + - head_depth # depth image of the head: 224 by 224 - joint # joint angle of the arm: 7-dim - hand_joint # joint angle of the hand: 16-dim - obj_start_sensor # relative position from ee to target object: 3-dim diff --git a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml index ab0b32a5d5..55a310b963 100644 --- a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml +++ b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml @@ -9,7 +9,7 @@ sim_sensors: arm_rgb_sensor: height: 224 width: 224 - hfov: 47 + hfov: 60 arm_depth_sensor: height: 224 width: 224 From ca42e080c25913c8f0efc01401ead1e7c825e6a1 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 11:30:10 -0400 Subject: [PATCH 43/50] polish reward design --- .../benchmark/rearrange/play/pick_murp.yaml | 26 ++++++++++++++----- .../sub_tasks/articulated_object_sensors.py | 10 ++++--- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index e554a385d5..5847f77396 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -44,22 +44,36 @@ habitat: task: type: RearrangePickTask-v0 count_obj_collisions: True - desired_resting_position: [0.5, 0.0, 1.0] reward_measure: art_obj_reward success_measure: art_obj_success - - # Reach task config render_target: True ee_sample_factor: 0.8 - - # In radians base_angle_noise: 0.0 base_noise: 0.0 constraint_violation_ends_episode: False force_regenerate: True + # Things added for Murp + desired_resting_position: [0.58, 0.00, 1.30] + success_state: 1.5708 # 90 degree for the fridge door + end_on_success: True + success_reward: 2.5 + slack_reward: -0.01 + measurements: + art_obj_reward: + success_js_state: 1.5708 # 90 degree for the fridge door + art_dist_reward: 10.0 + art_at_desired_state_reward: 5.0 + ee_dist_reward: 10.0 + art_obj_at_desired_state: + success_dist_threshold: 0.17 # 10 degree + success_js_state: 1.5708 # 90 degree for the fridge door + art_obj_success: + rest_dist_threshold: 0.15 + must_call_stop: False + environment: - max_episode_steps: 0 + max_episode_steps: 1500 simulator: seed: 100 additional_object_paths: diff --git a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py index 23b2b6270c..4ff9deb644 100644 --- a/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/sub_tasks/articulated_object_sensors.py @@ -215,7 +215,9 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): def update_metric(self, *args, episode, task, observations, **kwargs): if type(task._sim) == IsaacRearrangeSim: - dist = self._config.success_js_state - get_door_quat(task)[0] + dist = self._config.success_js_state - np.deg2rad( + abs(get_door_quat(task)[0]) + ) else: dist = task.success_js_state - task.get_use_marker().get_targ_js() @@ -270,7 +272,6 @@ def update_metric(self, *args, episode, task, observations, **kwargs): self._metric = ( is_art_obj_state_succ and ee_to_rest_distance < self._config.rest_dist_threshold - and not self._sim.grasp_mgr.is_grasped ) if self._config.must_call_stop: if called_stop: @@ -357,7 +358,7 @@ def reset_metric(self, *args, episode, task, observations, **kwargs): ].get_metric() self._prev_art_state = link_state - # TODO: jimmy: havre to implement grasping logics + # TODO: jimmy: have to implement grasping logics self._any_has_grasped = False # task._sim.grasp_mgr.is_grasped self._prev_ee_dist_to_marker = dist_to_marker self._prev_ee_to_rest = ee_to_rest_distance @@ -405,7 +406,7 @@ def update_metric(self, *args, episode, task, observations, **kwargs): if not is_art_obj_state_succ: reward += self._config.art_dist_reward * dist_diff - # TODO: jimmy: havre to implement grasping logics + # TODO: jimmy: have to implement grasping logics cur_has_grasped = False # task._sim.grasp_mgr.is_grasped if type(task._sim) == IsaacRearrangeSim: @@ -416,6 +417,7 @@ def update_metric(self, *args, episode, task, observations, **kwargs): cur_ee_dist_to_marker = task.measurements.measures[ EndEffectorDistToMarker.cls_uuid ].get_metric() + if cur_has_grasped and not self._any_has_grasped: if task._sim.grasp_mgr.snapped_marker_id != task.use_marker_name: # Grasped wrong marker From 347a25dc851cafd0ca980ce7cd31e72d012a139c Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 12:09:30 -0400 Subject: [PATCH 44/50] fix the bug of target location. it should be ee not base --- .../habitat/config/benchmark/rearrange/play/pick_murp.yaml | 3 ++- habitat-lab/habitat/tasks/rearrange/actions/actions.py | 6 +++--- habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml index 5847f77396..49de50eee0 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml @@ -58,13 +58,14 @@ habitat: success_state: 1.5708 # 90 degree for the fridge door end_on_success: True success_reward: 2.5 - slack_reward: -0.01 + slack_reward: -0.1 measurements: art_obj_reward: success_js_state: 1.5708 # 90 degree for the fridge door art_dist_reward: 10.0 art_at_desired_state_reward: 5.0 ee_dist_reward: 10.0 + marker_dist_reward: 5.0 art_obj_at_desired_state: success_dist_threshold: 0.17 # 10 degree success_js_state: 1.5708 # 90 degree for the fridge door diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 5d53954ae9..af9f32f1de 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -1094,6 +1094,6 @@ def step(self, *args, **kwargs): # self._sim.articulated_agent._robot_wrapper.right_arm_joint_pos # ) # ) - print(f"self.ee_target: {self.ee_target}") - print(f"self.ee_rot_target: {self.ee_rot_target}") - print(f"self.target_finger: {self.target_finger}") + # print(f"self.ee_target: {self.ee_target}") + # print(f"self.ee_rot_target: {self.ee_rot_target}") + # print(f"self.target_finger: {self.target_finger}") diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index 1d5ab286c9..759791a7d7 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -443,7 +443,7 @@ def reconfigure(self, config: "DictConfig", ep_info: RearrangeEpisode): self._setup_targets(ep_info) # Set the target start pos self.target_start_pos = np.array( - [self._targets[key]["base"][0] for key in self._targets] + [self._targets[key]["ee"][0] for key in self._targets] ) return From 6479d76da298cd83961582336dd9b896c7815c4c Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 19 Mar 2025 12:35:26 -0400 Subject: [PATCH 45/50] fix bug --- examples/interactive_play.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 2e85590b46..4f07fb4b39 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -116,7 +116,7 @@ def get_input_vel_ctlr( ee_hand_control="ee", ): if skip_pygame: - return step_env(env, "empty", {}), None, False + return step_env(env, "rearrange_stop", {}), None, False multi_agent = len(env._sim.agents_mgr) > 1 if multi_agent: From f3d5791b48951948468255b756aa6666b42d5136 Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Wed, 19 Mar 2025 17:55:49 +0000 Subject: [PATCH 46/50] flexible way to switch urdf --- examples/interactive_play.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 4f07fb4b39..7316c7818d 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -91,7 +91,6 @@ NAMED_WINDOW = "Play Murp" USE_CV2 = True -TEST_MACHINE = "lambda" # h200 / lambda # cv2 relative functions @@ -928,6 +927,13 @@ def has_pygame(): "--walk-pose-path", type=str, default=DEFAULT_POSE_PATH ) + parser.add_argument( + "--use-h200", + action="store_true", + default=False, + help="If we want to run the test in h200 aws", + ) + args = parser.parse_args() if not has_pygame() and not args.no_render: raise ImportError( @@ -983,15 +989,12 @@ def has_pygame(): ) ik_arm_urdf = "" + # Make sure we use the correct urdf for the arm control to compute IK - if TEST_MACHINE == "h200": + if args.use_h200: ik_arm_urdf = "/home/jimmytyyang/research/hab_training/habitat-lab/data/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" - elif TEST_MACHINE == "lambda": - ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" else: - raise ValueError( - f"Cannot recongize the TEST_MACHINE: {TEST_MACHINE}" - ) + ik_arm_urdf = "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf" sim_config.agents.main_agent.ik_arm_urdf = ik_arm_urdf # task_config.actions.arm_action.arm_controller = "ArmEEAction" From 6970f1c03c0797ae80997889abfefc4535622564 Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Wed, 19 Mar 2025 19:19:34 +0000 Subject: [PATCH 47/50] move the yaml --- examples/interactive_play.py | 2 +- .../habitat/config/benchmark/rearrange/play/play_murp.yaml | 7 ------- .../benchmark/rearrange/{play => skills}/pick_murp.yaml | 5 +++-- 3 files changed, 4 insertions(+), 10 deletions(-) delete mode 100644 habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml rename habitat-lab/habitat/config/benchmark/rearrange/{play => skills}/pick_murp.yaml (98%) diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 7316c7818d..606493e008 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -82,7 +82,7 @@ # Please reach out to the paper authors to obtain this file DEFAULT_POSE_PATH = "data/humanoids/humanoid_data/walking_motion_processed.pkl" -DEFAULT_CFG = "benchmark/rearrange/play/play_murp.yaml" +DEFAULT_CFG = "benchmark/rearrange/skills/pick_murp.yaml" DEFAULT_RENDER_STEPS_LIMIT = 60 SAVE_VIDEO_DIR = "./data/vids" SAVE_ACTIONS_DIR = "./data/interactive_play_replays" diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml deleted file mode 100644 index 67b87cfdc3..0000000000 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play_murp.yaml +++ /dev/null @@ -1,7 +0,0 @@ -# @package _global_ -defaults: - - pick_murp - - override /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: murp_agent - - override /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp - - override /habitat/task/rearrange/actions: murp_base_arm - - _self_ diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml similarity index 98% rename from habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml rename to habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml index 49de50eee0..adbb173e75 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml @@ -3,8 +3,9 @@ defaults: - /habitat: habitat_config_base - /habitat/simulator: isaac_rearrange_sim - - /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: rgbd_head_rgbd_arm_agent - - /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp_agent + - /habitat/simulator/sensor_setups@habitat.simulator.agents.main_agent: murp_agent + - /habitat/simulator/agents@habitat.simulator.agents.main_agent: murp + - /habitat/task: task_config_base - /habitat/task/rearrange/actions: murp_base_arm - /habitat/task/measurements: From 6b26003807fce091bdfddd04a9d4bcb0873cedda Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Wed, 19 Mar 2025 19:49:47 +0000 Subject: [PATCH 48/50] space dict for reach ee --- .../habitat/tasks/rearrange/actions/actions.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index af9f32f1de..afab475d7a 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -921,7 +921,19 @@ def __init__(self, *args, **kwargs): @property def action_space(self): # 6-dim for the ee, x, y, z, roll, pitch, yaw, and 16-dim for the hand - return spaces.Box(shape=(22,), low=-1, high=1, dtype=np.float32) + return spaces.Dict( + { + f"{self._action_arg_prefix}target_pos": spaces.Box( + shape=(3,), low=-1, high=1, dtype=np.float32 + ), + f"{self._action_arg_prefix}target_rot": spaces.Box( + shape=(3,), low=-1, high=1, dtype=np.float32 + ), + f"{self._action_arg_prefix}target_finger": spaces.Box( + shape=(16,), low=-1, high=1, dtype=np.float32 + ), + } + ) def reset(self, *args, **kwargs): try: From 2629c6dcc5b04fa7bba1544d4ba891c0103126b8 Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Wed, 19 Mar 2025 20:14:52 +0000 Subject: [PATCH 49/50] add IK file --- .../habitat/config/benchmark/rearrange/skills/pick_murp.yaml | 1 + habitat-lab/habitat/tasks/rearrange/actions/actions.py | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml index adbb173e75..987305e3eb 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml @@ -96,6 +96,7 @@ habitat: arm_rgb_sensor: height: 224 width: 224 + ik_arm_urdf: /home/jimmytyyang/research/hab_training/habitat-lab/data/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf habitat_sim_v0: enable_physics: False dataset: diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index afab475d7a..67a18db6fe 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -955,7 +955,8 @@ def reset(self, *args, **kwargs): self.target_finger = ( self._robot_wrapper._target_hand_joint_positions ) - except: + except Exception as e: + print(f"Arm Reach EE Action issue: {e}") self.ee_target = None self.ee_rot_target = None self.target_finger = None From 80946a4bf4219fdf17f4de83bf1b0b3acdc8be82 Mon Sep 17 00:00:00 2001 From: jimmytyyang user Date: Wed, 26 Mar 2025 14:35:00 +0000 Subject: [PATCH 50/50] disable rgb and depth images in yaml to improve training fps --- .../benchmark/rearrange/skills/pick_murp.yaml | 31 +++++++++-------- .../simulator/sensor_setups/murp_agent.yaml | 34 ++++++++++--------- 2 files changed, 34 insertions(+), 31 deletions(-) diff --git a/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml index 987305e3eb..9f921d0542 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml @@ -35,8 +35,9 @@ defaults: habitat: gym: obs_keys: - - articulated_agent_arm_depth # depth image of the arm: 224 by 224 - - head_depth # depth image of the head: 224 by 224 + # Disable any rendering of the images to improve performance + # - articulated_agent_arm_depth # depth image of the arm: 224 by 224 + # - head_depth # depth image of the head: 224 by 224 - joint # joint angle of the arm: 7-dim - hand_joint # joint angle of the hand: 16-dim - obj_start_sensor # relative position from ee to target object: 3-dim @@ -83,19 +84,19 @@ habitat: agents: main_agent: radius: 0.3 - sim_sensors: - head_rgb_sensor: - height: 224 - width: 224 - head_depth_sensor: - height: 224 - width: 224 - arm_depth_sensor: - height: 224 - width: 224 - arm_rgb_sensor: - height: 224 - width: 224 + # sim_sensors: + # head_rgb_sensor: + # height: 224 + # width: 224 + # head_depth_sensor: + # height: 224 + # width: 224 + # arm_depth_sensor: + # height: 224 + # width: 224 + # arm_rgb_sensor: + # height: 224 + # width: 224 ik_arm_urdf: /home/jimmytyyang/research/hab_training/habitat-lab/data/franka_tmr/franka_description_tmr/urdf/franka_tmr_right_arm_only.urdf habitat_sim_v0: enable_physics: False diff --git a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml index 55a310b963..182afd4f7c 100644 --- a/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml +++ b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml @@ -1,18 +1,20 @@ # @package habitat.simulator.agents.murp_agent -defaults: - - /habitat/simulator/sim_sensors@sim_sensors.head_rgb_sensor: head_rgb_sensor - - /habitat/simulator/sim_sensors@sim_sensors.head_depth_sensor: head_depth_sensor - - /habitat/simulator/sim_sensors@sim_sensors.arm_rgb_sensor: arm_rgb_sensor # here for cameras - - /habitat/simulator/sim_sensors@sim_sensors.arm_depth_sensor: arm_depth_sensor # here for cameras +# Disable rendering of the images +# defaults: +# - /habitat/simulator/sim_sensors@sim_sensors.head_rgb_sensor: head_rgb_sensor +# - /habitat/simulator/sim_sensors@sim_sensors.head_depth_sensor: head_depth_sensor +# - /habitat/simulator/sim_sensors@sim_sensors.arm_rgb_sensor: arm_rgb_sensor # here for cameras +# - /habitat/simulator/sim_sensors@sim_sensors.arm_depth_sensor: arm_depth_sensor # here for cameras -sim_sensors: - arm_rgb_sensor: - height: 224 - width: 224 - hfov: 60 - arm_depth_sensor: - height: 224 - width: 224 - hfov: 60 - min_depth: 0.0 - max_depth: 1.7 +# Disable rendering of the images +# sim_sensors: +# arm_rgb_sensor: +# height: 224 +# width: 224 +# hfov: 60 +# arm_depth_sensor: +# height: 224 +# width: 224 +# hfov: 60 +# min_depth: 0.0 +# max_depth: 1.7