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 8d215ac425..606493e008 100644 --- a/examples/interactive_play.py +++ b/examples/interactive_play.py @@ -78,12 +78,24 @@ 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.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" +# 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): @@ -99,16 +111,22 @@ def get_input_vel_ctlr( agent_to_control, control_humanoid, humanoid_controller, + key=None, + 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: 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" @@ -116,26 +134,33 @@ def get_input_vel_ctlr( else: 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" 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(22) + 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: 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]) @@ -157,16 +182,16 @@ def get_input_vel_ctlr( if not_block_input: # Base control - if keys[pygame.K_j]: + if keys[pygame.K_j] or key == ord("l"): # Left base_action = [0, 1] - elif keys[pygame.K_l]: + elif keys[pygame.K_l] or key == ord("'"): # Right base_action = [0, -1] - elif keys[pygame.K_k]: + elif keys[pygame.K_k] or key == ord(";"): # Back base_action = [-1, 0] - elif keys[pygame.K_i]: + elif keys[pygame.K_i] or key == ord("p"): # Forward base_action = [1, 0] @@ -207,6 +232,101 @@ def get_input_vel_ctlr( elif keys[pygame.K_7]: arm_action[6] = -1.0 + 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 # This is for Spot robot which a user can only control the effective arm in the real robot @@ -267,6 +387,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,7 +518,14 @@ 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:6], + "target_finger": arm_action[6:], + } + else: + args = {arm_key: arm_action, grip_key: magic_grasp} if magic_grasp is None: arm_action = [*arm_action, 0.0] @@ -458,6 +612,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: @@ -470,13 +634,19 @@ 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, {}) - pygame.init() - 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]] # type: ignore + ) # type: ignore update_idx = 0 target_fps = 60.0 @@ -497,7 +667,15 @@ def play_env(env, args, config): humanoid_controller = HumanoidRearrangeController(args.walk_pose_path) 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 @@ -530,6 +708,8 @@ def play_env(env, args, config): agent_to_control, 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]: @@ -583,6 +763,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]] @@ -605,10 +786,15 @@ def play_env(env, args, config): 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_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] @@ -741,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( @@ -758,7 +951,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 ) } @@ -788,16 +981,26 @@ 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" + + ik_arm_urdf = "" + + # Make sure we use the correct urdf for the arm control to compute IK + 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" + else: + 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" 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 dfa70e7543..a5acdab7fc 100644 --- a/habitat-lab/habitat/articulated_agents/robots/murp_robot.py +++ b/habitat-lab/habitat/articulated_agents/robots/murp_robot.py @@ -93,18 +93,18 @@ def _get_murp_params(cls): 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), 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,20 @@ 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, 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), attached_link_id=-1, ), }, @@ -153,9 +164,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.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml index 99ab8a0724..1832b806bd 100644 --- a/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml +++ b/habitat-lab/habitat/config/benchmark/rearrange/play/play.yaml @@ -2,14 +2,14 @@ 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 - /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 @@ -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/" @@ -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 diff --git a/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml new file mode 100644 index 0000000000..9f921d0542 --- /dev/null +++ b/habitat-lab/habitat/config/benchmark/rearrange/skills/pick_murp.yaml @@ -0,0 +1,104 @@ +# @package _global_ +defaults: + - /habitat: habitat_config_base + + - /habitat/simulator: isaac_rearrange_sim + - /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: + - 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 + - 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 + - door_orientation_sensor + + - /habitat/dataset/rearrangement: replica_cad + - _self_ + +# Config for empty task to explore the scene. +habitat: + gym: + obs_keys: + # 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 + - relative_resting_position # relative resting x,y,z location for the ee: 3-dim + - door_orientation # door yaw: 1-dim + task: + type: RearrangePickTask-v0 + count_obj_collisions: True + reward_measure: art_obj_reward + success_measure: art_obj_success + render_target: True + ee_sample_factor: 0.8 + 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.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 + art_obj_success: + rest_dist_threshold: 0.15 + must_call_stop: False + + environment: + max_episode_steps: 1500 + simulator: + seed: 100 + additional_object_paths: + - "data/objects/ycb/configs/" + 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 + 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: + data_path: data/datasets/replica_cad/rearrange/v1/{split}/rearrange_easy.json.gz diff --git a/habitat-lab/habitat/config/default_structured_configs.py b/habitat-lab/habitat/config/default_structured_configs.py index 1394fb417e..5dbf31dafc 100644 --- a/habitat-lab/habitat/config/default_structured_configs.py +++ b/habitat-lab/habitat/config/default_structured_configs.py @@ -60,6 +60,8 @@ "IsHoldingSensorConfig", "EEPositionSensorConfig", "JointSensorConfig", + "HandJointSensorConfig", + "DoorOrientationSensorConfig", "HumanoidJointSensorConfig", "TargetStartSensorConfig", "GoalSensorConfig", @@ -269,6 +271,30 @@ 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 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" + 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 class BaseVelocityNonCylinderActionConfig(ActionConfig): r""" @@ -515,6 +541,26 @@ class JointSensorConfig(LabSensorConfig): type: str = "JointSensor" dimensionality: int = 7 arm_joint_mask: Optional[List[int]] = None + right_left_hand: str = "right" + + +@dataclass +class HandJointSensorConfig(LabSensorConfig): + r""" + Rearrangement only. Returns the hand joint positions of the robot. + """ + type: str = "HandJointSensor" + dimensionality: int = 16 + right_left_hand: str = "right" + + +@dataclass +class DoorOrientationSensorConfig(LabSensorConfig): + r""" + The door roll + """ + type: str = "DoorOrientationSensor" + dimensionality: int = 1 @dataclass @@ -891,6 +937,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 @@ -947,6 +994,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 @@ -1661,6 +1709,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 @@ -1969,6 +2019,18 @@ 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.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", @@ -2229,6 +2291,18 @@ 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.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/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 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/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..182afd4f7c --- /dev/null +++ b/habitat-lab/habitat/config/habitat/simulator/sensor_setups/murp_agent.yaml @@ -0,0 +1,20 @@ +# @package habitat.simulator.agents.murp_agent +# 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 + +# 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 diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm.yaml new file mode 100644 index 0000000000..aff2b09908 --- /dev/null +++ b/habitat-lab/habitat/config/habitat/task/rearrange/actions/murp_base_arm.yaml @@ -0,0 +1,20 @@ +# @package habitat.task.actions +defaults: + - /habitat/task/actions: + - base_vel_isaac # BaseVelIsaacAction + - /habitat/task/actions: + - arm_reach_ee + - rearrange_stop + - _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 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/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/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..62126fbec5 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,12 +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 + 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 add_reference_to_stage(usd_path=asset_path, prim_path=robot_prim_path) self._isaac_service.usd_visualizer.on_add_reference_to_stage( @@ -48,7 +51,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 +59,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 +66,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 +110,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 +117,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 +129,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 +148,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 @@ -207,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" ) @@ -226,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 [ @@ -296,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) - 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 + # 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 = 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 @@ -311,7 +306,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 +317,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,10 +383,9 @@ 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 + z_target = 0.1 # todo: get from navmesh or assume ground_z==0 max_linear_vel = 3.0 # Extract the vertical position and velocity @@ -416,7 +408,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 +420,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 +432,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 +444,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 +456,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 ) @@ -482,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 @@ -536,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 @@ -546,14 +537,19 @@ 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) - 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 @@ -566,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/actions.py b/habitat-lab/habitat/isaac_sim/actions.py index 37fceb564d..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,11 +9,14 @@ BaseVelAction, ) +# from gym import spaces + @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: @@ -22,6 +24,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] ) @@ -44,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() + pass + # 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 - - 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 @@ -71,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) @@ -117,12 +120,11 @@ 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 class BaseVelKinematicIsaacAction(BaseVelAction): - def update_base(self): ctrl_freq = self._sim.ctrl_freq trans = self.cur_articulated_agent.base_transformation diff --git a/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py b/habitat-lab/habitat/isaac_sim/isaac_mobile_manipulator.py index d90afd8ad9..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 @@ -119,8 +119,8 @@ def reset(self) -> None: @property def arm_joint_pos(self): - assert False # todo - pass + #assert False # todo + return self._robot_wrapper.arm_joint_pos @arm_joint_pos.setter def arm_joint_pos(self, ctrl: List[float]): @@ -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/isaac_sim/isaac_murp_robot.py b/habitat-lab/habitat/isaac_sim/isaac_murp_robot.py index 339bc33709..3bb3dae183 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 @@ -102,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 b62a35632d..67a18db6fe 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -9,10 +9,12 @@ 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 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, @@ -137,9 +139,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): @@ -229,37 +231,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 +272,9 @@ 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 @@ -644,7 +648,10 @@ 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 +675,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. @@ -786,7 +798,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( @@ -863,3 +875,238 @@ 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"] + 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] + ) + + 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( + linear_vel_usd + ) + + +@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): + # 6-dim for the ee, x, y, z, roll, pitch, yaw, and 16-dim for the hand + 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: + 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 + ) + 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 + + def calc_desired_joints(self): + 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) + + 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 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, + ] + ), + "retract": np.array( + [ + 2.6116285, + 1.5283098, + 1.5708, + -0.50559217, + -1.5708, + 1.5708, + -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("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("retract") + ) + self._robot_wrapper._target_right_hand_joint_positions = ( + 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"] + + # 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) + 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) + + 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 + ) + + # 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"self.ee_rot_target: {self.ee_rot_target}") + # print(f"self.target_finger: {self.target_finger}") 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/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): diff --git a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py index 3815823ed1..759791a7d7 100644 --- a/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py +++ b/habitat-lab/habitat/tasks/rearrange/isaac_rearrange_sim.py @@ -24,6 +24,7 @@ import numpy as np import numpy.typing as npt +import habitat import habitat_sim # flake8: noqa @@ -72,7 +73,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) @@ -102,7 +102,10 @@ def bind_physics_material_to_hierarchy( @registry.register_simulator(name="IsaacRearrangeSim-v0") class IsaacRearrangeSim(HabitatSim): def __init__(self, config: "DictConfig"): - config.scene = "NONE" + 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(): @@ -114,9 +117,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 +139,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 @@ -147,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 @@ -169,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 @@ -304,6 +308,7 @@ def _get_target_trans(self): 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 @@ -394,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) @@ -402,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 @@ -433,78 +439,85 @@ def reconfigure(self, config: "DictConfig", ep_info: RearrangeEpisode): for ao in self.art_objs } - 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) + # use target 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]["ee"][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): @@ -541,7 +554,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 @@ -574,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): @@ -698,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] @@ -971,7 +1048,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) @@ -1189,95 +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..6de9c96cc1 100644 --- a/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py +++ b/habitat-lab/habitat/tasks/rearrange/rearrange_sensors.py @@ -9,12 +9,14 @@ 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 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, @@ -28,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. @@ -76,8 +109,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 +130,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) @@ -235,14 +269,80 @@ 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) +@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): + 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) + + +@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): + # 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 class HumanoidJointSensor(UsesArticulatedAgentInterface, Sensor): def __init__(self, sim, config, *args, **kwargs): @@ -637,10 +737,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) @@ -802,14 +904,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 @@ -850,32 +961,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 @@ -1042,16 +1162,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..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 @@ -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,19 @@ 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: + # 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() @registry.register_measure @@ -168,11 +210,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): - dist = task.success_js_state - task.get_use_marker().get_targ_js() + if type(task._sim) == IsaacRearrangeSim: + 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() # If not absolute distance, we can have a joint state greater than the # target. @@ -205,7 +252,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): @@ -225,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: @@ -252,7 +298,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 +344,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: 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 self._any_at_desired_state = False @@ -316,7 +368,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 +377,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 +392,32 @@ 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: have to implement grasping logics + cur_has_grasped = False # task._sim.grasp_mgr.is_grasped + + 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() - 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 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() diff --git a/heuristic_expert_w_grasp.py b/heuristic_expert_w_grasp.py index 00e67b9b0d..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 @@ -49,11 +47,11 @@ 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: - data_path = "/home/joanne/habitat-lab/data/" + data_path = "home/jmmy/research/hab_training/habitat-lab/data/" def make_sim_cfg(agent_dict): @@ -131,18 +129,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, @@ -154,14 +140,11 @@ 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 + "/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( - data_path, - "franka_tmr/franka_description_tmr/allegro/allegro.urdf", # Lambda Change + "/home/jmmy/research/hab_training/murp/murp/platforms/franka_tmr/franka_description_tmr/urdf/franka_right_arm.urdf" ) ->>>>>>> 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 @@ -247,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" @@ -256,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] @@ -271,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 @@ -399,7 +388,7 @@ 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 im = process_obs_img(obs) @@ -497,10 +486,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 +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): + 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 +516,34 @@ 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.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] + self.grasp_fingers = self.target_joints.copy() + self.close_fingers = self.target_joints.copy() + self.close_fingers[BASE_THUMB_JOINT] += 1.1 + # self.close_fingers[CURVE_JOINTS] -=0.5 + self.close_fingers[SECONDARY_JOINTS] += 0.7 + self.close_fingers[TERTIARY_JOINTS] += 1.0 + self.close_fingers[OPEN_JOINTS] += 0.7 + + def get_targets(self, name="target", hand="right"): # Lambda Changes if name == "target": return ( @@ -556,8 +562,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[1] -= 0.1 + self.open_xyz[0] += 0.1 return ( torch.tensor(self.close_fingers, device="cuda:0"), @@ -602,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 @@ -626,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() @@ -660,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 @@ -685,23 +695,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"]) @@ -732,29 +742,23 @@ 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 ) 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 = ( @@ -764,25 +768,39 @@ 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=30, open_iters=30, move_iters=19 + ) if __name__ == "__main__": @@ -790,7 +808,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 +816,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") 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.