From 3cb0bb70bfd6d7ef3d724259441247f515ef773f Mon Sep 17 00:00:00 2001 From: Ruslan Date: Fri, 3 Nov 2023 15:38:52 +0200 Subject: [PATCH 1/6] Using cleaner Spot-floating bug fix (#1661) * fix spot floating bug --- habitat-lab/habitat/tasks/rearrange/actions/actions.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index 99a5796ecf..bf3a9612e2 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -409,9 +409,12 @@ def update_base(self, fix_leg=False): end_pos = self._sim.step_filter( rigid_state.translation, target_rigid_state.translation ) - # Offset the base if the base height is different between end_pos - # and the current state - if end_pos[1] != rigid_state.translation[1]: + + # try_step may fail, in which case it simply returns the start argument + did_try_step_fail = end_pos == rigid_state.translation + if not did_try_step_fail: + # If try_step succeeded, it snapped our start position to the navmesh + # We should apply the base offset end_pos -= self.cur_articulated_agent.params.base_offset target_trans = mn.Matrix4.from_( From 0d35d37f8f5b56eaca25a4951fb26cf4330166bf Mon Sep 17 00:00:00 2001 From: rpartsey Date: Sat, 4 Nov 2023 18:46:53 +0200 Subject: [PATCH 2/6] Ruta's rearrange study progress_str changes --- .../app_states/app_state_free_camera.py | 2 +- .../app_states/app_state_rearrange.py | 2 +- .../app_states/app_state_socialnav.py | 308 ++++++++++++++++++ .../app_states/app_state_socialnav_study.py | 2 +- 4 files changed, 311 insertions(+), 3 deletions(-) create mode 100644 examples/siro_sandbox/app_states/app_state_socialnav.py diff --git a/examples/siro_sandbox/app_states/app_state_free_camera.py b/examples/siro_sandbox/app_states/app_state_free_camera.py index bcbd8ea8a7..b911c6786b 100644 --- a/examples/siro_sandbox/app_states/app_state_free_camera.py +++ b/examples/siro_sandbox/app_states/app_state_free_camera.py @@ -134,7 +134,7 @@ def _update_help_text(self): self._episode_helper.num_iter_episodes - self._episode_helper.num_episodes_done ) - progress_str = f"{num_episodes_remaining} episodes remaining" + progress_str = f"{num_episodes_remaining} episodes left" self._sandbox_service.text_drawer.add_text( progress_str, TextOnScreenAlignment.TOP_RIGHT, diff --git a/examples/siro_sandbox/app_states/app_state_rearrange.py b/examples/siro_sandbox/app_states/app_state_rearrange.py index 47161e7f5c..6f7b9f1d2b 100644 --- a/examples/siro_sandbox/app_states/app_state_rearrange.py +++ b/examples/siro_sandbox/app_states/app_state_rearrange.py @@ -360,7 +360,7 @@ def _update_help_text(self): self._episode_helper.num_iter_episodes - self._episode_helper.num_episodes_done ) - progress_str = f"{num_episodes_remaining} episodes remaining" + progress_str = f"{num_episodes_remaining} episodes left" self._sandbox_service.text_drawer.add_text( progress_str, TextOnScreenAlignment.TOP_RIGHT, diff --git a/examples/siro_sandbox/app_states/app_state_socialnav.py b/examples/siro_sandbox/app_states/app_state_socialnav.py new file mode 100644 index 0000000000..30256673a9 --- /dev/null +++ b/examples/siro_sandbox/app_states/app_state_socialnav.py @@ -0,0 +1,308 @@ +from typing import List, Set + +import magnum as mn +import numpy as np +from app_states.app_state_abc import AppState +from camera_helper import CameraHelper +from controllers.gui_controller import GuiHumanoidController +from gui_navigation_helper import GuiNavigationHelper +from hablab_utils import get_agent_art_obj_transform +from sandbox_service import SandboxService + +from habitat.gui.gui_input import GuiInput +from habitat.gui.text_drawer import TextOnScreenAlignment + + +class AppStateSocialNav(AppState): + def __init__( + self, + sandbox_service: SandboxService, + gui_agent_ctrl: GuiHumanoidController, + ) -> None: + self._sandbox_service: SandboxService = sandbox_service + self._gui_agent_ctrl: GuiHumanoidController = gui_agent_ctrl + + self._cam_transform = None + self._camera_helper = CameraHelper( + self._sandbox_service.args, self._sandbox_service.gui_input + ) + self._nav_helper = GuiNavigationHelper( + self._sandbox_service, self.get_gui_controlled_agent_index() + ) + self._episode_helper = self._sandbox_service.episode_helper + + # task-specific parameters: + self._object_found_radius: float = 1.2 # TODO: make this a parameter + self._episode_found_obj_ids: Set = ( + None # will be set in on_environment_reset + ) + self._episode_target_obj_ids: List[ + str + ] = None # will be set in on_environment_reset + + def on_environment_reset(self, episode_recorder_dict): + self._episode_found_obj_ids = set() + + sim = self.get_sim() + obj_ids, _ = sim.get_targets() + self._episode_target_obj_ids = [ + sim._scene_obj_ids[obj_id] for obj_id in obj_ids + ] + + self._nav_helper.on_environment_reset() + self._camera_helper.update(self._get_camera_lookat_pos(), dt=0) + + def sim_update(self, dt, post_sim_update_dict): + if self._sandbox_service.gui_input.get_key_down(GuiInput.KeyNS.ESC): + self._sandbox_service.end_episode() + post_sim_update_dict["application_exit"] = True + + if ( + self._sandbox_service.gui_input.get_key_down(GuiInput.KeyNS.M) + and self._episode_helper.next_episode_exists() + ): + self._sandbox_service.end_episode(do_reset=True) + + if self._env_episode_active(): + self._update_task() + self._set_act_hints() + self._sandbox_service.compute_action_and_step_env() + + self._camera_helper.update(self._get_camera_lookat_pos(), dt) + + self.cam_transform = self._camera_helper.get_cam_transform() + post_sim_update_dict["cam_transform"] = self.cam_transform + + self._update_help_text() + + def get_num_agents(self): + return len(self.get_sim().agents_mgr._all_agent_data) + + def record_state(self): + agent_states = [] + for agent_idx in range(self.get_num_agents()): + agent_root = get_agent_art_obj_transform(self.get_sim(), agent_idx) + rotation_quat = mn.Quaternion.from_matrix(agent_root.rotation()) + rotation_list = list(rotation_quat.vector) + [rotation_quat.scalar] + pos = agent_root.translation + + agent_states.append( + { + "position": pos, + "rotation_xyzw": rotation_list, + } + ) + + self._sandbox_service.step_recorder.record( + "agent_states", agent_states + ) + + def get_sim(self): + return self._sandbox_service.sim + + def get_gui_controlled_agent_index(self): + return self._gui_agent_ctrl._agent_idx + + def _env_episode_active(self) -> bool: + return not ( + self._sandbox_service.env.episode_over or self._env_task_complete() + ) + + def _env_task_complete(self) -> bool: + return len(self._episode_target_obj_ids) == len( + self._episode_found_obj_ids + ) + + def _get_camera_lookat_pos(self): + agent_root = get_agent_art_obj_transform( + self.get_sim(), self.get_gui_controlled_agent_index() + ) + lookat_y_offset = mn.Vector3(0, 1, 0) + lookat = agent_root.translation + lookat_y_offset + return lookat + + def _update_task(self): + end_radius = 0.3 + self._num_remaining_objects = 0 + + # draw nav_hint and found object area + for obj_id in self._episode_target_obj_ids: + if obj_id in self._episode_found_obj_ids: + continue + + self._num_remaining_objects += 1 + + this_target_pos = self._get_target_object_position(obj_id) + + self._nav_helper._draw_nav_hint_from_agent( + self._camera_helper.get_xz_forward(), + mn.Vector3(this_target_pos), + end_radius, + mn.Color3(255 / 255, 128 / 255, 0), # orange + ) + + # draw found object area + can_grasp_position = mn.Vector3(this_target_pos) + can_grasp_position[1] = self._get_agent_feet_height() + self._sandbox_service.line_render.draw_circle( + can_grasp_position, + self._object_found_radius, + mn.Color3(255 / 255, 255 / 255, 0), # yellow + 24, + ) + + def _set_act_hints(self): + translation = self._get_agent_translation() + + min_dist = self._object_found_radius + closest_object_id = None + for obj_id in self._episode_target_obj_ids: + if obj_id in self._episode_found_obj_ids: + continue + + this_target_pos = self._get_target_object_position(obj_id) + # compute distance in xz plane + offset = this_target_pos - translation + offset.y = 0 + dist_xz = offset.length() + if dist_xz < min_dist: + min_dist = dist_xz + closest_object_id = obj_id + + if closest_object_id is not None: + self._episode_found_obj_ids.add(closest_object_id) + + walk_dir = None + distance_multiplier = 1.0 + if not self._camera_helper._first_person_mode: + ( + candidate_walk_dir, + candidate_distance_multiplier, + ) = self._nav_helper.get_humanoid_walk_hints_from_ray_cast( + visualize_path=True + ) + if self._sandbox_service.gui_input.get_mouse_button( + GuiInput.MouseNS.RIGHT + ): + walk_dir = candidate_walk_dir + distance_multiplier = candidate_distance_multiplier + + self._gui_agent_ctrl.set_act_hints( + walk_dir, + distance_multiplier, + None, + None, + self._camera_helper.lookat_offset_yaw, + ) + + def _get_agent_translation(self): + assert isinstance(self._gui_agent_ctrl, GuiHumanoidController) + return ( + self._gui_agent_ctrl._humanoid_controller.obj_transform_base.translation + ) + + def _get_agent_feet_height(self): + assert isinstance(self._gui_agent_ctrl, GuiHumanoidController) + base_offset = ( + self._gui_agent_ctrl.get_articulated_agent().params.base_offset + ) + agent_feet_translation = self._get_agent_translation() + base_offset + return agent_feet_translation[1] + + def _get_target_object_position(self, target_obj_id): + sim = self.get_sim() + rom = sim.get_rigid_object_manager() + return rom.get_object_by_id(target_obj_id).translation + + def _get_target_object_positions(self): + sim = self.get_sim() + rom = sim.get_rigid_object_manager() + return np.array( + [ + rom.get_object_by_id(obj_id).translation + for obj_id in self._episode_target_obj_ids + ] + ) + + def _update_help_text(self): + controls_str = self._get_controls_text() + if len(controls_str) > 0: + self._sandbox_service.text_drawer.add_text( + controls_str, TextOnScreenAlignment.TOP_LEFT + ) + + status_str = self._get_status_text() + if len(status_str) > 0: + self._sandbox_service.text_drawer.add_text( + status_str, + TextOnScreenAlignment.TOP_CENTER, + text_delta_x=-280, + text_delta_y=-50, + ) + + num_episodes_remaining = ( + self._episode_helper.num_iter_episodes + - self._episode_helper.num_episodes_done + ) + progress_str = f"{num_episodes_remaining} episodes left" + self._sandbox_service.text_drawer.add_text( + progress_str, + TextOnScreenAlignment.TOP_RIGHT, + text_delta_x=370, + ) + + def _get_controls_text(self): + found_object_controls_text = "Spacebar: mark object as found\n" + + controls_str: str = "" + controls_str += "ESC: exit\n" + if self._episode_helper.next_episode_exists(): + controls_str += "M: next episode\n" + + if self._env_episode_active(): + if self._camera_helper._first_person_mode: + # controls_str += "Left-click: toggle cursor\n" # make this "unofficial" for now + controls_str += "I, K: look up, down\n" + controls_str += "A, D: turn\n" + controls_str += "W, S: walk\n" + controls_str += found_object_controls_text + # third-person mode + else: + controls_str += "R + drag: rotate camera\n" + controls_str += "Right-click: walk\n" + controls_str += "A, D: turn\n" + controls_str += "W, S: walk\n" + controls_str += "Scroll: zoom\n" + controls_str += found_object_controls_text + + return controls_str + + def _get_status_text(self): + assert self._num_remaining_objects is not None + + status_str = "" + if not self._env_episode_active(): + if self._env_task_complete(): + status_str += "Task complete!\n" + else: + status_str += "Oops! Something went wrong.\n" + elif self._num_remaining_objects > 0: + status_str += "Find the remaining {} object{}!".format( + self._num_remaining_objects, + "s" if self._num_remaining_objects > 1 else "", + ) + else: + # we don't expect to hit this case ever + status_str += "Oops! Something went wrong.\n" + + # center align the status_str + max_status_str_len = 50 + status_str = "/n".join( + line.center(max_status_str_len) for line in status_str.split("/n") + ) + + return status_str + + def is_app_state_done(self): + # terminal neverending app state + return False diff --git a/examples/siro_sandbox/app_states/app_state_socialnav_study.py b/examples/siro_sandbox/app_states/app_state_socialnav_study.py index 2b5e8345ff..15250c9509 100644 --- a/examples/siro_sandbox/app_states/app_state_socialnav_study.py +++ b/examples/siro_sandbox/app_states/app_state_socialnav_study.py @@ -367,7 +367,7 @@ def _update_help_text(self): self._episode_helper.num_iter_episodes - self._episode_helper.num_episodes_done ) - progress_str = f"{num_episodes_remaining} episodes remaining" + progress_str = f"{num_episodes_remaining} episodes left" self._sandbox_service.text_drawer.add_text( progress_str, TextOnScreenAlignment.TOP_RIGHT, From d19e8ce8a62cb7ce0c486e96e8810153cbe8c617 Mon Sep 17 00:00:00 2001 From: rpartsey Date: Sat, 4 Nov 2023 18:58:06 +0200 Subject: [PATCH 3/6] Ruta's shorter tutorial changes --- .../siro_sandbox/app_states/app_state_tutorial.py | 4 ++++ examples/siro_sandbox/hitl_tutorial.py | 4 ++-- examples/siro_sandbox/sandbox_app.py | 14 ++++++++++++++ 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/examples/siro_sandbox/app_states/app_state_tutorial.py b/examples/siro_sandbox/app_states/app_state_tutorial.py index 8d21fcd7be..a8cc3d8644 100644 --- a/examples/siro_sandbox/app_states/app_state_tutorial.py +++ b/examples/siro_sandbox/app_states/app_state_tutorial.py @@ -63,6 +63,10 @@ def _sim_update_tutorial(self, dt: float): if self._sandbox_service.gui_input.get_key_down(GuiInput.KeyNS.SPACE): self._tutorial.skip_stage() + if self._sandbox_service.gui_input.get_key_down(GuiInput.KeyNS.Q): + while not self._tutorial.is_completed(): + self._tutorial.skip_stage() + if self._tutorial.is_completed(): self._tutorial.stop_animations() else: diff --git a/examples/siro_sandbox/hitl_tutorial.py b/examples/siro_sandbox/hitl_tutorial.py index 2d1a517997..885d1e455f 100644 --- a/examples/siro_sandbox/hitl_tutorial.py +++ b/examples/siro_sandbox/hitl_tutorial.py @@ -30,8 +30,8 @@ TEXT_ROBOT_FOCUS: str = ( "This is your robot assistant.\nIt will help you accomplish your tasks.\n" ) -TEXT_AVATAR_FOCUS: str = "This is your avatar.\nYou will now gain control." -TEXT_HELP: str = "Spacebar: Skip" +TEXT_AVATAR_FOCUS: str = "You will now gain control of your avatar." +TEXT_HELP: str = "Q: Skip \nSpacebar: Skip to the next stage of the tutorial" class ObjectAnimation: diff --git a/examples/siro_sandbox/sandbox_app.py b/examples/siro_sandbox/sandbox_app.py index 1d1a5e6f2b..c7bb4a728c 100644 --- a/examples/siro_sandbox/sandbox_app.py +++ b/examples/siro_sandbox/sandbox_app.py @@ -381,6 +381,14 @@ def _reset_environment(self): self._app_state_index = ( 0 # start from the first app state for each episode ) + # if short_tutorial enabled we show the tutorial only once - before the first episode + # (skip the first app state (tutorial) after the first episode) + if ( + self._args.show_tutorial + and self._args.short_tutorial + and self._episode_helper.num_episodes_done > 0 + ): + self._app_state_index = 1 self._app_state = self._app_states[self._app_state_index] self._app_state.on_enter( prev_state=self._get_prev_app_state(), @@ -730,6 +738,12 @@ def _parse_debug_third_person(args, framebuffer_size): default=False, help="Shows an intro sequence that helps familiarize the user to the scene and task in a HITL context.", ) + parser.add_argument( + "--short-tutorial", + action="store_true", + default=False, + help="Shows an intro sequence that helps familiarize the user to the scene and task in a HITL context, ONLY for 1 episode. Requires --show-tutorial arguement to be specified as well.", + ) parser.add_argument( "--hide-humanoid-in-gui", action="store_true", From 12daab1b5a7d9f759a69e4dbe49f98604c5cc929 Mon Sep 17 00:00:00 2001 From: rpartsey Date: Tue, 7 Nov 2023 16:01:30 +0200 Subject: [PATCH 4/6] keepping only short tutorial version --- examples/siro_sandbox/sandbox_app.py | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/examples/siro_sandbox/sandbox_app.py b/examples/siro_sandbox/sandbox_app.py index c7bb4a728c..a7f1c82684 100644 --- a/examples/siro_sandbox/sandbox_app.py +++ b/examples/siro_sandbox/sandbox_app.py @@ -381,14 +381,13 @@ def _reset_environment(self): self._app_state_index = ( 0 # start from the first app state for each episode ) - # if short_tutorial enabled we show the tutorial only once - before the first episode - # (skip the first app state (tutorial) after the first episode) + # if show_tutorial enabled we show the tutorial once - before the first episode if ( self._args.show_tutorial - and self._args.short_tutorial and self._episode_helper.num_episodes_done > 0 ): - self._app_state_index = 1 + self._app_state_index += 1 + self._app_state = self._app_states[self._app_state_index] self._app_state.on_enter( prev_state=self._get_prev_app_state(), @@ -736,13 +735,7 @@ def _parse_debug_third_person(args, framebuffer_size): "--show-tutorial", action="store_true", default=False, - help="Shows an intro sequence that helps familiarize the user to the scene and task in a HITL context.", - ) - parser.add_argument( - "--short-tutorial", - action="store_true", - default=False, - help="Shows an intro sequence that helps familiarize the user to the scene and task in a HITL context, ONLY for 1 episode. Requires --show-tutorial arguement to be specified as well.", + help="Shows an intro sequence before the first episode that helps familiarize the user to task in a HITL context.", ) parser.add_argument( "--hide-humanoid-in-gui", From b893fc9268371ab2ed0eb3a9625113f7ee6cd824 Mon Sep 17 00:00:00 2001 From: rpartsey Date: Thu, 9 Nov 2023 11:01:06 +0200 Subject: [PATCH 5/6] enable tutorial for the AppStateSocialNavStudy --- examples/siro_sandbox/sandbox_app.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/examples/siro_sandbox/sandbox_app.py b/examples/siro_sandbox/sandbox_app.py index a7f1c82684..1335b29d16 100644 --- a/examples/siro_sandbox/sandbox_app.py +++ b/examples/siro_sandbox/sandbox_app.py @@ -216,6 +216,14 @@ def local_end_episode(do_reset=False): self.ctrl_helper.get_policy_driven_agent_controller(), ) ] + if args.show_tutorial: + self._app_states.insert( + 0, + AppStateTutorial( + self._sandbox_service, + self.ctrl_helper.get_gui_agent_controller(), + ), + ) elif args.app_state == "free_camera": self._app_states = [AppStateFreeCamera(self._sandbox_service)] else: @@ -783,9 +791,12 @@ def _parse_debug_third_person(args, framebuffer_size): "but --save-filepath-base argument is not set. Specify filepath base for the session episode data to be saved." ) - if args.show_tutorial and args.app_state != "rearrange": + if args.show_tutorial and args.app_state not in { + "rearrange", + "socialnav_study", + }: raise ValueError( - "--show-tutorial is only supported for --app-state=rearrange" + "--show-tutorial is only supported for --app-state=rearrange and --app-state=socialnav_study" ) if args.remote_gui_mode and args.app_state != "fetch": From e527d3a84dbcdc0d2ae67b903b246dfcbb150726 Mon Sep 17 00:00:00 2001 From: rpartsey Date: Thu, 9 Nov 2023 11:08:18 +0200 Subject: [PATCH 6/6] delete file added after rebase --- .../app_states/app_state_socialnav.py | 308 ------------------ 1 file changed, 308 deletions(-) delete mode 100644 examples/siro_sandbox/app_states/app_state_socialnav.py diff --git a/examples/siro_sandbox/app_states/app_state_socialnav.py b/examples/siro_sandbox/app_states/app_state_socialnav.py deleted file mode 100644 index 30256673a9..0000000000 --- a/examples/siro_sandbox/app_states/app_state_socialnav.py +++ /dev/null @@ -1,308 +0,0 @@ -from typing import List, Set - -import magnum as mn -import numpy as np -from app_states.app_state_abc import AppState -from camera_helper import CameraHelper -from controllers.gui_controller import GuiHumanoidController -from gui_navigation_helper import GuiNavigationHelper -from hablab_utils import get_agent_art_obj_transform -from sandbox_service import SandboxService - -from habitat.gui.gui_input import GuiInput -from habitat.gui.text_drawer import TextOnScreenAlignment - - -class AppStateSocialNav(AppState): - def __init__( - self, - sandbox_service: SandboxService, - gui_agent_ctrl: GuiHumanoidController, - ) -> None: - self._sandbox_service: SandboxService = sandbox_service - self._gui_agent_ctrl: GuiHumanoidController = gui_agent_ctrl - - self._cam_transform = None - self._camera_helper = CameraHelper( - self._sandbox_service.args, self._sandbox_service.gui_input - ) - self._nav_helper = GuiNavigationHelper( - self._sandbox_service, self.get_gui_controlled_agent_index() - ) - self._episode_helper = self._sandbox_service.episode_helper - - # task-specific parameters: - self._object_found_radius: float = 1.2 # TODO: make this a parameter - self._episode_found_obj_ids: Set = ( - None # will be set in on_environment_reset - ) - self._episode_target_obj_ids: List[ - str - ] = None # will be set in on_environment_reset - - def on_environment_reset(self, episode_recorder_dict): - self._episode_found_obj_ids = set() - - sim = self.get_sim() - obj_ids, _ = sim.get_targets() - self._episode_target_obj_ids = [ - sim._scene_obj_ids[obj_id] for obj_id in obj_ids - ] - - self._nav_helper.on_environment_reset() - self._camera_helper.update(self._get_camera_lookat_pos(), dt=0) - - def sim_update(self, dt, post_sim_update_dict): - if self._sandbox_service.gui_input.get_key_down(GuiInput.KeyNS.ESC): - self._sandbox_service.end_episode() - post_sim_update_dict["application_exit"] = True - - if ( - self._sandbox_service.gui_input.get_key_down(GuiInput.KeyNS.M) - and self._episode_helper.next_episode_exists() - ): - self._sandbox_service.end_episode(do_reset=True) - - if self._env_episode_active(): - self._update_task() - self._set_act_hints() - self._sandbox_service.compute_action_and_step_env() - - self._camera_helper.update(self._get_camera_lookat_pos(), dt) - - self.cam_transform = self._camera_helper.get_cam_transform() - post_sim_update_dict["cam_transform"] = self.cam_transform - - self._update_help_text() - - def get_num_agents(self): - return len(self.get_sim().agents_mgr._all_agent_data) - - def record_state(self): - agent_states = [] - for agent_idx in range(self.get_num_agents()): - agent_root = get_agent_art_obj_transform(self.get_sim(), agent_idx) - rotation_quat = mn.Quaternion.from_matrix(agent_root.rotation()) - rotation_list = list(rotation_quat.vector) + [rotation_quat.scalar] - pos = agent_root.translation - - agent_states.append( - { - "position": pos, - "rotation_xyzw": rotation_list, - } - ) - - self._sandbox_service.step_recorder.record( - "agent_states", agent_states - ) - - def get_sim(self): - return self._sandbox_service.sim - - def get_gui_controlled_agent_index(self): - return self._gui_agent_ctrl._agent_idx - - def _env_episode_active(self) -> bool: - return not ( - self._sandbox_service.env.episode_over or self._env_task_complete() - ) - - def _env_task_complete(self) -> bool: - return len(self._episode_target_obj_ids) == len( - self._episode_found_obj_ids - ) - - def _get_camera_lookat_pos(self): - agent_root = get_agent_art_obj_transform( - self.get_sim(), self.get_gui_controlled_agent_index() - ) - lookat_y_offset = mn.Vector3(0, 1, 0) - lookat = agent_root.translation + lookat_y_offset - return lookat - - def _update_task(self): - end_radius = 0.3 - self._num_remaining_objects = 0 - - # draw nav_hint and found object area - for obj_id in self._episode_target_obj_ids: - if obj_id in self._episode_found_obj_ids: - continue - - self._num_remaining_objects += 1 - - this_target_pos = self._get_target_object_position(obj_id) - - self._nav_helper._draw_nav_hint_from_agent( - self._camera_helper.get_xz_forward(), - mn.Vector3(this_target_pos), - end_radius, - mn.Color3(255 / 255, 128 / 255, 0), # orange - ) - - # draw found object area - can_grasp_position = mn.Vector3(this_target_pos) - can_grasp_position[1] = self._get_agent_feet_height() - self._sandbox_service.line_render.draw_circle( - can_grasp_position, - self._object_found_radius, - mn.Color3(255 / 255, 255 / 255, 0), # yellow - 24, - ) - - def _set_act_hints(self): - translation = self._get_agent_translation() - - min_dist = self._object_found_radius - closest_object_id = None - for obj_id in self._episode_target_obj_ids: - if obj_id in self._episode_found_obj_ids: - continue - - this_target_pos = self._get_target_object_position(obj_id) - # compute distance in xz plane - offset = this_target_pos - translation - offset.y = 0 - dist_xz = offset.length() - if dist_xz < min_dist: - min_dist = dist_xz - closest_object_id = obj_id - - if closest_object_id is not None: - self._episode_found_obj_ids.add(closest_object_id) - - walk_dir = None - distance_multiplier = 1.0 - if not self._camera_helper._first_person_mode: - ( - candidate_walk_dir, - candidate_distance_multiplier, - ) = self._nav_helper.get_humanoid_walk_hints_from_ray_cast( - visualize_path=True - ) - if self._sandbox_service.gui_input.get_mouse_button( - GuiInput.MouseNS.RIGHT - ): - walk_dir = candidate_walk_dir - distance_multiplier = candidate_distance_multiplier - - self._gui_agent_ctrl.set_act_hints( - walk_dir, - distance_multiplier, - None, - None, - self._camera_helper.lookat_offset_yaw, - ) - - def _get_agent_translation(self): - assert isinstance(self._gui_agent_ctrl, GuiHumanoidController) - return ( - self._gui_agent_ctrl._humanoid_controller.obj_transform_base.translation - ) - - def _get_agent_feet_height(self): - assert isinstance(self._gui_agent_ctrl, GuiHumanoidController) - base_offset = ( - self._gui_agent_ctrl.get_articulated_agent().params.base_offset - ) - agent_feet_translation = self._get_agent_translation() + base_offset - return agent_feet_translation[1] - - def _get_target_object_position(self, target_obj_id): - sim = self.get_sim() - rom = sim.get_rigid_object_manager() - return rom.get_object_by_id(target_obj_id).translation - - def _get_target_object_positions(self): - sim = self.get_sim() - rom = sim.get_rigid_object_manager() - return np.array( - [ - rom.get_object_by_id(obj_id).translation - for obj_id in self._episode_target_obj_ids - ] - ) - - def _update_help_text(self): - controls_str = self._get_controls_text() - if len(controls_str) > 0: - self._sandbox_service.text_drawer.add_text( - controls_str, TextOnScreenAlignment.TOP_LEFT - ) - - status_str = self._get_status_text() - if len(status_str) > 0: - self._sandbox_service.text_drawer.add_text( - status_str, - TextOnScreenAlignment.TOP_CENTER, - text_delta_x=-280, - text_delta_y=-50, - ) - - num_episodes_remaining = ( - self._episode_helper.num_iter_episodes - - self._episode_helper.num_episodes_done - ) - progress_str = f"{num_episodes_remaining} episodes left" - self._sandbox_service.text_drawer.add_text( - progress_str, - TextOnScreenAlignment.TOP_RIGHT, - text_delta_x=370, - ) - - def _get_controls_text(self): - found_object_controls_text = "Spacebar: mark object as found\n" - - controls_str: str = "" - controls_str += "ESC: exit\n" - if self._episode_helper.next_episode_exists(): - controls_str += "M: next episode\n" - - if self._env_episode_active(): - if self._camera_helper._first_person_mode: - # controls_str += "Left-click: toggle cursor\n" # make this "unofficial" for now - controls_str += "I, K: look up, down\n" - controls_str += "A, D: turn\n" - controls_str += "W, S: walk\n" - controls_str += found_object_controls_text - # third-person mode - else: - controls_str += "R + drag: rotate camera\n" - controls_str += "Right-click: walk\n" - controls_str += "A, D: turn\n" - controls_str += "W, S: walk\n" - controls_str += "Scroll: zoom\n" - controls_str += found_object_controls_text - - return controls_str - - def _get_status_text(self): - assert self._num_remaining_objects is not None - - status_str = "" - if not self._env_episode_active(): - if self._env_task_complete(): - status_str += "Task complete!\n" - else: - status_str += "Oops! Something went wrong.\n" - elif self._num_remaining_objects > 0: - status_str += "Find the remaining {} object{}!".format( - self._num_remaining_objects, - "s" if self._num_remaining_objects > 1 else "", - ) - else: - # we don't expect to hit this case ever - status_str += "Oops! Something went wrong.\n" - - # center align the status_str - max_status_str_len = 50 - status_str = "/n".join( - line.center(max_status_str_len) for line in status_str.split("/n") - ) - - return status_str - - def is_app_state_done(self): - # terminal neverending app state - return False