diff --git a/docs/conf.py b/docs/conf.py index 981d0a51b0..2d102f6de9 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -21,8 +21,23 @@ # Monkey patch the registry to be the _Registry class instead of the singleton for docs habitat_sim.registry = type(habitat_sim.registry) # TODO: remove once utils/__init__.py is removed again -habitat_sim.utils.__all__.remove("quat_from_angle_axis") -habitat_sim.utils.__all__.remove("quat_rotate_vector") +habitat_sim.utils.common.__all__ = [ + x + for x in habitat_sim.utils.common.__all__ + if x + not in [ + "quat_from_coeffs", + "quat_to_coeffs", + "quat_from_magnum", + "quat_to_magnum", + "quat_from_angle_axis", + "quat_to_angle_axis", + "quat_rotate_vector", + "quat_from_two_vectors", + ] +] +# habitat_sim.utils.__all__.remove("quat_from_angle_axis") +# habitat_sim.utils.__all__.remove("quat_rotate_vector") PROJECT_TITLE = "Habitat" PROJECT_SUBTITLE = "Sim Docs" diff --git a/examples/marker_viewer.py b/examples/marker_viewer.py new file mode 100644 index 0000000000..4b6a6a8155 --- /dev/null +++ b/examples/marker_viewer.py @@ -0,0 +1,1300 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import ctypes +import math +import os +import string +import sys +import time +from enum import Enum +from typing import Any, Callable, Dict, List, Optional, Tuple, Union + +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + +import magnum as mn +import numpy as np +from magnum import shaders, text +from magnum.platform.glfw import Application + +import habitat_sim +from habitat_sim import ReplayRenderer, ReplayRendererConfiguration +from habitat_sim.logging import LoggingContext, logger +from habitat_sim.utils.classes import MarkerSetsEditor, ObjectEditor, SemanticDisplay +from habitat_sim.utils.common import quat_from_angle_axis +from habitat_sim.utils.namespace import hsim_physics +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# file holding all URDF filenames +URDF_FILES = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "urdfFileNames.txt" +) +# file holding hashes of objects that have no links +NOLINK_URDF_FILES = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "urdfsWithNoLinks.txt" +) + + +class HabitatSimInteractiveViewer(Application): + # the maximum number of chars displayable in the app window + # using the magnum text module. These chars are used to + # display the CPU/GPU usage data + MAX_DISPLAY_TEXT_CHARS = 512 + + # how much to displace window text relative to the center of the + # app window (e.g if you want the display text in the top left of + # the app window, you will displace the text + # window width * -TEXT_DELTA_FROM_CENTER in the x axis and + # window height * TEXT_DELTA_FROM_CENTER in the y axis, as the text + # position defaults to the middle of the app window) + TEXT_DELTA_FROM_CENTER = 0.49 + + # font size of the magnum in-window display text that displays + # CPU and GPU usage info + DISPLAY_FONT_SIZE = 16.0 + + def __init__(self, sim_settings: Dict[str, Any]) -> None: + self.sim_settings: Dict[str:Any] = sim_settings + + self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] + self.num_env: int = ( + self.sim_settings["num_environments"] if self.enable_batch_renderer else 1 + ) + + # Compute environment camera resolution based on the number of environments to render in the window. + window_size: mn.Vector2 = ( + self.sim_settings["window_width"], + self.sim_settings["window_height"], + ) + + configuration = self.Configuration() + configuration.title = "Habitat Sim Interactive Viewer" + configuration.size = window_size + Application.__init__(self, configuration) + self.fps: float = 60.0 + + # Compute environment camera resolution based on the number of environments to render in the window. + grid_size: mn.Vector2i = ReplayRenderer.environment_grid_size(self.num_env) + camera_resolution: mn.Vector2 = mn.Vector2(self.framebuffer_size) / mn.Vector2( + grid_size + ) + self.sim_settings["width"] = camera_resolution[0] + self.sim_settings["height"] = camera_resolution[1] + + # draw Bullet debug line visualizations (e.g. collision meshes) + self.debug_bullet_draw = False + # draw active contact point debug line visualizations + self.contact_debug_draw = False + + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" + + # set up our movement map + key = Application.Key + self.pressed = { + key.UP: False, + key.DOWN: False, + key.LEFT: False, + key.RIGHT: False, + key.A: False, + key.D: False, + key.S: False, + key.W: False, + key.X: False, + key.Z: False, + } + + # set up our movement key bindings map + self.key_to_action = { + key.UP: "look_up", + key.DOWN: "look_down", + key.LEFT: "turn_left", + key.RIGHT: "turn_right", + key.A: "move_left", + key.D: "move_right", + key.S: "move_backward", + key.W: "move_forward", + key.X: "move_down", + key.Z: "move_up", + } + + # Load a TrueTypeFont plugin and open the font file + self.display_font = text.FontManager().load_and_instantiate("TrueTypeFont") + relative_path_to_font = "../data/fonts/ProggyClean.ttf" + self.display_font.open_file( + os.path.join(os.path.dirname(__file__), relative_path_to_font), + 13, + ) + + # Glyphs we need to render everything + self.glyph_cache = text.GlyphCacheGL( + mn.PixelFormat.R8_UNORM, mn.Vector2i(256), mn.Vector2i(1) + ) + self.display_font.fill_glyph_cache( + self.glyph_cache, + string.ascii_lowercase + + string.ascii_uppercase + + string.digits + + ":-_+,.! %µ", + ) + + # magnum text object that displays CPU/GPU usage data in the app window + self.window_text = text.Renderer2D( + self.display_font, + self.glyph_cache, + HabitatSimInteractiveViewer.DISPLAY_FONT_SIZE, + text.Alignment.TOP_LEFT, + ) + self.window_text.reserve(HabitatSimInteractiveViewer.MAX_DISPLAY_TEXT_CHARS) + + # text object transform in window space is Projection matrix times Translation Matrix + # put text in top left of window + self.window_text_transform = mn.Matrix3.projection( + self.framebuffer_size + ) @ mn.Matrix3.translation( + mn.Vector2(self.framebuffer_size) + * mn.Vector2( + -HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + ) + ) + self.shader = shaders.VectorGL2D() + + # make magnum text background transparent + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + # Set blend function + mn.gl.Renderer.set_blend_equation( + mn.gl.Renderer.BlendEquation.ADD, mn.gl.Renderer.BlendEquation.ADD + ) + + # variables that track app data and CPU/GPU usage + self.num_frames_to_track = 60 + + # Cycle mouse utilities + self.mouse_interaction = MouseMode.LOOK + self.previous_mouse_point = None + + # toggle physics simulation on/off + self.simulating = True + + # toggle a single simulation step at the next opportunity if not + # simulating continuously. + self.simulate_single_step = False + + # configure our simulator + self.cfg: Optional[habitat_sim.simulator.Configuration] = None + self.sim: Optional[habitat_sim.simulator.Simulator] = None + self.tiled_sims: list[habitat_sim.simulator.Simulator] = None + self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None + self.replay_renderer: Optional[ReplayRenderer] = None + + self.last_hit_details = None + + self.navmesh_dirty = False + + # mouse raycast visualization + self.mouse_cast_results = None + self.mouse_cast_has_hits = False + + self.ao_link_map = None + + self.agent_start_location = mn.Vector3(-5.7, 0.0, -4.0) + self.ao_place_location = mn.Vector3(-7.7, 1.0, -4.0) + + # Load simulatioon scene + self.reconfigure_sim() + + # load file holding urdf filenames needing handles + print( + f"URDF hashes file name : {URDF_FILES} | No-link URDFS file name : {NOLINK_URDF_FILES}" + ) + # Build a List of URDF hash names loaded from disk, where each entry + # is a dictionary of hash, status, and notes (if present). + # As URDFs are completed their status is changed from "unfinished" to "done" + self.urdf_hash_names_list = self.load_urdf_filenames() + # Start with first idx in self.urdf_hash_names_list + self.urdf_edit_hash_idx = self._get_next_hash_idx( + start_idx=0, forward=True, status="unfinished" + ) + + # load markersets for every object and ao into a cache + task_names_set = {"faucets", "handles"} + self.markersets_util = MarkerSetsEditor(self.sim, task_names_set) + self.markersets_util.set_current_taskname("handles") + + # Editing for object selection + self.obj_editor = ObjectEditor(self.sim) + # Set default editing to rotation + self.obj_editor.set_edit_mode_rotate() + # Force save of urdf hash to NOLINK_URDF_FILES file + self.force_urdf_notes_save = False + + # Load first object to place markers on + self.load_urdf_obj() + + # Semantics + self.dbg_semantics = SemanticDisplay(self.sim) + + # compute NavMesh if not already loaded by the scene. + if ( + not self.sim.pathfinder.is_loaded + and self.cfg.sim_cfg.scene_id.lower() != "none" + ): + self.navmesh_config_and_recompute() + + self.time_since_last_simulation = 0.0 + LoggingContext.reinitialize_from_env() + logger.setLevel("INFO") + self.print_help_text() + + def _get_next_hash_idx(self, start_idx: int, forward: bool, status: str): + if forward: + iter_range = range(start_idx, len(self.urdf_hash_names_list)) + else: + iter_range = range(start_idx, -1, -1) + + for i in iter_range: + if self.urdf_hash_names_list[i]["status"] == status: + return i + print( + f"No {status} hashes left to be found {('forward of' if forward else 'backward from')} starting idx {start_idx}." + ) + return -1 + + def _set_hash_list_status(self, hash_idx: int, is_finished: bool): + pass + + def load_urdf_filenames(self): + # list of dicts holding hash, status, notes + urdf_hash_names_list: List[Dict[str:str]] = [] + # File names of all URDFs + with open(URDF_FILES, "r") as f: + for line in f.readlines(): + vals = line.split(",", maxsplit=2) + finished = "done" if vals[1].strip().lower() == "true" else "unfinished" + new_dict = {"hash": vals[0].strip(), "status": finished} + if len(vals) > 2: + new_dict["notes"] = vals[2].strip() + else: + new_dict["notes"] = "" + urdf_hash_names_list.append(new_dict) + + return urdf_hash_names_list + + def update_nolink_file(self, save_no_markers: bool): + # remove urdf hash from NOLINK_URDF_FILES if it has links, add it if it does not + urdf_hash: str = self.urdf_hash_names_list[self.urdf_edit_hash_idx]["hash"] + # preserve all text in file after comma + urdf_nolink_hash_names: Dict[str, str] = {} + with open(NOLINK_URDF_FILES, "r") as f: + for line in f.readlines(): + if len(line.strip()) == 0: + continue + vals = line.split(",", maxsplit=1) + # hash is idx0; notes is idx1 + urdf_nolink_hash_names[vals[0]] = vals[1].strip() + if save_no_markers: + # it has no markers or we are forcing a save, so add it to record if it isn't already there + if urdf_hash not in urdf_nolink_hash_names: + # add empty string + urdf_nolink_hash_names[urdf_hash] = "" + else: + # if it has markers now, remove it from record + urdf_nolink_hash_names.pop(urdf_hash, None) + # save no-link status results + with open(NOLINK_URDF_FILES, "w") as f: + for file_hash, notes in urdf_nolink_hash_names.items(): + f.write(f"{file_hash}, {notes}\n") + + def save_urdf_filesnames(self): + # save current state of URDF files + with open(URDF_FILES, "w") as f: + for urdf_entry in self.urdf_hash_names_list: + notes = "" if len(urdf_entry) > 2 else f", {urdf_entry['notes']}" + status = urdf_entry["status"].strip().lower() == "done" + f.write(f"{urdf_entry['hash']}, {status}{notes}\n") + + def _delete_sel_obj_update_nolink_file(self, sel_obj_hash: str): + sel_obj = self.obj_editor.get_target_sel_obj() + if sel_obj is None: + sel_obj = hsim_physics.get_obj_from_handle(sel_obj_hash) + + save_no_markers = ( + self.force_urdf_notes_save + or sel_obj.marker_sets.num_tasksets == 0 + or (not sel_obj.marker_sets.has_taskset("handles")) + ) + print( + f"Object {sel_obj.handle} has {sel_obj.marker_sets.num_tasksets} tasksets and Force save set to {self.force_urdf_notes_save} == Save as no marker urdf? {save_no_markers}" + ) + # remove currently selected objects + removed_obj_handles = self.obj_editor.remove_sel_objects() + # should only have 1 handle + for handle in removed_obj_handles: + print(f"Removed {handle}") + # finalize removal + self.obj_editor.remove_all_objs() + # update record of object hashes with/without markers with current file's state + self.update_nolink_file(save_no_markers=save_no_markers) + + def load_urdf_obj(self): + # Next object to be edited + sel_obj_hash = self.urdf_hash_names_list[self.urdf_edit_hash_idx]["hash"] + print(f"URDF hash we want : `{sel_obj_hash}`") + # Load object into scene + _, self.navmesh_dirty = self.obj_editor.load_from_substring( + navmesh_dirty=self.navmesh_dirty, + obj_substring=sel_obj_hash, + build_loc=self.ao_place_location, + ) + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + self.markersets_util.update_markersets() + self.markersets_util.set_current_taskname("handles") + + def cycle_through_urdfs(self, shift_pressed: bool) -> None: + # current object hash + old_sel_obj_hash = self.urdf_hash_names_list[self.urdf_edit_hash_idx]["hash"] + # Determine the status we are looking for when we search for the next desired index + if shift_pressed: + status = "done" + start_idx = self.urdf_edit_hash_idx + else: + status = "unfinished" + start_idx = self.urdf_edit_hash_idx + # Moving forward - set current to finished + self.urdf_hash_names_list[self.urdf_edit_hash_idx]["status"] = "done" + + # Get the idx of the next object we want to edit + # Either the idx of the next record that is unfinished, or the most recent previous record that is done + next_idx = self._get_next_hash_idx( + start_idx=start_idx, forward=not shift_pressed, status=status + ) + + # If we don't have a valid next index then handle edge case + if next_idx == -1: + if not shift_pressed: + # save current status + self.save_urdf_filesnames() + # moving forward - done! + print( + f"Finished going through all {len(self.urdf_hash_names_list)} loaded urdf files. Exiting." + ) + self.exit_event(Application.ExitEvent) + else: + # moving backward, at the start of all the objects so nowhere to go + print(f"No objects previous to current object {old_sel_obj_hash}.") + return + # set edited state in urdf file list appropriately if moving backward, set previous to unfinished, leave current unfinished + if shift_pressed: + # Moving backward - set previous to unfinished, leave current unchanged + self.urdf_hash_names_list[next_idx]["status"] = "unfinished" + + # remove the current selected object and update the no_link file + self._delete_sel_obj_update_nolink_file(sel_obj_hash=old_sel_obj_hash) + + # Update the current edit hash idx + self.urdf_edit_hash_idx = next_idx + # save current status + self.save_urdf_filesnames() + print(f"URDF hash we just finished : `{old_sel_obj_hash}`") + # load next urdf object + self.load_urdf_obj() + # reset force save to False for each object + self.force_urdf_notes_save = False + + def draw_contact_debug(self, debug_line_render: Any): + """ + This method is called to render a debug line overlay displaying active contact points and normals. + Yellow lines show the contact distance along the normal and red lines show the contact normal at a fixed length. + """ + yellow = mn.Color4.yellow() + red = mn.Color4.red() + cps = self.sim.get_physics_contact_points() + debug_line_render.set_line_width(1.5) + camera_position = self.render_camera.render_camera.node.absolute_translation + # only showing active contacts + active_contacts = (x for x in cps if x.is_active) + for cp in active_contacts: + # red shows the contact distance + debug_line_render.draw_transformed_line( + cp.position_on_b_in_ws, + cp.position_on_b_in_ws + + cp.contact_normal_on_b_in_ws * -cp.contact_distance, + red, + ) + # yellow shows the contact normal at a fixed length for visualization + debug_line_render.draw_transformed_line( + cp.position_on_b_in_ws, + # + cp.contact_normal_on_b_in_ws * cp.contact_distance, + cp.position_on_b_in_ws + cp.contact_normal_on_b_in_ws * 0.1, + yellow, + ) + debug_line_render.draw_circle( + translation=cp.position_on_b_in_ws, + radius=0.005, + color=yellow, + normal=camera_position - cp.position_on_b_in_ws, + ) + + def debug_draw(self): + """ + Additional draw commands to be called during draw_event. + """ + + debug_line_render = self.sim.get_debug_line_render() + if self.debug_bullet_draw: + render_cam = self.render_camera.render_camera + proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) + self.sim.physics_debug_draw(proj_mat) + + if self.contact_debug_draw: + self.draw_contact_debug(debug_line_render) + # draw semantic information + self.dbg_semantics.draw_region_debug(debug_line_render=debug_line_render) + # draw markersets information + if self.markersets_util.marker_sets_per_obj is not None: + self.markersets_util.draw_marker_sets_debug( + debug_line_render, + self.render_camera.render_camera.node.absolute_translation, + ) + + self.obj_editor.draw_selected_objects(debug_line_render) + # mouse raycast circle + # This is confusing with the marker placement + # if self.mouse_cast_has_hits: + # debug_line_render.draw_circle( + # translation=self.mouse_cast_results.hits[0].point, + # radius=0.005, + # color=mn.Color4(mn.Vector3(1.0), 1.0), + # normal=self.mouse_cast_results.hits[0].normal, + # ) + + def draw_event( + self, + simulation_call: Optional[Callable] = None, + global_call: Optional[Callable] = None, + active_agent_id_and_sensor_name: Tuple[int, str] = (0, "color_sensor"), + ) -> None: + """ + Calls continuously to re-render frames and swap the two frame buffers + at a fixed rate. + """ + agent_acts_per_sec = self.fps + + mn.gl.default_framebuffer.clear( + mn.gl.FramebufferClear.COLOR | mn.gl.FramebufferClear.DEPTH + ) + + # Agent actions should occur at a fixed rate per second + self.time_since_last_simulation += Timer.prev_frame_duration + num_agent_actions: int = self.time_since_last_simulation * agent_acts_per_sec + self.move_and_look(int(num_agent_actions)) + + # Occasionally a frame will pass quicker than 1/60 seconds + if self.time_since_last_simulation >= 1.0 / self.fps: + if self.simulating or self.simulate_single_step: + self.sim.step_world(1.0 / self.fps) + self.simulate_single_step = False + if simulation_call is not None: + simulation_call() + if global_call is not None: + global_call() + if self.navmesh_dirty: + self.navmesh_config_and_recompute() + self.navmesh_dirty = False + + # reset time_since_last_simulation, accounting for potential overflow + self.time_since_last_simulation = math.fmod( + self.time_since_last_simulation, 1.0 / self.fps + ) + + keys = active_agent_id_and_sensor_name + + if self.enable_batch_renderer: + self.render_batch() + else: + self.sim._Simulator__sensors[keys[0]][keys[1]].draw_observation() + agent = self.sim.get_agent(keys[0]) + self.render_camera = agent.scene_node.node_sensor_suite.get(keys[1]) + self.debug_draw() + self.render_camera.render_target.blit_rgba_to_default() + + # draw CPU/GPU usage data and other info to the app window + mn.gl.default_framebuffer.bind() + self.draw_text(self.render_camera.specification()) + + self.swap_buffers() + Timer.next_frame() + self.redraw() + + def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: + """ + Set up our own agent and agent controls + """ + make_action_spec = habitat_sim.agent.ActionSpec + make_actuation_spec = habitat_sim.agent.ActuationSpec + MOVE, LOOK = 0.07, 1.5 + + # all of our possible actions' names + action_list = [ + "move_left", + "turn_left", + "move_right", + "turn_right", + "move_backward", + "look_up", + "move_forward", + "look_down", + "move_down", + "move_up", + ] + + action_space: Dict[str, habitat_sim.agent.ActionSpec] = {} + + # build our action space map + for action in action_list: + actuation_spec_amt = MOVE if "move" in action else LOOK + action_spec = make_action_spec( + action, make_actuation_spec(actuation_spec_amt) + ) + action_space[action] = action_spec + + sensor_spec: List[habitat_sim.sensor.SensorSpec] = self.cfg.agents[ + self.agent_id + ].sensor_specifications + + agent_config = habitat_sim.agent.AgentConfiguration( + height=1.5, + radius=0.1, + sensor_specifications=sensor_spec, + action_space=action_space, + body_type="cylinder", + ) + return agent_config + + def reconfigure_sim(self) -> None: + """ + Utilizes the current `self.sim_settings` to configure and set up a new + `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces + the current simulator instance, reloading the most recently loaded scene + """ + # configure our sim_settings but then set the agent to our default + self.cfg = make_cfg(self.sim_settings) + self.agent_id: int = self.sim_settings["default_agent"] + self.cfg.agents[self.agent_id] = self.default_agent_config() + + if self.enable_batch_renderer: + self.cfg.enable_batch_renderer = True + self.cfg.sim_cfg.create_renderer = False + self.cfg.sim_cfg.enable_gfx_replay_save = True + + if self.sim_settings["use_default_lighting"]: + logger.info("Setting default lighting override for scene.") + self.cfg.sim_cfg.override_scene_light_defaults = True + self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY + + if self.sim is None: + self.tiled_sims = [] + for _i in range(self.num_env): + self.tiled_sims.append(habitat_sim.Simulator(self.cfg)) + self.sim = self.tiled_sims[0] + else: # edge case + for i in range(self.num_env): + if ( + self.tiled_sims[i].config.sim_cfg.scene_id + == self.cfg.sim_cfg.scene_id + ): + # we need to force a reset, so change the internal config scene name + self.tiled_sims[i].config.sim_cfg.scene_id = "NONE" + self.tiled_sims[i].reconfigure(self.cfg) + + # post reconfigure + self.default_agent = self.sim.get_agent(self.agent_id) + + new_agent_state = habitat_sim.AgentState() + new_agent_state.position = self.agent_start_location + new_agent_state.rotation = quat_from_angle_axis( + 0.5 * np.pi, + np.array([0, 1, 0]), + ) + self.default_agent.set_state(new_agent_state) + + self.render_camera = self.default_agent.scene_node.node_sensor_suite.get( + "color_sensor" + ) + + # set sim_settings scene name as actual loaded scene + self.sim_settings["scene"] = self.sim.curr_scene_name + + # Initialize replay renderer + if self.enable_batch_renderer and self.replay_renderer is None: + self.replay_renderer_cfg = ReplayRendererConfiguration() + self.replay_renderer_cfg.num_environments = self.num_env + self.replay_renderer_cfg.standalone = ( + False # Context is owned by the GLFW window + ) + self.replay_renderer_cfg.sensor_specifications = self.cfg.agents[ + self.agent_id + ].sensor_specifications + self.replay_renderer_cfg.gpu_device_id = self.cfg.sim_cfg.gpu_device_id + self.replay_renderer_cfg.force_separate_semantic_scene_graph = False + self.replay_renderer_cfg.leave_context_with_background_renderer = False + self.replay_renderer = ReplayRenderer.create_batch_replay_renderer( + self.replay_renderer_cfg + ) + # Pre-load composite files + if sim_settings["composite_files"] is not None: + for composite_file in sim_settings["composite_files"]: + self.replay_renderer.preload_file(composite_file) + + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + # check that clearing joint positions on save won't corrupt the content + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + for joint_val in ao.joint_positions: + assert ( + joint_val == 0 + ), "If this fails, there are non-zero joint positions in the scene_instance or default pose. Export with 'i' will clear these." + + Timer.start() + self.step = -1 + + def render_batch(self): + """ + This method updates the replay manager with the current state of environments and renders them. + """ + for i in range(self.num_env): + # Apply keyframe + keyframe = self.tiled_sims[i].gfx_replay_manager.extract_keyframe() + self.replay_renderer.set_environment_keyframe(i, keyframe) + # Copy sensor transforms + sensor_suite = self.tiled_sims[i]._sensors + for sensor_uuid, sensor in sensor_suite.items(): + transform = sensor._sensor_object.node.absolute_transformation() + self.replay_renderer.set_sensor_transform(i, sensor_uuid, transform) + # Render + self.replay_renderer.render(mn.gl.default_framebuffer) + + def move_and_look(self, repetitions: int) -> None: + """ + This method is called continuously with `self.draw_event` to monitor + any changes in the movement keys map `Dict[KeyEvent.key, Bool]`. + When a key in the map is set to `True` the corresponding action is taken. + """ + if repetitions == 0: + return + + agent = self.sim.agents[self.agent_id] + press: Dict[Application.Key.key, bool] = self.pressed + act: Dict[Application.Key.key, str] = self.key_to_action + + action_queue: List[str] = [act[k] for k, v in press.items() if v] + + for _ in range(int(repetitions)): + [agent.act(x) for x in action_queue] + + def invert_gravity(self) -> None: + """ + Sets the gravity vector to the negative of it's previous value. This is + a good method for testing simulation functionality. + """ + gravity: mn.Vector3 = self.sim.get_gravity() * -1 + self.sim.set_gravity(gravity) + + def key_press_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key press by performing the corresponding functions. + If the key pressed is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the + key will be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + pressed = Application.Key + mod = Application.Modifier + + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + # warning: ctrl doesn't always pass through with other key-presses + + if key == pressed.ESC: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + elif key == pressed.TAB: + self.cycle_through_urdfs(shift_pressed=shift_pressed) + + elif key == pressed.SPACE: + if not self.sim.config.sim_cfg.enable_physics: + logger.warn("Warning: physics was not enabled during setup") + else: + self.simulating = not self.simulating + logger.info(f"Command: physics simulating set to {self.simulating}") + + elif key == pressed.PERIOD: + if self.simulating: + logger.warn("Warning: physics simulation already running") + else: + self.simulate_single_step = True + logger.info("Command: physics step taken") + + elif key == pressed.COMMA: + self.debug_bullet_draw = not self.debug_bullet_draw + logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") + + elif key == pressed.B: + # Save all markersets that have been changed + self.markersets_util.save_all_dirty_markersets() + + elif key == pressed.C: + self.contact_debug_draw = not self.contact_debug_draw + log_str = f"Command: toggle contact debug draw: {self.contact_debug_draw}" + if self.contact_debug_draw: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + # TODO: add a nice log message with concise contact pair naming. + log_str = f"{log_str}: performing discrete collision detection and visualize active contacts." + self.sim.perform_discrete_collision_detection() + logger.info(log_str) + elif key == pressed.Q: + # rotate selected object(s) to left + self.navmesh_dirty = self.obj_editor.edit_left(self.navmesh_dirty) + elif key == pressed.E: + # rotate selected object(s) right + self.navmesh_dirty = self.obj_editor.edit_right(self.navmesh_dirty) + elif key == pressed.R: + # cycle through rotation amount + self.obj_editor.change_edit_vals(toggle=shift_pressed) + elif key == pressed.F: + self.force_urdf_notes_save = not self.force_urdf_notes_save + print( + f"Force save of hash to URDF notes file set to {self.force_urdf_notes_save}" + ) + elif key == pressed.G: + # If shift pressed then open, otherwise close + # If alt pressed then selected, otherwise all + self.obj_editor.set_ao_joint_states( + do_open=shift_pressed, selected=alt_pressed + ) + if not shift_pressed: + # if closing then redo navmesh + self.navmesh_config_and_recompute() + elif key == pressed.H: + self.print_help_text() + elif key == pressed.K: + # Cyle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + + elif key == pressed.M: + self.cycle_mouse_mode() + logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + + elif key == pressed.N: + # (default) - toggle navmesh visualization + # NOTE: (+ALT) - re-sample the agent position on the NavMesh + # NOTE: (+SHIFT) - re-compute the NavMesh + if alt_pressed: + logger.info("Command: resample agent state from navmesh") + if self.sim.pathfinder.is_loaded: + new_agent_state = habitat_sim.AgentState() + new_agent_state.position = ( + self.sim.pathfinder.get_random_navigable_point() + ) + new_agent_state.rotation = quat_from_angle_axis( + self.sim.random.uniform_float(0, 2.0 * np.pi), + np.array([0, 1, 0]), + ) + self.default_agent.set_state(new_agent_state) + else: + logger.warning( + "NavMesh is not initialized. Cannot sample new agent state." + ) + elif shift_pressed: + logger.info("Command: recompute navmesh") + self.navmesh_config_and_recompute() + else: + if self.sim.pathfinder.is_loaded: + self.sim.navmesh_visualization = not self.sim.navmesh_visualization + logger.info("Command: toggle navmesh") + else: + logger.warn("Warning: recompute navmesh first") + + elif key == pressed.V: + # self.invert_gravity() + # logger.info("Command: gravity inverted") + # Duplicate all the selected objects and place them in the scene + # or inject a new object by queried handle substring in front of + # the agent if no objects selected + + new_obj_list, self.navmesh_dirty = self.obj_editor.build_objects( + self.navmesh_dirty, + build_loc=self.ao_place_location, + ) + if len(new_obj_list) == 0: + print("Failed to add any new objects.") + else: + print(f"Finished adding {len(new_obj_list)} object(s).") + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + self.markersets_util.update_markersets() + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = True + event.accepted = True + self.redraw() + + def key_release_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key release. When a key is released, if it + is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the key will + be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = False + event.accepted = True + self.redraw() + + def calc_mouse_cast_results(self, screen_location: mn.Vector3) -> None: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(screen_location)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + self.mouse_cast_has_hits = ( + mouse_cast_results is not None and mouse_cast_results.has_hits() + ) + self.mouse_cast_results = mouse_cast_results + + def is_left_mse_btn( + self, event: Union[Application.PointerEvent, Application.PointerMoveEvent] + ) -> bool: + """ + Returns whether the left mouse button is pressed + """ + if isinstance(event, Application.PointerEvent): + return event.pointer == Application.Pointer.MOUSE_LEFT + elif isinstance(event, Application.PointerMoveEvent): + return event.pointers & Application.Pointer.MOUSE_LEFT + else: + return False + + def is_right_mse_btn( + self, event: Union[Application.PointerEvent, Application.PointerMoveEvent] + ) -> bool: + """ + Returns whether the right mouse button is pressed + """ + if isinstance(event, Application.PointerEvent): + return event.pointer == Application.Pointer.MOUSE_RIGHT + elif isinstance(event, Application.PointerMoveEvent): + return event.pointers & Application.Pointer.MOUSE_RIGHT + else: + return False + + def pointer_move_event(self, event: Application.PointerMoveEvent) -> None: + """ + Handles `Application.PointerMoveEvent`. When in LOOK mode, enables the left + mouse button to steer the agent's facing direction. + """ + + # if interactive mode -> LOOK MODE + if self.is_left_mse_btn(event) and self.mouse_interaction == MouseMode.LOOK: + agent = self.sim.agents[self.agent_id] + delta = self.get_mouse_position(event.relative_position) / 2 + action = habitat_sim.agent.ObjectControls() + act_spec = habitat_sim.agent.ActuationSpec + + # left/right on agent scene node + action(agent.scene_node, "turn_right", act_spec(delta.x)) + + # up/down on cameras' scene nodes + action = habitat_sim.agent.ObjectControls() + sensors = list(self.default_agent.scene_node.subtree_sensors.values()) + [action(s.object, "look_down", act_spec(delta.y), False) for s in sensors] + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def pointer_press_event(self, event: Application.PointerEvent) -> None: + """ + Handles `Application.PointerEvent`. When in MARKER mode : + LEFT CLICK : places a marker at mouse position on targeted object if not the stage + RIGHT CLICK : removes the closest marker to mouse position on targeted object + """ + physics_enabled = self.sim.get_physics_simulation_library() + is_left_mse_btn = self.is_left_mse_btn(event) + is_right_mse_btn = self.is_right_mse_btn(event) + mod = Application.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + # alt_pressed = bool(event.modifiers & mod.ALT) + self.calc_mouse_cast_results(event.position) + + if physics_enabled and self.mouse_cast_has_hits: + # If look enabled + if self.mouse_interaction == MouseMode.LOOK: + mouse_cast_results = self.mouse_cast_results + if is_right_mse_btn: + # Find object being clicked + obj_found = False + obj = None + # find first non-stage object + hit_idx = 0 + while hit_idx < len(mouse_cast_results.hits) and not obj_found: + self.last_hit_details = mouse_cast_results.hits[hit_idx] + hit_obj_id = mouse_cast_results.hits[hit_idx].object_id + obj = hsim_physics.get_obj_from_id(self.sim, hit_obj_id) + if obj is None: + hit_idx += 1 + else: + obj_found = True + if obj_found: + print( + f"Object: {obj.handle} is {'Articlated' if obj.is_articulated else 'Rigid'} Object at {obj.translation}" + ) + else: + print("This is the stage.") + + if not shift_pressed: + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(obj) + elif obj_found: + # add or remove object from selected objects, depending on whether it is already selected or not + self.obj_editor.toggle_sel_obj(obj) + # else if marker enabled + elif self.mouse_interaction == MouseMode.MARKER: + # hit_info = self.mouse_cast_results.hits[0] + sel_obj = self.markersets_util.place_marker_at_hit_location( + self.mouse_cast_results.hits[0], + self.ao_link_map, + is_left_mse_btn, + ) + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(sel_obj) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def scroll_event(self, event: Application.ScrollEvent) -> None: + """ + Handles `Application.ScrollEvent`. When in LOOK mode, enables camera + zooming (fine-grained zoom using shift) When in MARKER mode, wheel cycles through available taskset names + """ + scroll_mod_val = ( + event.offset.y + if abs(event.offset.y) > abs(event.offset.x) + else event.offset.x + ) + if not scroll_mod_val: + return + + # use shift to scale action response + mod = Application.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + # alt_pressed = bool(event.modifiers & mod.ALT) + # ctrl_pressed = bool(event.modifiers & mod.CTRL) + + # if interactive mode is False -> LOOK MODE + if self.mouse_interaction == MouseMode.LOOK: + # use shift for fine-grained zooming + mod_val = 1.01 if shift_pressed else 1.1 + mod = mod_val if scroll_mod_val > 0 else 1.0 / mod_val + cam = self.render_camera + cam.zoom(mod) + self.redraw() + + elif self.mouse_interaction == MouseMode.MARKER: + self.markersets_util.cycle_current_taskname(scroll_mod_val > 0) + self.redraw() + event.accepted = True + + def pointer_release_event(self, event: Application.PointerEvent) -> None: + """ + Release any existing constraints. + """ + event.accepted = True + + def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> mn.Vector2i: + """ + This function will get a screen-space mouse position appropriately + scaled based on framebuffer size and window size. Generally these would be + the same value, but on certain HiDPI displays (Retina displays) they may be + different. + """ + scaling = mn.Vector2i(self.framebuffer_size) / mn.Vector2i(self.window_size) + return mouse_event_position * scaling + + def cycle_mouse_mode(self) -> None: + """ + This method defines how to cycle through the mouse mode. + """ + if self.mouse_interaction == MouseMode.LOOK: + self.mouse_interaction = MouseMode.MARKER + elif self.mouse_interaction == MouseMode.MARKER: + self.mouse_interaction = MouseMode.LOOK + + def navmesh_config_and_recompute(self) -> None: + """ + This method is setup to be overridden in for setting config accessibility + in inherited classes. + """ + if self.cfg.sim_cfg.scene_id.lower() == "none": + return + self.navmesh_settings = habitat_sim.NavMeshSettings() + self.navmesh_settings.set_defaults() + self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height + self.navmesh_settings.agent_radius = self.cfg.agents[self.agent_id].radius + self.navmesh_settings.include_static_objects = True + self.sim.recompute_navmesh( + self.sim.pathfinder, + self.navmesh_settings, + ) + + def exit_event(self, event: Application.ExitEvent): + """ + Overrides exit_event to properly close the Simulator before exiting the + application. + """ + for i in range(self.num_env): + self.tiled_sims[i].close(destroy=True) + event.accepted = True + exit(0) + + def draw_text(self, sensor_spec): + # make magnum text background transparent for text + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + + self.shader.bind_vector_texture(self.glyph_cache.texture) + self.shader.transformation_projection_matrix = self.window_text_transform + self.shader.color = [1.0, 1.0, 1.0] + + sensor_type_string = str(sensor_spec.sensor_type.name) + sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + if self.mouse_interaction == MouseMode.LOOK: + mouse_mode_string = "LOOK" + elif self.mouse_interaction == MouseMode.MARKER: + mouse_mode_string = "MARKER" + edit_string = self.obj_editor.edit_disp_str() + self.window_text.render( + f""" +{self.fps} FPS +Sensor Type: {sensor_type_string} +Sensor Subtype: {sensor_subtype_string} +{edit_string} +Selected MarkerSets TaskSet name : {self.markersets_util.get_current_taskname()} +Mouse Interaction Mode: {mouse_mode_string} +FORCE SAVE URDF HASH IN NOTES FILE : {self.force_urdf_notes_save} + """ + ) + self.shader.draw(self.window_text.mesh) + + # Disable blending for text + mn.gl.Renderer.disable(mn.gl.Renderer.Feature.BLENDING) + + def print_help_text(self) -> None: + """ + Print the Key Command help text. + """ + logger.info( + """ +===================================================== +Welcome to the Habitat-sim Python Viewer application! +===================================================== +Mouse Functions ('m' to toggle mode): +---------------- +In LOOK mode (default): + LEFT: + Click and drag to rotate the agent and look up/down. + WHEEL: + Modify orthographic camera zoom/perspective camera FOV (+SHIFT for fine grained control) + +In MARKER mode : + LEFT CLICK : Add a marker to the target object at the mouse location, if not the stage + RIGHT CLICK : Remove the closest marker to the mouse location on the target object + +Key Commands: +------------- + esc: Exit the application. + 'h': Display this help message. + 'm': Cycle mouse interaction modes. + + Agent Controls: + 'wasd': Move the agent's body forward/backward and left/right. + 'zx': Move the agent's body up/down. + arrow keys: Turn the agent's body left/right and camera look up/down. + + Utilities: + 'r': Reset the simulator with the most recently loaded scene. + 'n': Show/hide NavMesh wireframe. + (+SHIFT) Recompute NavMesh with default settings. + (+ALT) Re-sample the agent(camera)'s position and orientation from the NavMesh. + ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). + 'c': Run a discrete collision detection pass and render a debug wireframe overlay showing active contact points and normals (yellow=fixed length normals, red=collision distances). + (+SHIFT) Toggle the contact point debug render overlay on/off. + 'g' : Modify AO link states : + (+SHIFT) : Open Selected AO + (-SHIFT) : Close Selected AO + Object Interactions: + SPACE: Toggle physics simulation on/off. + '.': Take a single simulation step if not simulating continuously. + 'v': (physics) Invert gravity. + 't': Load URDF from filepath + (+SHIFT) quick re-load the previously specified URDF + (+ALT) load the URDF with fixed base +===================================================== +""" + ) + + +class MouseMode(Enum): + LOOK = 0 + MARKER = 2 + + +class Timer: + """ + Timer class used to keep track of time between buffer swaps + and guide the display frame rate. + """ + + start_time = 0.0 + prev_frame_time = 0.0 + prev_frame_duration = 0.0 + running = False + + @staticmethod + def start() -> None: + """ + Starts timer and resets previous frame time to the start time. + """ + Timer.running = True + Timer.start_time = time.time() + Timer.prev_frame_time = Timer.start_time + Timer.prev_frame_duration = 0.0 + + @staticmethod + def stop() -> None: + """ + Stops timer and erases any previous time data, resetting the timer. + """ + Timer.running = False + Timer.start_time = 0.0 + Timer.prev_frame_time = 0.0 + Timer.prev_frame_duration = 0.0 + + @staticmethod + def next_frame() -> None: + """ + Records previous frame duration and updates the previous frame timestamp + to the current time. If the timer is not currently running, perform nothing. + """ + if not Timer.running: + return + Timer.prev_frame_duration = time.time() - Timer.prev_frame_time + Timer.prev_frame_time = time.time() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + # optional arguments + parser.add_argument( + "--scene", + default="./data/test_assets/scenes/simple_room.glb", + type=str, + help='scene/stage file to load (default: "./data/test_assets/scenes/simple_room.glb")', + ) + parser.add_argument( + "--dataset", + default="default", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "default")', + ) + parser.add_argument( + "--disable-physics", + action="store_true", + help="disable physics simulation (default: False)", + ) + parser.add_argument( + "--use-default-lighting", + action="store_true", + help="Override configured lighting to use default lighting for the stage.", + ) + parser.add_argument( + "--hbao", + action="store_true", + help="Enable horizon-based ambient occlusion, which provides soft shadows in corners and crevices.", + ) + parser.add_argument( + "--enable-batch-renderer", + action="store_true", + help="Enable batch rendering mode. The number of concurrent environments is specified with the num-environments parameter.", + ) + parser.add_argument( + "--num-environments", + default=1, + type=int, + help="Number of concurrent environments to batch render. Note that only the first environment simulates physics and can be controlled.", + ) + parser.add_argument( + "--composite-files", + type=str, + nargs="*", + help="Composite files that the batch renderer will use in-place of simulation assets to improve memory usage and performance. If none is specified, the original scene files will be loaded from disk.", + ) + parser.add_argument( + "--width", + default=800, + type=int, + help="Horizontal resolution of the window.", + ) + parser.add_argument( + "--height", + default=600, + type=int, + help="Vertical resolution of the window.", + ) + + args = parser.parse_args() + + if args.num_environments < 1: + parser.error("num-environments must be a positive non-zero integer.") + if args.width < 1: + parser.error("width must be a positive non-zero integer.") + if args.height < 1: + parser.error("height must be a positive non-zero integer.") + + # Setting up sim_settings + sim_settings: Dict[str, Any] = default_sim_settings + sim_settings["scene"] = args.scene + sim_settings["scene_dataset_config_file"] = args.dataset + sim_settings["enable_physics"] = not args.disable_physics + sim_settings["use_default_lighting"] = args.use_default_lighting + sim_settings["enable_batch_renderer"] = args.enable_batch_renderer + sim_settings["num_environments"] = args.num_environments + sim_settings["composite_files"] = args.composite_files + sim_settings["window_width"] = args.width + sim_settings["window_height"] = args.height + sim_settings["default_agent_navmesh"] = False + sim_settings["enable_hbao"] = args.hbao + + # start the application + HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/examples/mod_viewer.py b/examples/mod_viewer.py new file mode 100644 index 0000000000..0b4bb8b736 --- /dev/null +++ b/examples/mod_viewer.py @@ -0,0 +1,2497 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import ctypes +import json +import math +import os +import string +import sys +import time +from enum import Enum +from typing import Any, Callable, Dict, List, Optional, Tuple, Union + +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + +import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle +import magnum as mn +import numpy as np +from habitat.datasets.rearrange.navmesh_utils import ( + get_largest_island_index, + unoccluded_navmesh_snap, +) +from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler +from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer +from magnum import shaders, text +from magnum.platform.glfw import Application + +import habitat_sim +from habitat_sim import ReplayRenderer, ReplayRendererConfiguration, physics +from habitat_sim.gfx import DEFAULT_LIGHTING_KEY, DebugLineRender +from habitat_sim.logging import LoggingContext, logger +from habitat_sim.utils.classes import MarkerSetsEditor, ObjectEditor, SemanticDisplay +from habitat_sim.utils.common import quat_from_angle_axis +from habitat_sim.utils.namespace import hsim_physics +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# add tools directory so I can import things to try them in the viewer +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../tools")) +print(sys.path) + +# from tools import collision_shape_automation as csa + +# CollisionProxyOptimizer initialized before the application +# _cpo: Optional[csa.CollisionProxyOptimizer] = None +# _cpo_threads = [] + + +# def _cpo_initialized(): +# global _cpo +# global _cpo_threads +# if _cpo is None: +# return False +# return all(not thread.is_alive() for thread in _cpo_threads) + + +class RecColorMode(Enum): + """ + Defines the coloring mode for receptacle debug drawing. + """ + + DEFAULT = 0 # all magenta + GT_ACCESS = 1 # red to green + GT_STABILITY = 2 + PR_ACCESS = 3 + PR_STABILITY = 4 + FILTERING = 5 # colored by filter status (green=active, yellow=manually filtered, red=automatically filtered (access), magenta=automatically filtered (access), blue=automatically filtered (height)) + + +class ColorLERP: + """ + xyz lerp between two colors. + """ + + def __init__(self, c0: mn.Color4, c1: mn.Color4): + self.c0 = c0.to_xyz() + self.c1 = c1.to_xyz() + self.delta = self.c1 - self.c0 + + def at(self, t: float) -> mn.Color4: + """ + Compute the LERP at time t [0,1]. + """ + assert t >= 0 and t <= 1, "Extrapolation not recommended in color space." + t_color_xyz = self.c0 + self.delta * t + return mn.Color4.from_xyz(t_color_xyz) + + +# red to green lerp for heatmaps +rg_lerp = ColorLERP(mn.Color4.red(), mn.Color4.green()) + + +class HabitatSimInteractiveViewer(Application): + # the maximum number of chars displayable in the app window + # using the magnum text module. These chars are used to + # display the CPU/GPU usage data + MAX_DISPLAY_TEXT_CHARS = 512 + + # how much to displace window text relative to the center of the + # app window (e.g if you want the display text in the top left of + # the app window, you will displace the text + # window width * -TEXT_DELTA_FROM_CENTER in the x axis and + # window height * TEXT_DELTA_FROM_CENTER in the y axis, as the text + # position defaults to the middle of the app window) + TEXT_DELTA_FROM_CENTER = 0.49 + + # font size of the magnum in-window display text that displays + # CPU and GPU usage info + DISPLAY_FONT_SIZE = 16.0 + + def __init__( + self, + sim_settings: Dict[str, Any], + mm: Optional[habitat_sim.metadata.MetadataMediator] = None, + ) -> None: + self.sim_settings: Dict[str:Any] = sim_settings + + self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] + self.num_env: int = ( + self.sim_settings["num_environments"] if self.enable_batch_renderer else 1 + ) + + # Compute environment camera resolution based on the number of environments to render in the window. + window_size: mn.Vector2 = ( + self.sim_settings["window_width"], + self.sim_settings["window_height"], + ) + + configuration = self.Configuration() + configuration.title = "Habitat Sim Interactive Viewer" + configuration.size = window_size + Application.__init__(self, configuration) + self.fps: float = 60.0 + + # Compute environment camera resolution based on the number of environments to render in the window. + grid_size: mn.Vector2i = ReplayRenderer.environment_grid_size(self.num_env) + camera_resolution: mn.Vector2 = mn.Vector2(self.framebuffer_size) / mn.Vector2( + grid_size + ) + self.sim_settings["width"] = camera_resolution[0] + self.sim_settings["height"] = camera_resolution[1] + + # set up our movement map + + key = Application.Key + self.pressed = { + key.UP: False, + key.DOWN: False, + key.LEFT: False, + key.RIGHT: False, + key.A: False, + key.D: False, + key.S: False, + key.W: False, + key.X: False, + key.Z: False, + } + + # set up our movement key bindings map + self.key_to_action = { + key.UP: "look_up", + key.DOWN: "look_down", + key.LEFT: "turn_left", + key.RIGHT: "turn_right", + key.A: "move_left", + key.D: "move_right", + key.S: "move_backward", + key.W: "move_forward", + key.X: "move_down", + key.Z: "move_up", + } + + # Load a TrueTypeFont plugin and open the font file + self.display_font = text.FontManager().load_and_instantiate("TrueTypeFont") + relative_path_to_font = "../data/fonts/ProggyClean.ttf" + self.display_font.open_file( + os.path.join(os.path.dirname(__file__), relative_path_to_font), + 13, + ) + + # Glyphs we need to render everything + self.glyph_cache = text.GlyphCacheGL( + mn.PixelFormat.R8_UNORM, mn.Vector2i(256), mn.Vector2i(1) + ) + self.display_font.fill_glyph_cache( + self.glyph_cache, + string.ascii_lowercase + + string.ascii_uppercase + + string.digits + + ":-_+,.! %µ", + ) + + # magnum text object that displays CPU/GPU usage data in the app window + self.window_text = text.Renderer2D( + self.display_font, + self.glyph_cache, + HabitatSimInteractiveViewer.DISPLAY_FONT_SIZE, + text.Alignment.TOP_LEFT, + ) + self.window_text.reserve(HabitatSimInteractiveViewer.MAX_DISPLAY_TEXT_CHARS) + + # text object transform in window space is Projection matrix times Translation Matrix + # put text in top left of window + self.window_text_transform = mn.Matrix3.projection( + self.framebuffer_size + ) @ mn.Matrix3.translation( + mn.Vector2(self.framebuffer_size) + * mn.Vector2( + -HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + ) + ) + self.shader = shaders.VectorGL2D() + + # Set blend function + mn.gl.Renderer.set_blend_equation( + mn.gl.Renderer.BlendEquation.ADD, mn.gl.Renderer.BlendEquation.ADD + ) + + # variables that track app data and CPU/GPU usage + self.num_frames_to_track = 60 + + # global _cpo + # self._cpo = _cpo + # self.cpo_initialized = False + self.proxy_obj_postfix = "_collision_stand-in" + + # initialization code below here + # TODO isolate all initialization so tabbing through scenes can be properly supported + # configure our simulator + self.cfg: Optional[habitat_sim.simulator.Configuration] = None + self.sim: Optional[habitat_sim.simulator.Simulator] = None + self.tiled_sims: list[habitat_sim.simulator.Simulator] = None + self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None + self.replay_renderer: Optional[ReplayRenderer] = None + + # draw Bullet debug line visualizations (e.g. collision meshes) + self.debug_bullet_draw = False + # draw active contact point debug line visualizations + self.contact_debug_draw = False + + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" + + # Cycle mouse utilities + self.mouse_interaction = MouseMode.LOOK + self.mouse_grabber: Optional[MouseGrabber] = None + self.previous_mouse_point = None + + # toggle physics simulation on/off + self.simulating = False + # toggle a single simulation step at the next opportunity if not + # simulating continuously. + self.simulate_single_step = False + + # receptacle visualization + self.receptacles = None + self.display_receptacles = False + self.show_filtered = True + self.rec_access_filter_threshold = 0.12 # empirically chosen + self.rec_color_mode = RecColorMode.FILTERING + # map receptacle to parent objects + self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} + self.poh_to_rec: Dict[str, List[hab_receptacle.Receptacle]] = {} + # contains filtering metadata and classification of meshes filtered automatically and manually + self.rec_filter_data = None + # TODO need to determine filter path for each scene during tabbing? + # Currently this field is only set as command-line argument + self.rec_filter_path = self.sim_settings["rec_filter_file"] + + # display stability samples for selected object w/ receptacle + self.display_selected_stability_samples = True + + # collision proxy visualization + self.col_proxy_objs = None + self.col_proxies_visible = True + self.original_objs_visible = True + + # mouse raycast visualization + self.mouse_cast_results = None + self.mouse_cast_has_hits = False + + # last clicked or None for stage + self.selected_rec = None + self.ao_link_map = None + self.navmesh_dirty = False + + # index of the largest indoor island + self.largest_island_ix = -1 + + # Sim reconfigure + self.reconfigure_sim(mm) + + # load markersets for every object and ao into a cache + task_names_set = set() + task_names_set.add("faucets") + self.markersets_util = MarkerSetsEditor(self.sim, task_names_set) + + # Editing + self.obj_editor = ObjectEditor(self.sim) + + # Semantics + self.dbg_semantics = SemanticDisplay(self.sim) + + # sys.exit(0) + # load appropriate filter file for scene + self.load_scene_filter_file() + + # ----------------------------------------- + # Clutter Generation Integration: + self.clutter_object_set = [ + "002_master_chef_can", + "003_cracker_box", + "004_sugar_box", + "005_tomato_soup_can", + "007_tuna_fish_can", + "008_pudding_box", + "009_gelatin_box", + "010_potted_meat_can", + "024_bowl", + ] + self.clutter_object_handles = [] + self.clutter_object_instances = [] + # cache initial states for classification of unstable objects + self.clutter_object_initial_states = [] + self.num_unstable_objects = 0 + # add some clutter objects to the MM + self.sim.metadata_mediator.object_template_manager.load_configs( + "data/objects/ycb/configs/" + ) + self.initialize_clutter_object_set() + # ----------------------------------------- + + # compute NavMesh if not already loaded by the scene. + if ( + not self.sim.pathfinder.is_loaded + and self.cfg.sim_cfg.scene_id.lower() != "none" + and not self.sim_settings["viewer_ignore_navmesh"] + ): + self.navmesh_config_and_recompute() + + self.time_since_last_simulation = 0.0 + LoggingContext.reinitialize_from_env() + logger.setLevel("INFO") + self.print_help_text() + + def modify_param_from_term(self): + """ + Prompts the user to enter an attribute name and new value. + Attempts to fulfill the user's request. + """ + # first get an attribute + user_attr = input("++++++++++++\nProvide an attribute to edit: ") + if not hasattr(self, user_attr): + print(f" The '{user_attr}' attribute does not exist.") + return + + # then get a value + user_val = input(f"Now provide a value for '{user_attr}': ") + cur_attr_val = getattr(self, user_attr) + if cur_attr_val is not None: + try: + # try type conversion + new_val = type(cur_attr_val)(user_val) + + # special handling for bool because all strings become True with cast + if isinstance(cur_attr_val, bool): + if user_val.lower() == "false": + new_val = False + elif user_val.lower() == "true": + new_val = True + + setattr(self, user_attr, new_val) + print( + f"attr '{user_attr}' set to '{getattr(self, user_attr)}' (type={type(new_val)})." + ) + except Exception: + print(f"Failed to cast '{user_val}' to {type(cur_attr_val)}.") + else: + print("That attribute is unset, so I don't know the type.") + + def load_scene_filter_file(self): + """ + Load the filter file for a scene from config. + """ + + scene_user_defined = self.sim.metadata_mediator.get_scene_user_defined( + self.sim.curr_scene_name + ) + if scene_user_defined is not None and scene_user_defined.has_value( + "scene_filter_file" + ): + scene_filter_file = scene_user_defined.get("scene_filter_file") + # construct the dataset level path for the filter data file + scene_filter_file = os.path.join( + os.path.dirname(mm.active_dataset), scene_filter_file + ) + print(f"scene_filter_file = {scene_filter_file}") + self.load_receptacles() + self.load_filtered_recs(scene_filter_file) + self.rec_filter_path = scene_filter_file + else: + print( + f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_name}." + ) + + def get_closest_tri_receptacle( + self, pos: mn.Vector3, max_dist: float = 3.5 + ) -> Optional[hab_receptacle.TriangleMeshReceptacle]: + """ + Return the closest receptacle to the given position or None. + + :param pos: The point to compare with receptacle verts. + :param max_dist: The maximum allowable distance to the receptacle to count. + + :return: None if failed or closest receptacle. + """ + if self.receptacles is None or not self.display_receptacles: + return None + closest_rec = None + closest_rec_dist = max_dist + for obj in self.obj_editor.sel_objs: + # find for all currently selected objects + recs = ( + self.receptacles + if (obj is None or obj.handle not in self.poh_to_rec) + else self.poh_to_rec[obj.handle] + ) + for receptacle in recs: + g_trans = receptacle.get_global_transform(self.sim) + if (g_trans.translation - pos).length() < max_dist: + # receptacles object transform should be close to the point + if isinstance(receptacle, hab_receptacle.TriangleMeshReceptacle): + r_dist = receptacle.dist_to_rec(self.sim, pos) + if r_dist < closest_rec_dist: + closest_rec_dist = r_dist + closest_rec = receptacle + else: + global_keypoints = None + if isinstance(receptacle, hab_receptacle.AABBReceptacle): + global_keypoints = ( + hsim_physics.get_global_keypoints_from_bb( + receptacle.bounds, g_trans + ) + ) + elif isinstance(receptacle, hab_receptacle.AnyObjectReceptacle): + global_keypoints = hsim_physics.get_bb_corners( + receptacle._get_global_bb(self.sim) + ) + + for g_point in global_keypoints: + v_dist = (pos - g_point).length() + if v_dist < closest_rec_dist: + closest_rec_dist = v_dist + closest_rec = receptacle + + return closest_rec + + def compute_rec_filter_state( + self, + access_threshold: float = 0.12, + stab_threshold: float = 0.5, + filter_shape: str = "pr0", + ) -> None: + """ + Check all receptacles against automated filters to fill the + + :param access_threshold: Access threshold for filtering. Roughly % of sample points with some raycast access. + :param stab_threshold: Stability threshold for filtering. Roughly % of sample points with stable object support. + :param filter_shape: Which shape metrics to use for filter. Choices typically "gt"(ground truth) or "pr0"(proxy shape). + """ + # load receptacles if not done + if self.receptacles is None: + self.load_receptacles() + # assert ( + # self._cpo is not None + # ), "Must initialize the CPO before automatic filtering. Re-run with '--init-cpo'." + + # initialize if necessary + if self.rec_filter_data is None: + self.rec_filter_data = { + "active": [], + "manually_filtered": [], + "access_filtered": [], + "access_threshold": access_threshold, # set in filter procedure + "stability_filtered": [], + "stability threshold": stab_threshold, # set in filter procedure + "cook_surface": [], + # TODO: + "height_filtered": [], + "max_height": 0, + "min_height": 0, + } + + # for rec in self.receptacles: + # rec_unique_name = rec.unique_name + # # respect already marked receptacles + # if rec_unique_name not in self.rec_filter_data["manually_filtered"]: + # rec_dat = self._cpo.gt_data[self.rec_to_poh[rec]]["receptacles"][ + # rec.name + # ] + # rec_shape_data = rec_dat["shape_id_results"][filter_shape] + # # filter by access + # if ( + # "access_results" in rec_shape_data + # and rec_shape_data["access_results"]["receptacle_access_score"] + # < access_threshold + # ): + # self.rec_filter_data["access_filtered"].append(rec_unique_name) + # # filter by stability + # elif ( + # "stability_results" in rec_shape_data + # and rec_shape_data["stability_results"]["success_ratio"] + # < stab_threshold + # ): + # self.rec_filter_data["stability_filtered"].append(rec_unique_name) + # # TODO: add more filters + # # TODO: 1. filter by height relative to the floor + # # TODO: 2. filter outdoor (raycast up) + # # TODO: 3/4: filter by access/stability in scene context (relative to other objects) + # # remaining receptacles are active + # else: + # self.rec_filter_data["active"].append(rec_unique_name) + + def export_filtered_recs(self, filepath: Optional[str] = None) -> None: + """ + Save a JSON with filtering metadata and filtered Receptacles for a scene. + + :param filepath: Defines the output filename for this JSON. If omitted, defaults to "./rec_filter_data.json". + """ + if filepath is None: + filepath = "rec_filter_data.json" + os.makedirs(os.path.dirname(filepath), exist_ok=True) + with open(filepath, "w") as f: + f.write(json.dumps(self.rec_filter_data, indent=2)) + print(f"Exported filter annotations to {filepath}.") + + def load_filtered_recs(self, filepath: Optional[str] = None) -> None: + """ + Load a Receptacle filtering metadata JSON to visualize the state of the scene. + + :param filepath: Defines the input filename for this JSON. If omitted, defaults to "./rec_filter_data.json". + """ + if filepath is None: + filepath = "rec_filter_data.json" + if not os.path.exists(filepath): + print(f"Filtered rec metadata file {filepath} does not exist. Cannot load.") + return + with open(filepath, "r") as f: + self.rec_filter_data = json.load(f) + + # assert the format is correct + assert "active" in self.rec_filter_data + assert "manually_filtered" in self.rec_filter_data + assert "access_filtered" in self.rec_filter_data + assert "stability_filtered" in self.rec_filter_data + assert "height_filtered" in self.rec_filter_data + print(f"Loaded filter annotations from {filepath}") + + def load_receptacles(self): + """ + Load all receptacle data and setup helper datastructures. + """ + self.receptacles = hab_receptacle.find_receptacles(self.sim) + self.receptacles = [ + rec + for rec in self.receptacles + if "collision_stand-in" not in rec.parent_object_handle + ] + for receptacle in self.receptacles: + if receptacle not in self.rec_to_poh: + po_handle = hsim_physics.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ).creation_attributes.handle + self.rec_to_poh[receptacle] = po_handle + if receptacle.parent_object_handle not in self.poh_to_rec: + self.poh_to_rec[receptacle.parent_object_handle] = [] + self.poh_to_rec[receptacle.parent_object_handle].append(receptacle) + + def add_col_proxy_object( + self, obj_instance: habitat_sim.physics.ManagedRigidObject + ) -> habitat_sim.physics.ManagedRigidObject: + """ + Add a collision object visualization proxy to the scene overlapping with the given object. + Return the new proxy object. + """ + # replace the object with a collision_object + obj_temp_handle = obj_instance.creation_attributes.handle + otm = self.sim.get_object_template_manager() + object_template = otm.get_template_by_handle(obj_temp_handle) + object_template.scale = obj_instance.scale + np.ones(3) * 0.01 + object_template.render_asset_handle = object_template.collision_asset_handle + object_template.is_collidable = False + reg_id = otm.register_template( + object_template, + object_template.handle + self.proxy_obj_postfix, + ) + ro_mngr = self.sim.get_rigid_object_manager() + new_obj = ro_mngr.add_object_by_template_id(reg_id) + new_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + new_obj.translation = obj_instance.translation + new_obj.rotation = obj_instance.rotation + return new_obj + + def draw_contact_debug(self, debug_line_render: Any): + """ + This method is called to render a debug line overlay displaying active contact points and normals. + Yellow lines show the contact distance along the normal and red lines show the contact normal at a fixed length. + """ + yellow = mn.Color4.yellow() + red = mn.Color4.red() + cps = self.sim.get_physics_contact_points() + debug_line_render.set_line_width(1.5) + camera_position = self.render_camera.render_camera.node.absolute_translation + # only showing active contacts + active_contacts = (x for x in cps if x.is_active) + for cp in active_contacts: + # red shows the contact distance + debug_line_render.draw_transformed_line( + cp.position_on_b_in_ws, + cp.position_on_b_in_ws + + cp.contact_normal_on_b_in_ws * -cp.contact_distance, + red, + ) + # yellow shows the contact normal at a fixed length for visualization + debug_line_render.draw_transformed_line( + cp.position_on_b_in_ws, + # + cp.contact_normal_on_b_in_ws * cp.contact_distance, + cp.position_on_b_in_ws + cp.contact_normal_on_b_in_ws * 0.1, + yellow, + ) + debug_line_render.draw_circle( + translation=cp.position_on_b_in_ws, + radius=0.005, + color=yellow, + normal=camera_position - cp.position_on_b_in_ws, + ) + + def _draw_receptacle_per_obj(self, obj, debug_line_render): + # if self.rec_filter_data is None and self.cpo_initialized: + # self.compute_rec_filter_state( + # access_threshold=self.rec_access_filter_threshold + # ) + c_pos = self.render_camera.node.absolute_translation + c_forward = self.render_camera.node.absolute_transformation().transform_vector( + mn.Vector3(0, 0, -1) + ) + for receptacle in self.receptacles: + rec_unique_name = receptacle.unique_name + # filter all non-active receptacles + if ( + self.rec_filter_data is not None + and not self.show_filtered + and rec_unique_name not in self.rec_filter_data["active"] + ): + continue + + rec_dat = None + # if self.cpo_initialized: + # rec_dat = self._cpo.gt_data[self.rec_to_poh[receptacle]]["receptacles"][ + # receptacle.name + # ] + + r_trans = receptacle.get_global_transform(self.sim) + # display point samples for selected object + if ( + rec_dat is not None + and self.display_selected_stability_samples + and obj is not None + and obj.handle == receptacle.parent_object_handle + ): + # display colored circles for stability samples on the selected object + point_metric_dat = rec_dat["shape_id_results"]["gt"]["access_results"][ + "receptacle_point_access_scores" + ] + if self.rec_color_mode == RecColorMode.GT_STABILITY: + point_metric_dat = rec_dat["shape_id_results"]["gt"][ + "stability_results" + ]["point_stabilities"] + elif self.rec_color_mode == RecColorMode.PR_STABILITY: + point_metric_dat = rec_dat["shape_id_results"]["pr0"][ + "stability_results" + ]["point_stabilities"] + elif self.rec_color_mode == RecColorMode.PR_ACCESS: + point_metric_dat = rec_dat["shape_id_results"]["pr0"][ + "access_results" + ]["receptacle_point_access_scores"] + + for point_metric, point in zip( + point_metric_dat, + rec_dat["sample_points"], + ): + debug_line_render.draw_circle( + translation=r_trans.transform_point(point), + radius=0.02, + normal=mn.Vector3(0, 1, 0), + color=rg_lerp.at(point_metric), + num_segments=12, + ) + + rec_obj = hsim_physics.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ) + key_points = [r_trans.translation] + key_points.extend( + hsim_physics.get_bb_corners(rec_obj.root_scene_node.cumulative_bb) + ) + + in_view = False + for ix, key_point in enumerate(key_points): + r_pos = key_point + if ix > 0: + r_pos = rec_obj.transformation.transform_point(key_point) + c_to_r = r_pos - c_pos + # only display receptacles within 8 meters centered in view + if ( + c_to_r.length() < 8 + and mn.math.dot((c_to_r).normalized(), c_forward) > 0.7 + ): + in_view = True + break + if in_view: + # handle coloring + rec_color = None + if self.selected_rec == receptacle: + # white + rec_color = mn.Color4.cyan() + elif ( + self.rec_filter_data is not None + ) and self.rec_color_mode == RecColorMode.FILTERING: + # blue indicates no filter data for the receptacle, it may be newer than the filter file. + rec_color = mn.Color4.blue() + if ( + "cook_surface" in self.rec_filter_data + and rec_unique_name in self.rec_filter_data["cook_surface"] + ): + rec_color = mn.Color4(1.0, 0.66, 0.0, 1.0) # orange again + elif rec_unique_name in self.rec_filter_data["active"]: + rec_color = mn.Color4.green() + elif rec_unique_name in self.rec_filter_data["manually_filtered"]: + rec_color = mn.Color4.yellow() + elif rec_unique_name in self.rec_filter_data["access_filtered"]: + rec_color = mn.Color4.red() + elif rec_unique_name in self.rec_filter_data["stability_filtered"]: + rec_color = mn.Color4.magenta() + elif rec_unique_name in self.rec_filter_data["height_filtered"]: + # I changed the height filter from orange to dark purple + rec_color = mn.Color4(0.5, 0, 0.5, 1.0) + # elif ( + # self.cpo_initialized and self.rec_color_mode != RecColorMode.DEFAULT + # ): + # if self.rec_color_mode == RecColorMode.GT_STABILITY: + # rec_color = rg_lerp.at( + # rec_dat["shape_id_results"]["gt"]["stability_results"][ + # "success_ratio" + # ] + # ) + # elif self.rec_color_mode == RecColorMode.GT_ACCESS: + # rec_color = rg_lerp.at( + # rec_dat["shape_id_results"]["gt"]["access_results"][ + # "receptacle_access_score" + # ] + # ) + # elif self.rec_color_mode == RecColorMode.PR_STABILITY: + # rec_color = rg_lerp.at( + # rec_dat["shape_id_results"]["pr0"]["stability_results"][ + # "success_ratio" + # ] + # ) + # elif self.rec_color_mode == RecColorMode.PR_ACCESS: + # rec_color = rg_lerp.at( + # rec_dat["shape_id_results"]["pr0"]["access_results"][ + # "receptacle_access_score" + # ] + # ) + + receptacle.debug_draw(self.sim, color=rec_color) + if True: + t_form = receptacle.get_global_transform(self.sim) + debug_line_render.push_transform(t_form) + debug_line_render.draw_transformed_line( + mn.Vector3(0), receptacle.up, mn.Color4.cyan() + ) + debug_line_render.pop_transform() + + def draw_receptacles(self, debug_line_render): + for obj in self.obj_editor.sel_objs: + self._draw_receptacle_per_obj(obj, debug_line_render=debug_line_render) + + def debug_draw(self): + """ + Additional draw commands to be called during draw_event. + """ + if self.debug_bullet_draw: + render_cam = self.render_camera.render_camera + proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) + self.sim.physics_debug_draw(proj_mat) + + debug_line_render: DebugLineRender = self.sim.get_debug_line_render() + if self.contact_debug_draw: + self.draw_contact_debug(debug_line_render) + + # draw semantic information + self.dbg_semantics.draw_region_debug(debug_line_render=debug_line_render) + + # draw markersets information + if self.markersets_util.marker_sets_per_obj is not None: + self.markersets_util.draw_marker_sets_debug( + debug_line_render, + self.render_camera.render_camera.node.absolute_translation, + ) + if self.receptacles is not None and self.display_receptacles: + self.draw_receptacles(debug_line_render) + + self.obj_editor.draw_selected_objects(debug_line_render) + # mouse raycast circle + if self.mouse_cast_has_hits: + debug_line_render.draw_circle( + translation=self.mouse_cast_results.hits[0].point, + radius=0.005, + color=mn.Color4(mn.Vector3(1.0), 1.0), + normal=self.mouse_cast_results.hits[0].normal, + ) + + def draw_event( + self, + simulation_call: Optional[Callable] = None, + global_call: Optional[Callable] = None, + active_agent_id_and_sensor_name: Tuple[int, str] = (0, "color_sensor"), + ) -> None: + """ + Calls continuously to re-render frames and swap the two frame buffers + at a fixed rate. + """ + # until cpo initialization is finished, keep checking + # if not self.cpo_initialized: + # self.cpo_initialized = _cpo_initialized() + + agent_acts_per_sec = self.fps + + mn.gl.default_framebuffer.clear( + mn.gl.FramebufferClear.COLOR | mn.gl.FramebufferClear.DEPTH + ) + + # Agent actions should occur at a fixed rate per second + self.time_since_last_simulation += Timer.prev_frame_duration + num_agent_actions: int = self.time_since_last_simulation * agent_acts_per_sec + self.move_and_look(int(num_agent_actions)) + + # Occasionally a frame will pass quicker than 1/60 seconds + if self.time_since_last_simulation >= 1.0 / self.fps: + if self.simulating or self.simulate_single_step: + self.sim.step_world(1.0 / self.fps) + self.simulate_single_step = False + if simulation_call is not None: + simulation_call() + # compute object stability after physics step + self.num_unstable_objects = 0 + for obj_initial_state, obj in zip( + self.clutter_object_initial_states, self.clutter_object_instances + ): + translation_error = ( + obj_initial_state[0] - obj.translation + ).length() + if translation_error > 0.1: + self.num_unstable_objects += 1 + + if global_call is not None: + global_call() + if self.navmesh_dirty: + self.navmesh_config_and_recompute() + # reset time_since_last_simulation, accounting for potential overflow + self.time_since_last_simulation = math.fmod( + self.time_since_last_simulation, 1.0 / self.fps + ) + + keys = active_agent_id_and_sensor_name + + if self.enable_batch_renderer: + self.render_batch() + else: + self.sim._Simulator__sensors[keys[0]][keys[1]].draw_observation() + agent = self.sim.get_agent(keys[0]) + self.render_camera = agent.scene_node.node_sensor_suite.get(keys[1]) + self.debug_draw() + self.render_camera.render_target.blit_rgba_to_default() + + # draw CPU/GPU usage data and other info to the app window + mn.gl.default_framebuffer.bind() + self.draw_text(self.render_camera.specification()) + + self.swap_buffers() + Timer.next_frame() + self.redraw() + + def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: + """ + Set up our own agent and agent controls + """ + make_action_spec = habitat_sim.agent.ActionSpec + make_actuation_spec = habitat_sim.agent.ActuationSpec + MOVE, LOOK = 0.07, 1.5 + + # all of our possible actions' names + action_list = [ + "move_left", + "turn_left", + "move_right", + "turn_right", + "move_backward", + "look_up", + "move_forward", + "look_down", + "move_down", + "move_up", + ] + + action_space: Dict[str, habitat_sim.agent.ActionSpec] = {} + + # build our action space map + for action in action_list: + actuation_spec_amt = MOVE if "move" in action else LOOK + action_spec = make_action_spec( + action, make_actuation_spec(actuation_spec_amt) + ) + action_space[action] = action_spec + + sensor_spec: List[habitat_sim.sensor.SensorSpec] = self.cfg.agents[ + self.agent_id + ].sensor_specifications + + agent_config = habitat_sim.agent.AgentConfiguration( + height=1.5, + radius=0.1, + sensor_specifications=sensor_spec, + action_space=action_space, + body_type="cylinder", + ) + return agent_config + + def initialize_clutter_object_set(self) -> None: + """ + Get the template handles for configured clutter objects. + """ + + self.clutter_object_handles = [] + for obj_name in self.clutter_object_set: + matching_handles = ( + self.sim.metadata_mediator.object_template_manager.get_template_handles( + obj_name + ) + ) + assert ( + len(matching_handles) > 0 + ), f"No matching template for '{obj_name}' in the dataset." + self.clutter_object_handles.append(matching_handles[0]) + + def reconfigure_sim( + self, mm: Optional[habitat_sim.metadata.MetadataMediator] = None + ) -> None: + """ + Utilizes the current `self.sim_settings` to configure and set up a new + `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces + the current simulator instance, reloading the most recently loaded scene + """ + # configure our sim_settings but then set the agent to our default + self.cfg = make_cfg(self.sim_settings) + self.cfg.metadata_mediator = mm + self.agent_id: int = self.sim_settings["default_agent"] + self.cfg.agents[self.agent_id] = self.default_agent_config() + + if self.enable_batch_renderer: + self.cfg.enable_batch_renderer = True + self.cfg.sim_cfg.create_renderer = False + self.cfg.sim_cfg.enable_gfx_replay_save = True + + if self.sim_settings["use_default_lighting"]: + logger.info("Setting default lighting override for scene.") + self.cfg.sim_cfg.override_scene_light_defaults = True + self.cfg.sim_cfg.scene_light_setup = DEFAULT_LIGHTING_KEY + + if self.sim is None: + self.tiled_sims = [] + for _i in range(self.num_env): + self.tiled_sims.append(habitat_sim.Simulator(self.cfg)) + self.sim = self.tiled_sims[0] + else: # edge case + for i in range(self.num_env): + if ( + self.tiled_sims[i].config.sim_cfg.scene_id + == self.cfg.sim_cfg.scene_id + ): + # we need to force a reset, so change the internal config scene name + self.tiled_sims[i].config.sim_cfg.scene_id = "NONE" + self.tiled_sims[i].reconfigure(self.cfg) + + # #resave scene instance + # self.sim.save_current_scene_config(overwrite=True) + # sys. exit() + + # post reconfigure + self.default_agent = self.sim.get_agent(self.agent_id) + self.render_camera = self.default_agent.scene_node.node_sensor_suite.get( + "color_sensor" + ) + + # set sim_settings scene name as actual loaded scene + self.sim_settings["scene"] = self.sim.curr_scene_name + + # Initialize replay renderer + if self.enable_batch_renderer and self.replay_renderer is None: + self.replay_renderer_cfg = ReplayRendererConfiguration() + self.replay_renderer_cfg.num_environments = self.num_env + self.replay_renderer_cfg.standalone = ( + False # Context is owned by the GLFW window + ) + self.replay_renderer_cfg.sensor_specifications = self.cfg.agents[ + self.agent_id + ].sensor_specifications + self.replay_renderer_cfg.gpu_device_id = self.cfg.sim_cfg.gpu_device_id + self.replay_renderer_cfg.force_separate_semantic_scene_graph = False + self.replay_renderer_cfg.leave_context_with_background_renderer = False + self.replay_renderer = ReplayRenderer.create_batch_replay_renderer( + self.replay_renderer_cfg + ) + # Pre-load composite files + if sim_settings["composite_files"] is not None: + for composite_file in sim_settings["composite_files"]: + self.replay_renderer.preload_file(composite_file) + + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + self.dbv = DebugVisualizer(self.sim) + + Timer.start() + self.step = -1 + + def render_batch(self): + """ + This method updates the replay manager with the current state of environments and renders them. + """ + for i in range(self.num_env): + # Apply keyframe + keyframe = self.tiled_sims[i].gfx_replay_manager.extract_keyframe() + self.replay_renderer.set_environment_keyframe(i, keyframe) + # Copy sensor transforms + sensor_suite = self.tiled_sims[i]._sensors + for sensor_uuid, sensor in sensor_suite.items(): + transform = sensor._sensor_object.node.absolute_transformation() + self.replay_renderer.set_sensor_transform(i, sensor_uuid, transform) + # Render + self.replay_renderer.render(mn.gl.default_framebuffer) + + def move_and_look(self, repetitions: int) -> None: + """ + This method is called continuously with `self.draw_event` to monitor + any changes in the movement keys map `Dict[KeyEvent.key, Bool]`. + When a key in the map is set to `True` the corresponding action is taken. + """ + # avoids unnecessary updates to grabber's object position + if repetitions == 0: + return + + agent = self.sim.agents[self.agent_id] + press: Dict[Application.Key.key, bool] = self.pressed + act: Dict[Application.Key.key, str] = self.key_to_action + + action_queue: List[str] = [act[k] for k, v in press.items() if v] + + for _ in range(int(repetitions)): + [agent.act(x) for x in action_queue] + + # update the grabber transform when our agent is moved + if self.mouse_grabber is not None: + # update location of grabbed object + self.update_grab_position(self.previous_mouse_point) + + def invert_gravity(self) -> None: + """ + Sets the gravity vector to the negative of it's previous value. This is + a good method for testing simulation functionality. + """ + gravity: mn.Vector3 = self.sim.get_gravity() * -1 + self.sim.set_gravity(gravity) + + def cycleScene(self, change_scene: bool, shift_pressed: bool): + if change_scene: + # cycle the active scene from the set available in MetadataMediator + inc = -1 if shift_pressed else 1 + scene_ids = self.sim.metadata_mediator.get_scene_handles() + cur_scene_index = 0 + if self.sim_settings["scene"] not in scene_ids: + matching_scenes = [ + (ix, x) + for ix, x in enumerate(scene_ids) + if self.sim_settings["scene"] in x + ] + if not matching_scenes: + logger.warning( + f"The current scene, '{self.sim_settings['scene']}', is not in the list, starting cycle at index 0." + ) + else: + cur_scene_index = matching_scenes[0][0] + else: + cur_scene_index = scene_ids.index(self.sim_settings["scene"]) + + next_scene_index = min(max(cur_scene_index + inc, 0), len(scene_ids) - 1) + self.sim_settings["scene"] = scene_ids[next_scene_index] + self.reconfigure_sim() + logger.info(f"Reconfigured simulator for scene: {self.sim_settings['scene']}") + + def check_rec_accessibility( + self, rec: hab_receptacle.Receptacle, max_height: float = 1.2, clean_up=True + ) -> Tuple[bool, str]: + """ + Use unoccluded navmesh snap to check whether a Receptacle is accessible. + """ + print(f"Checking Receptacle accessibility for {rec.unique_name}") + + # first check if the receptacle is close enough to the navmesh + rec_global_keypoints = hsim_physics.get_global_keypoints_from_bb( + rec.bounds, rec.get_global_transform(self.sim) + ) + floor_point = None + for keypoint in rec_global_keypoints: + floor_point = self.sim.pathfinder.snap_point( + keypoint, island_index=self.largest_island_ix + ) + if not np.isnan(floor_point[0]): + break + if np.isnan(floor_point[0]): + print(" - Receptacle too far from active navmesh boundary.") + return False, "access_filtered" + + # then check that the height is acceptable + rec_min = min(rec_global_keypoints, key=lambda x: x[1]) + if rec_min[1] - floor_point[1] > max_height: + print( + f" - Receptacle exceeds maximum height {rec_min[1]-floor_point[1]} vs {max_height}." + ) + return False, "height_filtered" + + # try to sample 10 objects on the receptacle + target_number = 10 + obj_samp = ObjectSampler( + self.clutter_object_handles, + ["rec set"], + orientation_sample="up", + num_objects=(1, target_number), + ) + obj_samp.max_sample_attempts = len(self.clutter_object_handles) + obj_samp.max_placement_attempts = 10 + obj_samp.target_objects_number = target_number + rec_set_unique_names = [rec.unique_name] + rec_set_obj = hab_receptacle.ReceptacleSet( + "rec set", [""], [], rec_set_unique_names, [] + ) + recep_tracker = hab_receptacle.ReceptacleTracker( + {}, + {"rec set": rec_set_obj}, + ) + new_objs = obj_samp.sample(self.sim, recep_tracker, [], snap_down=True) + + # if we can't sample objects, this receptacle is out + if len(new_objs) == 0: + print(" - failed to sample any objects.") + return False, "access_filtered" + print(f" - sampled {len(new_objs)} / {target_number} objects.") + + for obj, _rec in new_objs: + self.clutter_object_instances.append(obj) + self.clutter_object_initial_states.append((obj.translation, obj.rotation)) + + # now try unoccluded navmesh snapping to the objects to test accessibility + obj_positions = [obj.translation for obj, _ in new_objs] + for obj, _ in new_objs: + obj.translation += mn.Vector3(100, 0, 0) + failure_count = 0 + + for o_ix, (obj, _) in enumerate(new_objs): + obj.translation = obj_positions[o_ix] + snap_point = unoccluded_navmesh_snap( + obj.translation, + 1.3, + self.sim.pathfinder, + self.sim, + obj.object_id, + self.largest_island_ix, + ) + # self.dbv.look_at(look_at=obj.translation, look_from=snap_point) + # self.dbv.get_observation().show() + if snap_point is None: + failure_count += 1 + obj.translation += mn.Vector3(100, 0, 0) + for o_ix, (obj, _) in enumerate(new_objs): + obj.translation = obj_positions[o_ix] + failure_rate = (float(failure_count) / len(new_objs)) * 100 + print(f" - failure_rate = {failure_rate}") + print( + f" - accessibility rate = {len(new_objs)-failure_count}|{len(new_objs)} ({100-failure_rate}%)" + ) + + accessible = failure_rate < 20 # 80% accessibility required + + if clean_up: + # removing all clutter objects currently + rom = self.sim.get_rigid_object_manager() + print(f"Removing {len(self.clutter_object_instances)} clutter objects.") + for obj in self.clutter_object_instances: + rom.remove_object_by_handle(obj.handle) + self.clutter_object_initial_states.clear() + self.clutter_object_instances.clear() + + if not accessible: + return False, "access_filtered" + + return True, "active" + + def set_filter_status_for_rec( + self, rec: hab_receptacle.Receptacle, filter_status: str + ) -> None: + filter_types = [ + "access_filtered", + "stability_filtered", + "height_filtered", + "manually_filtered", + "active", + ] + assert filter_status in filter_types + filtered_rec_name = rec.unique_name + for filter_type in filter_types: + if filtered_rec_name in self.rec_filter_data[filter_type]: + self.rec_filter_data[filter_type].remove(filtered_rec_name) + self.rec_filter_data[filter_status].append(filtered_rec_name) + + def add_objects_to_receptacles(self, alt_pressed: bool, shift_pressed: bool): + rom = self.sim.get_rigid_object_manager() + # add objects to the selected receptacle or remove all objects + if shift_pressed: + # remove all + print(f"Removing {len(self.clutter_object_instances)} clutter objects.") + for obj in self.clutter_object_instances: + rom.remove_object_by_handle(obj.handle) + self.clutter_object_initial_states.clear() + self.clutter_object_instances.clear() + else: + # try to sample an object from the selected object receptacles + rec_set = None + if alt_pressed: + # use all active filter recs + rec_set = [ + rec + for rec in self.receptacles + if rec.unique_name in self.rec_filter_data["active"] + ] + elif self.selected_rec is not None: + rec_set = [self.selected_rec] + elif len(self.obj_editor.sel_objs) != 0: + rec_set = [] + for obj in self.obj_editor.sel_objs: + tmp_list = [ + rec + for rec in self.receptacles + if obj.handle == rec.parent_object_handle + ] + rec_set.extend(tmp_list) + if rec_set is not None: + if len(self.clutter_object_handles) == 0: + for obj_name in self.clutter_object_set: + matching_handles = self.sim.metadata_mediator.object_template_manager.get_template_handles( + obj_name + ) + assert ( + len(matching_handles) > 0 + ), f"No matching template for '{obj_name}' in the dataset." + self.clutter_object_handles.append(matching_handles[0]) + + rec_set_unique_names = [rec.unique_name for rec in rec_set] + obj_samp = ObjectSampler( + self.clutter_object_handles, + ["rec set"], + orientation_sample="up", + num_objects=(1, 10), + ) + obj_samp.receptacle_instances = self.receptacles + rec_set_obj = hab_receptacle.ReceptacleSet( + "rec set", [""], [], rec_set_unique_names, [] + ) + recep_tracker = hab_receptacle.ReceptacleTracker( + {}, + {"rec set": rec_set_obj}, + ) + new_objs = obj_samp.sample(self.sim, recep_tracker, [], snap_down=True) + for obj, rec in new_objs: + self.clutter_object_instances.append(obj) + self.clutter_object_initial_states.append( + (obj.translation, obj.rotation) + ) + print(f"Sampled '{obj.handle}' in '{rec.unique_name}'") + else: + print("No object selected, cannot sample clutter.") + + def key_press_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key press by performing the corresponding functions. + If the key pressed is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the + key will be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + pressed = Application.Key + mod = Application.Modifier + + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + # warning: ctrl doesn't always pass through with other key-presses + + if key == pressed.ESC: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + elif key == pressed.ONE: + # save scene instance + self.obj_editor.save_current_scene() + print("Saved modified scene instance JSON to original location.") + elif key == pressed.TWO: + # Undo any edits + self.obj_editor.undo_edit() + + elif key == pressed.SIX: + # Reset mouse wheel FOV zoom + self.render_camera.reset_zoom() + + elif key == pressed.TAB: + # Cycle through scenes + self.cycleScene(True, shift_pressed=shift_pressed) + + elif key == pressed.SPACE: + if not self.sim.config.sim_cfg.enable_physics: + logger.warn("Warning: physics was not enabled during setup") + else: + self.simulating = not self.simulating + logger.info(f"Command: physics simulating set to {self.simulating}") + + elif key == pressed.PERIOD: + if self.simulating: + logger.warn("Warning: physics simulation already running") + else: + self.simulate_single_step = True + logger.info("Command: physics step taken") + + elif key == pressed.COMMA: + self.debug_bullet_draw = not self.debug_bullet_draw + logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") + + elif key == pressed.B: + # Change editor values + self.obj_editor.change_edit_vals(toggle=shift_pressed) + + elif key == pressed.C: + self.contact_debug_draw = not self.contact_debug_draw + log_str = f"Command: toggle contact debug draw: {self.contact_debug_draw}" + if self.contact_debug_draw: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + # TODO: add a nice log message with concise contact pair naming. + log_str = f"{log_str}: performing discrete collision detection and visualize active contacts." + self.sim.perform_discrete_collision_detection() + logger.info(log_str) + + elif key == pressed.E: + # Cyle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + + elif key == pressed.F: + # toggle, load(+ALT), or save(+SHIFT) filtering + if shift_pressed and self.rec_filter_data is not None: + self.export_filtered_recs(self.rec_filter_path) + elif alt_pressed: + self.load_filtered_recs(self.rec_filter_path) + else: + self.show_filtered = not self.show_filtered + print(f"self.show_filtered = {self.show_filtered}") + + elif key == pressed.G: + # Change editor mode + self.obj_editor.change_edit_mode(toggle=shift_pressed) + + elif key == pressed.H: + self.print_help_text() + + elif key == pressed.I: + self.navmesh_dirty = self.obj_editor.edit_up( + self.navmesh_dirty, toggle=shift_pressed + ) + + elif key == pressed.J: + self.navmesh_dirty = self.obj_editor.edit_left(self.navmesh_dirty) + + elif key == pressed.K: + self.navmesh_dirty = self.obj_editor.edit_down( + self.navmesh_dirty, toggle=shift_pressed + ) + + elif key == pressed.L: + self.navmesh_dirty = self.obj_editor.edit_right(self.navmesh_dirty) + + elif key == pressed.M: + if shift_pressed: + # Save all markersets that have been changed + self.markersets_util.save_all_dirty_markersets() + else: + self.cycle_mouse_mode() + logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + + elif key == pressed.N: + # (default) - toggle navmesh visualization + # NOTE: (+ALT) - re-sample the agent position on the NavMesh + # NOTE: (+SHIFT) - re-compute the NavMesh + if alt_pressed: + logger.info("Command: resample agent state from navmesh") + if self.sim.pathfinder.is_loaded: + new_agent_state = habitat_sim.AgentState() + + print(f"Largest indoor island index = {self.largest_island_ix}") + new_agent_state.position = ( + self.sim.pathfinder.get_random_navigable_point( + island_index=self.largest_island_ix + ) + ) + new_agent_state.rotation = quat_from_angle_axis( + self.sim.random.uniform_float(0, 2.0 * np.pi), + np.array([0, 1, 0]), + ) + self.default_agent.set_state(new_agent_state) + else: + logger.warning( + "NavMesh is not initialized. Cannot sample new agent state." + ) + elif shift_pressed: + logger.info("Command: recompute navmesh") + self.navmesh_config_and_recompute() + else: + if self.sim.pathfinder.is_loaded: + self.sim.navmesh_visualization = not self.sim.navmesh_visualization + logger.info("Command: toggle navmesh") + else: + logger.warn("Warning: recompute navmesh first") + + elif key == pressed.O: + if shift_pressed: + # move non-proxy objects in/out of visible space + self.original_objs_visible = not self.original_objs_visible + print(f"self.original_objs_visible = {self.original_objs_visible}") + if not self.original_objs_visible: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: + if self.col_proxy_objs is None: + self.col_proxy_objs = [] + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + # add a new proxy object + self.col_proxy_objs.append(self.add_col_proxy_object(obj)) + else: + self.col_proxies_visible = not self.col_proxies_visible + print(f"self.col_proxies_visible = {self.col_proxies_visible}") + + # make the proxies visible or not by moving them + if not self.col_proxies_visible: + for obj in self.col_proxy_objs: + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + else: + for obj in self.col_proxy_objs: + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + + elif key == pressed.P: + # If shift pressed then open, otherwise close + # If alt pressed then selected, otherwise all + self.obj_editor.set_ao_joint_states( + do_open=shift_pressed, selected=alt_pressed + ) + if not shift_pressed: + # if closing then redo navmesh + self.navmesh_config_and_recompute() + + elif key == pressed.Q: + self.add_objects_to_receptacles( + alt_pressed=alt_pressed, shift_pressed=shift_pressed + ) + + elif key == pressed.R: + # Reload current scene + self.cycleScene(False, shift_pressed=shift_pressed) + + elif key == pressed.T: + if shift_pressed: + # open all the AO default links + aos = hsim_physics.get_all_ao_objects(self.sim) + for ao in aos: + default_link = hsim_physics.get_ao_default_link(ao, True) + hsim_physics.open_link(ao, default_link) + # compute and set the receptacle filters + for rix, rec in enumerate(self.receptacles): + rec_accessible, filter_type = self.check_rec_accessibility(rec) + self.set_filter_status_for_rec(rec, filter_type) + print(f"-- progress = {rix}/{len(self.receptacles)} --") + else: + if self.selected_rec is not None: + rec_accessible, filter_type = self.check_rec_accessibility( + self.selected_rec, clean_up=False + ) + self.set_filter_status_for_rec(self.selected_rec, filter_type) + else: + print("No selected receptacle, can't test accessibility.") + # self.modify_param_from_term() + + # load URDF + # fixed_base = alt_pressed + # urdf_file_path = "" + # if shift_pressed and self.cached_urdf: + # urdf_file_path = self.cached_urdf + # else: + # urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() + # if not urdf_file_path: + # logger.warn("Load URDF: no input provided. Aborting.") + # elif not urdf_file_path.endswith((".URDF", ".urdf")): + # logger.warn("Load URDF: input is not a URDF. Aborting.") + # elif os.path.exists(urdf_file_path): + # self.cached_urdf = urdf_file_path + # aom = self.sim.get_articulated_object_manager() + # ao = aom.add_articulated_object_from_urdf( + # urdf_file_path, + # fixed_base, + # 1.0, + # 1.0, + # True, + # maintain_link_order=False, + # intertia_from_urdf=False, + # ) + # ao.translation = ( + # self.default_agent.scene_node.transformation.transform_point( + # [0.0, 1.0, -1.5] + # ) + # ) + # # check removal and auto-creation + # joint_motor_settings = habitat_sim.physics.JointMotorSettings( + # position_target=0.0, + # position_gain=1.0, + # velocity_target=0.0, + # velocity_gain=1.0, + # max_impulse=1000.0, + # ) + # existing_motor_ids = ao.existing_joint_motor_ids + # for motor_id in existing_motor_ids: + # ao.remove_joint_motor(motor_id) + # ao.create_all_motors(joint_motor_settings) + # else: + # logger.warn("Load URDF: input file not found. Aborting.") + + elif key == pressed.U: + # Remove object + # 'Remove' all selected objects by moving them out of view. + # Removal only becomes permanent when scene is saved + self.obj_editor.remove_sel_objects() + + self.navmesh_config_and_recompute() + + elif key == pressed.V: + # load receptacles and toggle visibilty or color mode (+SHIFT) + if self.receptacles is None: + self.load_receptacles() + + if shift_pressed: + self.rec_color_mode = RecColorMode( + (self.rec_color_mode.value + 1) % len(RecColorMode) + ) + print(f"self.rec_color_mode = {self.rec_color_mode}") + self.display_receptacles = True + else: + self.display_receptacles = not self.display_receptacles + print(f"self.display_receptacles = {self.display_receptacles}") + + elif key == pressed.Y and self.selected_rec is not None: + if self.selected_rec.unique_name in self.rec_filter_data["cook_surface"]: + print(self.selected_rec.unique_name + " removed from 'cook_surface'") + self.rec_filter_data["cook_surface"].remove( + self.selected_rec.unique_name + ) + self.selected_rec = None + else: + print(self.selected_rec.unique_name + " added to 'cook_surface'") + self.rec_filter_data["cook_surface"].append( + self.selected_rec.unique_name + ) + self.selected_rec = None + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = True + event.accepted = True + self.redraw() + + def key_release_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key release. When a key is released, if it + is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the key will + be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = False + event.accepted = True + self.redraw() + + def calc_mouse_cast_results(self, screen_location: mn.Vector3) -> None: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(screen_location)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + self.mouse_cast_has_hits = ( + mouse_cast_results is not None and mouse_cast_results.has_hits() + ) + self.mouse_cast_results = mouse_cast_results + + def mouse_look_handler( + self, is_right_btn: bool, shift_pressed: bool, alt_pressed: bool + ): + if is_right_btn: + sel_obj = None + self.selected_rec = None + hit_info = self.mouse_cast_results.hits[0] + hit_id = hit_info.object_id + # right click in look mode to print object information + if hit_id == habitat_sim.stage_id: + print("This is the stage.") + else: + obj = hsim_physics.get_obj_from_id(self.sim, hit_id) + link_id = None + if obj.object_id != hit_id: + # this is a link + link_id = obj.link_object_ids[hit_id] + sel_obj = obj + print(f"Object: {obj.handle}") + if obj.is_articulated: + print("links = ") + for obj_id, link_id in obj.link_object_ids.items(): + print(f" {link_id} : {obj_id} : {obj.get_link_name(link_id)}") + if hit_id == obj_id: + print(" !^!") + if self.receptacles is not None: + for rec in self.receptacles: + if rec.parent_object_handle == obj.handle: + print(f" - Receptacle: {rec.name}") + if shift_pressed: + if obj.handle not in self.poh_to_rec: + new_rec = hab_receptacle.AnyObjectReceptacle( + obj.handle + "_aor", + parent_object_handle=obj.handle, + parent_link=link_id, + ) + self.receptacles.append(new_rec) + self.poh_to_rec[obj.handle] = [new_rec] + self.rec_to_poh[new_rec] = obj.creation_attributes.handle + self.selected_rec = self.get_closest_tri_receptacle(hit_info.point) + if self.selected_rec is not None: + print(f"Selected Receptacle: {self.selected_rec.name}") + elif alt_pressed: + filtered_rec = self.get_closest_tri_receptacle(hit_info.point) + if filtered_rec is not None: + filtered_rec_name = filtered_rec.unique_name + print(f"Modified Receptacle Filter State: {filtered_rec_name}") + if ( + filtered_rec_name + in self.rec_filter_data["manually_filtered"] + ): + print(" remove from manual filter") + # this was manually filtered, remove it and try to make active + self.rec_filter_data["manually_filtered"].remove( + filtered_rec_name + ) + add_to_active = True + for other_out_set in [ + "access_filtered", + "stability_filtered", + "height_filtered", + ]: + if ( + filtered_rec_name + in self.rec_filter_data[other_out_set] + ): + print(f" is in {other_out_set}") + add_to_active = False + break + if add_to_active: + print(" is active") + self.rec_filter_data["active"].append(filtered_rec_name) + elif filtered_rec_name in self.rec_filter_data["active"]: + print(" remove from active, add manual filter") + # this was active, remove it and mark manually filtered + self.rec_filter_data["active"].remove(filtered_rec_name) + self.rec_filter_data["manually_filtered"].append( + filtered_rec_name + ) + else: + print(" add to manual filter, but has other filter") + # this is already filtered, but add it to manual filters + self.rec_filter_data["manually_filtered"].append( + filtered_rec_name + ) + elif obj.is_articulated: + # get the default link + default_link = hsim_physics.get_ao_default_link(obj, True) + if default_link is None: + print("Selected AO has no default link.") + else: + if hsim_physics.link_is_open(obj, default_link, 0.05): + hsim_physics.close_link(obj, default_link) + else: + hsim_physics.open_link(obj, default_link) + + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(sel_obj) + + def mouse_grab_handler(self, is_right_btn: bool): + ao_link = -1 + hit_info = self.mouse_cast_results.hits[0] + + if hit_info.object_id > habitat_sim.stage_id: + obj = hsim_physics.get_obj_from_id( + self.sim, hit_info.object_id, self.ao_link_map + ) + + if obj is None: + raise AssertionError( + "hit object_id is not valid. Did not find object or link." + ) + + if obj.object_id == hit_info.object_id: + # ro or ao base link + print("RO or AO Base link grabbed") + object_pivot = obj.transformation.inverted().transform_point( + hit_info.point + ) + object_frame = obj.rotation.inverted() + elif obj.is_articulated: + print( + f"AO non-base link hit id `{hit_info.object_id}` on AO obj id : `{obj.object_id}`" + ) + # ao non-base link - hit info object id is to link + ao_link = obj.link_object_ids[hit_info.object_id] + object_pivot = ( + obj.get_link_scene_node(ao_link) + .transformation.inverted() + .transform_point(hit_info.point) + ) + object_frame = obj.get_link_scene_node(ao_link).rotation.inverted() + + print(f"Grabbed object `{obj.handle}` | hit id `{hit_info.object_id}`") + if ao_link >= 0: + print(f" link id {ao_link}") + + # setup the grabbing constraints + node = self.default_agent.scene_node + constraint_settings = physics.RigidConstraintSettings() + + constraint_settings.object_id_a = obj.object_id + constraint_settings.link_id_a = ao_link + constraint_settings.pivot_a = object_pivot + constraint_settings.frame_a = ( + object_frame.to_matrix() @ node.rotation.to_matrix() + ) + constraint_settings.frame_b = node.rotation.to_matrix() + constraint_settings.pivot_b = hit_info.point + + # by default use a point 2 point constraint + if is_right_btn: + constraint_settings.constraint_type = physics.RigidConstraintType.Fixed + + grip_depth = ( + hit_info.point + - self.render_camera.render_camera.node.absolute_translation + ).length() + + self.mouse_grabber = MouseGrabber( + constraint_settings, + grip_depth, + self.sim, + ) + + def is_left_mse_btn( + self, event: Union[Application.PointerEvent, Application.PointerMoveEvent] + ) -> bool: + """ + Returns whether the left mouse button is pressed + """ + if isinstance(event, Application.PointerEvent): + return event.pointer == Application.Pointer.MOUSE_LEFT + elif isinstance(event, Application.PointerMoveEvent): + return event.pointers & Application.Pointer.MOUSE_LEFT + else: + return False + + def is_right_mse_btn( + self, event: Union[Application.PointerEvent, Application.PointerMoveEvent] + ) -> bool: + """ + Returns whether the right mouse button is pressed + """ + if isinstance(event, Application.PointerEvent): + return event.pointer == Application.Pointer.MOUSE_RIGHT + elif isinstance(event, Application.PointerMoveEvent): + return event.pointers & Application.Pointer.MOUSE_RIGHT + else: + return False + + def pointer_move_event(self, event: Application.PointerMoveEvent) -> None: + """ + Handles `Application.PointerMoveEvent`. When in LOOK mode, enables the left + mouse button to steer the agent's facing direction. When in GRAB mode, + continues to update the grabber's object position with our agents position. + """ + + # if interactive mode -> LOOK MODE + if ( + bool(event.pointers & Application.Pointer.MOUSE_LEFT) + and self.mouse_interaction == MouseMode.LOOK + ): + agent = self.sim.agents[self.agent_id] + delta = self.get_mouse_position(event.relative_position) / 2 + action = habitat_sim.agent.ObjectControls() + act_spec = habitat_sim.agent.ActuationSpec + + # left/right on agent scene node + action(agent.scene_node, "turn_right", act_spec(delta.x)) + + # up/down on cameras' scene nodes + action = habitat_sim.agent.ObjectControls() + sensors = list(self.default_agent.scene_node.subtree_sensors.values()) + [action(s.object, "look_down", act_spec(delta.y), False) for s in sensors] + + # if interactive mode is TRUE -> GRAB MODE + elif self.mouse_interaction == MouseMode.GRAB and self.mouse_grabber: + # update location of grabbed object + self.update_grab_position(self.get_mouse_position(event.position)) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def pointer_press_event(self, event: Application.PointerEvent) -> None: + """ + Handles `Application.PointerEvent`. When in GRAB mode, click on + objects to drag their position. (right-click for fixed constraints) + """ + physics_enabled = self.sim.get_physics_simulation_library() + is_left_mse_btn = bool(event.pointer == Application.Pointer.MOUSE_LEFT) + is_right_mse_btn = bool(event.pointer == Application.Pointer.MOUSE_RIGHT) + mod = Application.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + self.calc_mouse_cast_results(event.position) + + # if interactive mode is True -> GRAB MODE + if physics_enabled and self.mouse_cast_has_hits: + if self.mouse_interaction == MouseMode.GRAB: + self.mouse_grab_handler(is_right_mse_btn) + elif self.mouse_interaction == MouseMode.LOOK: + self.mouse_look_handler( + is_right_mse_btn, + shift_pressed=shift_pressed, + alt_pressed=alt_pressed, + ) + + elif self.mouse_interaction == MouseMode.MARKER: + # hit_info = self.mouse_cast_results.hits[0] + sel_obj = self.markersets_util.place_marker_at_hit_location( + self.mouse_cast_results.hits[0], + self.ao_link_map, + is_left_mse_btn, + ) + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(sel_obj) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def scroll_event(self, event: Application.ScrollEvent) -> None: + """ + Handles `Application.ScrollEvent`. When in LOOK mode, enables camera + zooming (fine-grained zoom using shift) When in GRAB mode, adjusts the depth + of the grabber's object. (larger depth change rate using shift) + """ + scroll_mod_val = ( + event.offset.y + if abs(event.offset.y) > abs(event.offset.x) + else event.offset.x + ) + if not scroll_mod_val: + return + + # use shift to scale action response + shift_pressed = bool(event.modifiers & Application.Modifier.SHIFT) + alt_pressed = bool(event.modifiers & Application.Modifier.ALT) + ctrl_pressed = bool(event.modifiers & Application.Modifier.CTRL) + + # if interactive mode is False -> LOOK MODE + if self.mouse_interaction == MouseMode.LOOK: + # use shift for fine-grained zooming + # TODO : need to support camera handling like done for spot here. See spot_viewer.py + # if alt_pressed: + # # move camera in/out + # mod_val = 0.3 if shift_pressed else 0.15 + # scroll_delta = scroll_mod_val * mod_val + # self.camera_distance -= scroll_delta + # else: + mod_val = 1.01 if shift_pressed else 1.1 + mod = mod_val if scroll_mod_val > 0 else 1.0 / mod_val + + cam = self.render_camera + cam.zoom(mod) + # self.redraw() + + elif self.mouse_interaction == MouseMode.GRAB and self.mouse_grabber: + # adjust the depth + mod_val = 0.1 if shift_pressed else 0.01 + scroll_delta = scroll_mod_val * mod_val + if alt_pressed or ctrl_pressed: + # rotate the object's local constraint frame + agent_t = self.default_agent.scene_node.transformation_matrix() + # ALT - yaw + rotation_axis = agent_t.transform_vector(mn.Vector3(0, 1, 0)) + if alt_pressed and ctrl_pressed: + # ALT+CTRL - roll + rotation_axis = agent_t.transform_vector(mn.Vector3(0, 0, -1)) + elif ctrl_pressed: + # CTRL - pitch + rotation_axis = agent_t.transform_vector(mn.Vector3(1, 0, 0)) + self.mouse_grabber.rotate_local_frame_by_global_angle_axis( + rotation_axis, mn.Rad(scroll_delta) + ) + else: + # update location of grabbed object + self.mouse_grabber.grip_depth += scroll_delta + self.update_grab_position(self.get_mouse_position(event.position)) + elif self.mouse_interaction == MouseMode.MARKER: + self.markersets_util.cycle_current_taskname(scroll_mod_val > 0) + # marker_mod = 1 if scroll_mod_val > 0 else -1 + # self.current_markerset_taskset_idx = ( + # self.current_markerset_taskset_idx + # + len(self.markerset_taskset_names) + # + marker_mod + # ) % len(self.markerset_taskset_names) + + self.redraw() + event.accepted = True + + def pointer_release_event(self, event: Application.PointerEvent) -> None: + """ + Release any existing constraints. + """ + del self.mouse_grabber + self.mouse_grabber = None + event.accepted = True + + def update_grab_position(self, point: mn.Vector2i) -> None: + """ + Accepts a point derived from a mouse click event and updates the + transform of the mouse grabber. + """ + # check mouse grabber + if not self.mouse_grabber: + return + + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(point) + + rotation: mn.Matrix3x3 = self.default_agent.scene_node.rotation.to_matrix() + translation: mn.Vector3 = ( + render_camera.node.absolute_translation + + ray.direction * self.mouse_grabber.grip_depth + ) + self.mouse_grabber.update_transform(mn.Matrix4.from_(rotation, translation)) + + def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> mn.Vector2i: + """ + This function will get a screen-space mouse position appropriately + scaled based on framebuffer size and window size. Generally these would be + the same value, but on certain HiDPI displays (Retina displays) they may be + different. + """ + scaling = mn.Vector2i(self.framebuffer_size) / mn.Vector2i(self.window_size) + return mouse_event_position * scaling + + def cycle_mouse_mode(self) -> None: + """ + This method defines how to cycle through the mouse mode. + """ + if self.mouse_interaction == MouseMode.LOOK: + self.mouse_interaction = MouseMode.GRAB + elif self.mouse_interaction == MouseMode.GRAB: + self.mouse_interaction = MouseMode.MARKER + elif self.mouse_interaction == MouseMode.MARKER: + self.mouse_interaction = MouseMode.LOOK + + def navmesh_config_and_recompute(self) -> None: + """ + This method is setup to be overridden in for setting config accessibility + in inherited classes. + """ + self.navmesh_settings = habitat_sim.NavMeshSettings() + self.navmesh_settings.set_defaults() + self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height + self.navmesh_settings.agent_radius = self.cfg.agents[self.agent_id].radius + self.navmesh_settings.include_static_objects = True + + # first cache AO motion types and set to STATIC for navmesh + ao_motion_types = [] + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + # ignore the robot + if "hab_spot" not in ao.handle: + ao_motion_types.append((ao, ao.motion_type)) + ao.motion_type = habitat_sim.physics.MotionType.STATIC + + self.sim.recompute_navmesh( + self.sim.pathfinder, + self.navmesh_settings, + ) + + # reset AO motion types from cache + for ao, ao_orig_motion_type in ao_motion_types: + ao.motion_type = ao_orig_motion_type + + self.largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + + self.navmesh_dirty = False + + def exit_event(self, event: Application.ExitEvent): + """ + Overrides exit_event to properly close the Simulator before exiting the + application. + """ + for i in range(self.num_env): + self.tiled_sims[i].close(destroy=True) + event.accepted = True + exit(0) + + def draw_text(self, sensor_spec): + # make magnum text background transparent for text + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + + self.shader.bind_vector_texture(self.glyph_cache.texture) + self.shader.transformation_projection_matrix = self.window_text_transform + self.shader.color = [1.0, 1.0, 1.0] + + sensor_type_string = str(sensor_spec.sensor_type.name) + sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + if self.mouse_interaction == MouseMode.LOOK: + mouse_mode_string = "LOOK" + elif self.mouse_interaction == MouseMode.GRAB: + mouse_mode_string = "GRAB" + elif self.mouse_interaction == MouseMode.MARKER: + mouse_mode_string = "MARKER" + + edit_string = self.obj_editor.edit_disp_str() + current_regions = self.sim.semantic_scene.get_regions_for_point( + self.render_camera.node.absolute_translation + ) + region_name = ( + "--" + if len(current_regions) == 0 + else self.sim.semantic_scene.regions[current_regions[0]].id + ) + window_text = f""" +{self.fps} FPS +Scene ID : {os.path.split(self.cfg.sim_cfg.scene_id)[1].split('.scene_instance')[0]} +Sensor Type: {sensor_type_string} +Sensor Subtype: {sensor_subtype_string} +Mouse Interaction Mode: {mouse_mode_string} +{edit_string} +Selected MarkerSets TaskSet name : {self.markersets_util.get_current_taskname()} +Unstable Objects: {self.num_unstable_objects} of {len(self.clutter_object_instances)} +Current Region: {region_name} + """ + self.window_text.render(window_text) + self.shader.draw(self.window_text.mesh) + + # Disable blending for text + mn.gl.Renderer.disable(mn.gl.Renderer.Feature.BLENDING) + + def print_help_text(self) -> None: + """ + Print the Key Command help text. + """ + logger.info( + """ +===================================================== +Welcome to the Habitat-sim Python Viewer application! +===================================================== +Mouse Functions ('m' to toggle mode): +---------------- +In LOOK mode (default): + LEFT: + Click and drag to rotate the agent and look up/down. + WHEEL: + Modify orthographic camera zoom/perspective camera FOV (+SHIFT for fine grained control) + RIGHT: + Click an object to select the object. Prints object name and attached receptacle names. Selected object displays sample points when cpo is initialized. + (+SHIFT) select a receptacle. + (+ALT) add or remove a receptacle from the "manual filter set". + +In GRAB mode (with 'enable-physics'): + LEFT: + Click and drag to pickup and move an object with a point-to-point constraint (e.g. ball joint). + RIGHT: + Click and drag to pickup and move an object with a fixed frame constraint. + WHEEL (with picked object): + default - Pull gripped object closer or push it away. + (+ALT) rotate object fixed constraint frame (yaw) + (+CTRL) rotate object fixed constraint frame (pitch) + (+ALT+CTRL) rotate object fixed constraint frame (roll) + (+SHIFT) amplify scroll magnitude +In MARKER mode : + + +Edit Commands : +--------------- + 'g' : Change Edit mode to either Move or Rotate the selected object + 'b' (+ SHIFT) : Increment (Decrement) the current edit amounts. + - With an object selected: + When Edit Mode: Move Object mode is selected : + - 'j'/'l' : move the object along global X axis. + - 'i'/'k' : move the object along global Z axis. + (+SHIFT): move the object up/down (global Y axis) + When Edit Mode: Rotate Object mode is selected : + - 'j'/'l' : rotate the object around global Y axis. + - 'i'/'k' : arrow keys: rotate the object around global Z axis. + (+SHIFT): rotate the object around global X axis. + - 'u': delete the selected object + +Key Commands: +------------- + esc: Exit the application. + 'h': Display this help message. + 'm': Cycle mouse interaction modes. + + Agent Controls: + 'wasd': Move the agent's body forward/backward and left/right. + 'zx': Move the agent's body up/down. + arrow keys: Turn the agent's body left/right and camera look up/down. + + Utilities: + '1 (one)': Save current scene instance, overwriting existing scene instance. + 'r': Reset the simulator with the most recently loaded scene. + 'n': Show/hide NavMesh wireframe. + (+SHIFT) Recompute NavMesh with default settings. + (+ALT) Re-sample the agent(camera)'s position and orientation from the NavMesh. + ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). + 'c': Toggle the contact point debug render overlay on/off. If toggled to true, + then run a discrete collision detection pass and render a debug wireframe overlay + showing active contact points and normals (yellow=fixed length normals, red=collision distances). + 'p' Modify AO link states : + (+SHIFT) : Open Selected/All AOs + (-SHIFT) : Close Selected/All AOs + (+ALT) : Modify Selected AOs + (-ALT) : Modify All AOs + 'e' Toggle Semantic visualization bounds (currently only Semantic Region annotations) + + Object Interactions: + SPACE: Toggle physics simulation on/off. + '.': Take a single simulation step if not simulating continuously. + + Receptacle Evaluation Tool UI: + 'v': Load all Receptacles for the scene and toggle Receptacle visibility. + (+SHIFT) Iterate through receptacle color modes. + 'f': Toggle Receptacle view filtering. When on, only non-filtered Receptacles are visible. + (+SHIFT) Export current filter metadata to file. + (+ALT) Import filter metadata from file. + 'o': Toggle display of collision proxy shapes for the scene. + (+SHIFT) Toggle display of original render shapes (and Receptacles). + 't': CLI for modifying un-bound viewer parameters during runtime. + 'q': Sample an object placement from the currently selected object or receptacle. + (+SHIFT) Remove all previously sampled objects. + (+ALT) Sample from all "active" unfiltered Receptacles. + +===================================================== +""" + ) + + +class MouseMode(Enum): + LOOK = 0 + GRAB = 1 + MOTION = 2 + MARKER = 3 + + +class MouseGrabber: + """ + Create a MouseGrabber from RigidConstraintSettings to manipulate objects. + """ + + def __init__( + self, + settings: physics.RigidConstraintSettings, + grip_depth: float, + sim: habitat_sim.simulator.Simulator, + ) -> None: + self.settings = settings + self.simulator = sim + + # defines distance of the grip point from the camera for pivot updates + self.grip_depth = grip_depth + self.constraint_id = sim.create_rigid_constraint(settings) + + def __del__(self): + self.remove_constraint() + + def remove_constraint(self) -> None: + """ + Remove a rigid constraint by id. + """ + self.simulator.remove_rigid_constraint(self.constraint_id) + + def updatePivot(self, pos: mn.Vector3) -> None: + self.settings.pivot_b = pos + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + def update_frame(self, frame: mn.Matrix3x3) -> None: + self.settings.frame_b = frame + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + def update_transform(self, transform: mn.Matrix4) -> None: + self.settings.frame_b = transform.rotation() + self.settings.pivot_b = transform.translation + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + def rotate_local_frame_by_global_angle_axis( + self, axis: mn.Vector3, angle: mn.Rad + ) -> None: + """rotate the object's local constraint frame with a global angle axis input.""" + object_transform = mn.Matrix4() + rom = self.simulator.get_rigid_object_manager() + aom = self.simulator.get_articulated_object_manager() + if rom.get_library_has_id(self.settings.object_id_a): + object_transform = rom.get_object_by_id( + self.settings.object_id_a + ).transformation + else: + # must be an ao + object_transform = ( + aom.get_object_by_id(self.settings.object_id_a) + .get_link_scene_node(self.settings.link_id_a) + .transformation + ) + local_axis = object_transform.inverted().transform_vector(axis) + R = mn.Matrix4.rotation(angle, local_axis.normalized()) + self.settings.frame_a = R.rotation().__matmul__(self.settings.frame_a) + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + +class Timer: + """ + Timer class used to keep track of time between buffer swaps + and guide the display frame rate. + """ + + start_time = 0.0 + prev_frame_time = 0.0 + prev_frame_duration = 0.0 + running = False + + @staticmethod + def start() -> None: + """ + Starts timer and resets previous frame time to the start time. + """ + Timer.running = True + Timer.start_time = time.time() + Timer.prev_frame_time = Timer.start_time + Timer.prev_frame_duration = 0.0 + + @staticmethod + def stop() -> None: + """ + Stops timer and erases any previous time data, resetting the timer. + """ + Timer.running = False + Timer.start_time = 0.0 + Timer.prev_frame_time = 0.0 + Timer.prev_frame_duration = 0.0 + + @staticmethod + def next_frame() -> None: + """ + Records previous frame duration and updates the previous frame timestamp + to the current time. If the timer is not currently running, perform nothing. + """ + if not Timer.running: + return + Timer.prev_frame_duration = time.time() - Timer.prev_frame_time + Timer.prev_frame_time = time.time() + + +# def init_cpo_for_scene(sim_settings, mm: habitat_sim.metadata.MetadataMediator): +# """ +# Initialize and run the CPO for all objects in the scene. +# """ +# global _cpo +# global _cpo_threads + +# _cpo = csa.CollisionProxyOptimizer(sim_settings, None, mm) + +# # get object handles from a specific scene +# objects_in_scene = csa.get_objects_in_scene( +# dataset_path=sim_settings["scene_dataset_config_file"], +# scene_handle=sim_settings["scene"], +# mm=_cpo.mm, +# ) +# # get a subset with receptacles defined +# objects_in_scene = [ +# objects_in_scene[i] +# for i in range(len(objects_in_scene)) +# if csa.object_has_receptacles(objects_in_scene[i], mm.object_template_manager) +# ] + +# def run_cpo_for_obj(obj_handle): +# _cpo.setup_obj_gt(obj_handle) +# _cpo.compute_receptacle_stability(obj_handle, use_gt=True) +# _cpo.compute_receptacle_stability(obj_handle) +# _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=True) +# _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=False) + +# # run CPO initialization multi-threaded to unblock viewer initialization and use + +# threads = [] +# for obj_handle in objects_in_scene: +# run_cpo_for_obj(obj_handle) +# # threads.append(threading.Thread(target=run_cpo_for_obj, args=(obj_handle,))) +# for thread in threads: +# thread.start() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + # optional arguments + parser.add_argument( + "--scene", + default="./data/test_assets/scenes/simple_room.glb", + type=str, + help='scene/stage file to load (default: "./data/test_assets/scenes/simple_room.glb")', + ) + parser.add_argument( + "--dataset", + default="default", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "default")', + ) + parser.add_argument( + "--rec-filter-file", + default="./rec_filter_data.json", + type=str, + help='Receptacle filtering metadata (default: "./rec_filter_data.json")', + ) + parser.add_argument( + "--init-cpo", + action="store_true", + help="Initialize and run the CPO for the current scene.", + ) + parser.add_argument( + "--disable-physics", + action="store_true", + help="disable physics simulation (default: False)", + ) + parser.add_argument( + "--use-default-lighting", + action="store_true", + help="Override configured lighting to use default lighting for the stage.", + ) + parser.add_argument( + "--hbao", + action="store_true", + help="Enable horizon-based ambient occlusion, which provides soft shadows in corners and crevices.", + ) + parser.add_argument( + "--enable-batch-renderer", + action="store_true", + help="Enable batch rendering mode. The number of concurrent environments is specified with the num-environments parameter.", + ) + parser.add_argument( + "--num-environments", + default=1, + type=int, + help="Number of concurrent environments to batch render. Note that only the first environment simulates physics and can be controlled.", + ) + parser.add_argument( + "--composite-files", + type=str, + nargs="*", + help="Composite files that the batch renderer will use in-place of simulation assets to improve memory usage and performance. If none is specified, the original scene files will be loaded from disk.", + ) + parser.add_argument( + "--no-navmesh", + default=False, + action="store_true", + help="Don't build navmesh.", + ) + parser.add_argument( + "--width", + default=1080, + type=int, + help="Horizontal resolution of the window.", + ) + parser.add_argument( + "--height", + default=720, + type=int, + help="Vertical resolution of the window.", + ) + + args = parser.parse_args() + + if args.num_environments < 1: + parser.error("num-environments must be a positive non-zero integer.") + if args.width < 1: + parser.error("width must be a positive non-zero integer.") + if args.height < 1: + parser.error("height must be a positive non-zero integer.") + + # Setting up sim_settings + sim_settings: Dict[str, Any] = default_sim_settings + sim_settings["scene"] = args.scene + sim_settings["scene_dataset_config_file"] = args.dataset + sim_settings["enable_physics"] = not args.disable_physics + sim_settings["use_default_lighting"] = args.use_default_lighting + sim_settings["enable_batch_renderer"] = args.enable_batch_renderer + sim_settings["num_environments"] = args.num_environments + sim_settings["composite_files"] = args.composite_files + sim_settings["window_width"] = args.width + sim_settings["window_height"] = args.height + sim_settings["rec_filter_file"] = args.rec_filter_file + sim_settings["enable_hbao"] = args.hbao + sim_settings["viewer_ignore_navmesh"] = args.no_navmesh + + # don't need auto-navmesh + sim_settings["default_agent_navmesh"] = False + + mm = habitat_sim.metadata.MetadataMediator() + mm.active_dataset = sim_settings["scene_dataset_config_file"] + + # initialize the CPO. + # this will be done in parallel to viewer setup via multithreading + # if args.init_cpo: + # init_cpo_for_scene(sim_settings, mm) + + # start the application + HabitatSimInteractiveViewer(sim_settings, mm).exec() diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py new file mode 100644 index 0000000000..b4769d2c3b --- /dev/null +++ b/examples/spot_viewer.py @@ -0,0 +1,1260 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import ctypes +import math +import os +import string +import sys +import time +from typing import Any, Callable, Dict, List, Optional, Tuple, Union + +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + +import magnum as mn +from magnum import shaders, text +from magnum.platform.glfw import Application + +import habitat_sim +from habitat_sim import ReplayRenderer, ReplayRendererConfiguration +from habitat_sim.logging import LoggingContext, logger +from habitat_sim.utils.classes import ObjectEditor, SemanticDisplay +from habitat_sim.utils.namespace import hsim_physics +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# This class is dependent on hab-lab +from habitat_sim.utils.sim_utils import SpotAgent + + +class HabitatSimInteractiveViewer(Application): + # the maximum number of chars displayable in the app window + # using the magnum text module. These chars are used to + # display the CPU/GPU usage data + MAX_DISPLAY_TEXT_CHARS = 512 + + # how much to displace window text relative to the center of the + # app window (e.g if you want the display text in the top left of + # the app window, you will displace the text + # window width * -TEXT_DELTA_FROM_CENTER in the x axis and + # window height * TEXT_DELTA_FROM_CENTER in the y axis, as the text + # position defaults to the middle of the app window) + TEXT_DELTA_FROM_CENTER = 0.49 + + # font size of the magnum in-window display text that displays + # CPU and GPU usage info + DISPLAY_FONT_SIZE = 16.0 + + def __init__(self, sim_settings: Dict[str, Any]) -> None: + self.sim_settings: Dict[str:Any] = sim_settings + + self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] + self.num_env: int = ( + self.sim_settings["num_environments"] if self.enable_batch_renderer else 1 + ) + + # Compute environment camera resolution based on the number of environments to render in the window. + window_size: mn.Vector2 = ( + self.sim_settings["window_width"], + self.sim_settings["window_height"], + ) + + configuration = self.Configuration() + configuration.title = "Habitat Sim Interactive Viewer" + configuration.size = window_size + Application.__init__(self, configuration) + self.fps: float = 60.0 + + # Compute environment camera resolution based on the number of environments to render in the window. + grid_size: mn.Vector2i = ReplayRenderer.environment_grid_size(self.num_env) + camera_resolution: mn.Vector2 = mn.Vector2(self.framebuffer_size) / mn.Vector2( + grid_size + ) + self.sim_settings["width"] = camera_resolution[0] + self.sim_settings["height"] = camera_resolution[1] + + # draw Bullet debug line visualizations (e.g. collision meshes) + self.debug_bullet_draw = False + # draw active contact point debug line visualizations + self.contact_debug_draw = False + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" + + # set up our movement map + key = Application.Key + self.pressed = { + key.UP: False, + key.DOWN: False, + key.LEFT: False, + key.RIGHT: False, + key.A: False, + key.D: False, + key.S: False, + key.W: False, + key.X: False, + key.Z: False, + key.Q: False, + key.E: False, + } + + # Load a TrueTypeFont plugin and open the font file + self.display_font = text.FontManager().load_and_instantiate("TrueTypeFont") + relative_path_to_font = "../data/fonts/ProggyClean.ttf" + self.display_font.open_file( + os.path.join(os.path.dirname(__file__), relative_path_to_font), + 13, + ) + + # Glyphs we need to render everything + self.glyph_cache = text.GlyphCacheGL( + mn.PixelFormat.R8_UNORM, mn.Vector2i(256), mn.Vector2i(1) + ) + self.display_font.fill_glyph_cache( + self.glyph_cache, + string.ascii_lowercase + + string.ascii_uppercase + + string.digits + + ":-_+,.! %µ", + ) + + # magnum text object that displays CPU/GPU usage data in the app window + self.window_text = text.Renderer2D( + self.display_font, + self.glyph_cache, + HabitatSimInteractiveViewer.DISPLAY_FONT_SIZE, + text.Alignment.TOP_LEFT, + ) + self.window_text.reserve(HabitatSimInteractiveViewer.MAX_DISPLAY_TEXT_CHARS) + + # text object transform in window space is Projection matrix times Translation Matrix + # put text in top left of window + self.window_text_transform = mn.Matrix3.projection( + self.framebuffer_size + ) @ mn.Matrix3.translation( + mn.Vector2(self.framebuffer_size) + * mn.Vector2( + -HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + ) + ) + self.shader = shaders.VectorGL2D() + # whether to render window text or not + self.do_draw_text = True + + # make magnum text background transparent + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + mn.gl.Renderer.set_blend_equation( + mn.gl.Renderer.BlendEquation.ADD, mn.gl.Renderer.BlendEquation.ADD + ) + + # variables that track app data and CPU/GPU usage + self.num_frames_to_track = 60 + + self.previous_mouse_point = None + + # toggle physics simulation on/off + self.simulating = True + + # toggle a single simulation step at the next opportunity if not + # simulating continuously. + self.simulate_single_step = False + + # configure our simulator + self.cfg: Optional[habitat_sim.simulator.Configuration] = None + self.sim: Optional[habitat_sim.simulator.Simulator] = None + self.tiled_sims: list[habitat_sim.simulator.Simulator] = None + self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None + self.replay_renderer: Optional[ReplayRenderer] = None + + self.last_hit_details = None + self.removed_clutter: Dict[str, str] = {} + + self.navmesh_dirty = False + self.removed_objects_debug_frames = [] + + # mouse raycast visualization + self.mouse_cast_results = None + self.mouse_cast_has_hits = False + + self.reconfigure_sim() + + # Editing + self.obj_editor = ObjectEditor(self.sim) + + # Semantics + self.dbg_semantics = SemanticDisplay(self.sim) + + # create spot right after reconfigure + self.spot_agent = SpotAgent(self.sim) + # set for spot's radius + self.cfg.agents[self.agent_id].radius = 0.3 + + # compute NavMesh if not already loaded by the scene. + if self.cfg.sim_cfg.scene_id.lower() != "none": + self.navmesh_config_and_recompute() + + self.spot_agent.place_on_navmesh() + + self.time_since_last_simulation = 0.0 + LoggingContext.reinitialize_from_env() + logger.setLevel("INFO") + self.print_help_text() + + def draw_removed_objects_debug_frames(self): + """ + Draw debug frames for all the recently removed objects. + """ + for trans, aabb in self.removed_objects_debug_frames: + dblr = self.sim.get_debug_line_render() + dblr.push_transform(trans) + dblr.draw_box(aabb.min, aabb.max, mn.Color4.red()) + dblr.pop_transform() + + def remove_outdoor_objects(self): + """ + Check all object instance and remove those which are marked outdoors. + """ + self.removed_objects_debug_frames = [] + rom = self.sim.get_rigid_object_manager() + for obj in rom.get_objects_by_handle_substring().values(): + if self.obj_is_outdoor(obj): + self.removed_objects_debug_frames.append( + (obj.transformation, obj.root_scene_node.cumulative_bb) + ) + rom.remove_object_by_id(obj.object_id) + + def obj_is_outdoor(self, obj): + """ + Check if an object is outdoors or not by raycasting upwards. + """ + up = mn.Vector3(0, 1.0, 0) + ray_results = self.sim.cast_ray(habitat_sim.geo.Ray(obj.translation, up)) + if ray_results.has_hits(): + for hit in ray_results.hits: + if hit.object_id == obj.object_id: + continue + return False + + # no hits, so outdoors + return True + + def draw_contact_debug(self, debug_line_render: Any): + """ + This method is called to render a debug line overlay displaying active contact points and normals. + Red lines show the contact distance along the normal and yellow lines show the contact normal at a fixed length. + """ + yellow = mn.Color4.yellow() + red = mn.Color4.red() + cps = self.sim.get_physics_contact_points() + debug_line_render.set_line_width(1.5) + camera_position = self.render_camera.render_camera.node.absolute_translation + # only showing active contacts + active_contacts = (x for x in cps if x.is_active) + for cp in active_contacts: + # red shows the contact distance + debug_line_render.draw_transformed_line( + cp.position_on_b_in_ws, + cp.position_on_b_in_ws + + cp.contact_normal_on_b_in_ws * -cp.contact_distance, + red, + ) + # yellow shows the contact normal at a fixed length for visualization + debug_line_render.draw_transformed_line( + cp.position_on_b_in_ws, + # + cp.contact_normal_on_b_in_ws * cp.contact_distance, + cp.position_on_b_in_ws + cp.contact_normal_on_b_in_ws * 0.1, + yellow, + ) + debug_line_render.draw_circle( + translation=cp.position_on_b_in_ws, + radius=0.005, + color=yellow, + normal=camera_position - cp.position_on_b_in_ws, + ) + + def debug_draw(self): + """ + Additional draw commands to be called during draw_event. + """ + debug_line_render = self.sim.get_debug_line_render() + if self.debug_bullet_draw: + render_cam = self.render_camera.render_camera + proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) + self.sim.physics_debug_draw(proj_mat) + if self.contact_debug_draw: + self.draw_contact_debug(debug_line_render) + # draw semantic information + self.dbg_semantics.draw_region_debug(debug_line_render=debug_line_render) + + if self.last_hit_details is not None: + debug_line_render.draw_circle( + translation=self.last_hit_details.point, + radius=0.02, + normal=self.last_hit_details.normal, + color=mn.Color4.yellow(), + num_segments=12, + ) + # draw selected object frames if any objects are selected and any toggled object settings + self.obj_editor.draw_selected_objects(debug_line_render=debug_line_render) + # draw highlight box around all objects + self.obj_editor.draw_box_around_objs(debug_line_render=debug_line_render) + # mouse raycast circle + if self.mouse_cast_has_hits: + debug_line_render.draw_circle( + translation=self.mouse_cast_results.hits[0].point, + radius=0.005, + color=mn.Color4(mn.Vector3(1.0), 1.0), + normal=self.mouse_cast_results.hits[0].normal, + ) + + self.draw_removed_objects_debug_frames() + + def draw_event( + self, + simulation_call: Optional[Callable] = None, + global_call: Optional[Callable] = None, + active_agent_id_and_sensor_name: Tuple[int, str] = (0, "color_sensor"), + ) -> None: + """ + Calls continuously to re-render frames and swap the two frame buffers + at a fixed rate. + """ + agent_acts_per_sec = self.fps + + mn.gl.default_framebuffer.clear( + mn.gl.FramebufferClear.COLOR | mn.gl.FramebufferClear.DEPTH + ) + + # Agent actions should occur at a fixed rate per second + self.time_since_last_simulation += Timer.prev_frame_duration + num_agent_actions: int = self.time_since_last_simulation * agent_acts_per_sec + self.move_and_look(int(num_agent_actions)) + + # Occasionally a frame will pass quicker than 1/60 seconds + if self.time_since_last_simulation >= 1.0 / self.fps: + if self.simulating or self.simulate_single_step: + self.sim.step_world(1.0 / self.fps) + self.simulate_single_step = False + if simulation_call is not None: + simulation_call() + if global_call is not None: + global_call() + if self.navmesh_dirty: + self.navmesh_config_and_recompute() + self.navmesh_dirty = False + + # reset time_since_last_simulation, accounting for potential overflow + self.time_since_last_simulation = math.fmod( + self.time_since_last_simulation, 1.0 / self.fps + ) + # move agent camera based on input relative to spot + self.spot_agent.set_agent_camera_transform(self.default_agent.scene_node) + + keys = active_agent_id_and_sensor_name + + if self.enable_batch_renderer: + self.render_batch() + else: + self.sim._Simulator__sensors[keys[0]][keys[1]].draw_observation() + agent = self.sim.get_agent(keys[0]) + self.render_camera = agent.scene_node.node_sensor_suite.get(keys[1]) + self.debug_draw() + self.render_camera.render_target.blit_rgba_to_default() + + # draw CPU/GPU usage data and other info to the app window + mn.gl.default_framebuffer.bind() + if self.do_draw_text: + self.draw_text(self.render_camera.specification()) + + self.swap_buffers() + Timer.next_frame() + self.redraw() + + def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: + """ + Set up our own agent and agent controls + """ + make_action_spec = habitat_sim.agent.ActionSpec + make_actuation_spec = habitat_sim.agent.ActuationSpec + MOVE, LOOK = 0.07, 1.5 + + # all of our possible actions' names + action_list = [ + "move_left", + "turn_left", + "move_right", + "turn_right", + "move_backward", + "look_up", + "move_forward", + "look_down", + "move_down", + "move_up", + ] + + action_space: Dict[str, habitat_sim.agent.ActionSpec] = {} + + # build our action space map + for action in action_list: + actuation_spec_amt = MOVE if "move" in action else LOOK + action_spec = make_action_spec( + action, make_actuation_spec(actuation_spec_amt) + ) + action_space[action] = action_spec + + sensor_spec: List[habitat_sim.sensor.SensorSpec] = self.cfg.agents[ + self.agent_id + ].sensor_specifications + + agent_config = habitat_sim.agent.AgentConfiguration( + height=1.5, + radius=0.1, + sensor_specifications=sensor_spec, + action_space=action_space, + body_type="cylinder", + ) + return agent_config + + def reconfigure_sim(self) -> None: + """ + Utilizes the current `self.sim_settings` to configure and set up a new + `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces + the current simulator instance, reloading the most recently loaded scene + """ + # configure our sim_settings but then set the agent to our default + self.cfg = make_cfg(self.sim_settings) + self.agent_id: int = self.sim_settings["default_agent"] + self.cfg.agents[self.agent_id] = self.default_agent_config() + + if self.enable_batch_renderer: + self.cfg.enable_batch_renderer = True + self.cfg.sim_cfg.create_renderer = False + self.cfg.sim_cfg.enable_gfx_replay_save = True + + if self.sim_settings["use_default_lighting"]: + logger.info("Setting default lighting override for scene.") + self.cfg.sim_cfg.override_scene_light_defaults = True + self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY + + if self.sim is None: + self.tiled_sims = [] + for _i in range(self.num_env): + self.tiled_sims.append(habitat_sim.Simulator(self.cfg)) + self.sim = self.tiled_sims[0] + else: # edge case + for i in range(self.num_env): + if ( + self.tiled_sims[i].config.sim_cfg.scene_id + == self.cfg.sim_cfg.scene_id + ): + # we need to force a reset, so change the internal config scene name + self.tiled_sims[i].config.sim_cfg.scene_id = "NONE" + self.tiled_sims[i].reconfigure(self.cfg) + + # #resave scene instance + # self.sim.save_current_scene_config(overwrite=True) + # sys. exit() + + # post reconfigure + self.default_agent = self.sim.get_agent(self.agent_id) + self.render_camera = self.default_agent.scene_node.node_sensor_suite.get( + "color_sensor" + ) + + # set sim_settings scene name as actual loaded scene + self.sim_settings["scene"] = self.sim.curr_scene_name + + # Initialize replay renderer + if self.enable_batch_renderer and self.replay_renderer is None: + self.replay_renderer_cfg = ReplayRendererConfiguration() + self.replay_renderer_cfg.num_environments = self.num_env + self.replay_renderer_cfg.standalone = ( + False # Context is owned by the GLFW window + ) + self.replay_renderer_cfg.sensor_specifications = self.cfg.agents[ + self.agent_id + ].sensor_specifications + self.replay_renderer_cfg.gpu_device_id = self.cfg.sim_cfg.gpu_device_id + self.replay_renderer_cfg.force_separate_semantic_scene_graph = False + self.replay_renderer_cfg.leave_context_with_background_renderer = False + self.replay_renderer = ReplayRenderer.create_batch_replay_renderer( + self.replay_renderer_cfg + ) + # Pre-load composite files + if sim_settings["composite_files"] is not None: + for composite_file in sim_settings["composite_files"]: + self.replay_renderer.preload_file(composite_file) + + # check that clearing joint positions on save won't corrupt the content + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + for joint_val in ao.joint_positions: + assert ( + joint_val == 0 + ), "If this fails, there are non-zero joint positions in the scene_instance or default pose. Export with 'i' will clear these." + + Timer.start() + self.step = -1 + + def render_batch(self): + """ + This method updates the replay manager with the current state of environments and renders them. + """ + for i in range(self.num_env): + # Apply keyframe + keyframe = self.tiled_sims[i].gfx_replay_manager.extract_keyframe() + self.replay_renderer.set_environment_keyframe(i, keyframe) + # Copy sensor transforms + sensor_suite = self.tiled_sims[i]._sensors + for sensor_uuid, sensor in sensor_suite.items(): + transform = sensor._sensor_object.node.absolute_transformation() + self.replay_renderer.set_sensor_transform(i, sensor_uuid, transform) + # Render + self.replay_renderer.render(mn.gl.default_framebuffer) + + def move_and_look(self, repetitions: int) -> None: + """ + This method is called continuously with `self.draw_event` to monitor + any changes in the movement keys map `Dict[KeyEvent.key, Bool]`. + When a key in the map is set to `True` the corresponding action is taken. + """ + # avoids unnecessary updates to grabber's object position + if repetitions == 0: + return + + key = Application.Key + press: Dict[Application.Key.key, bool] = self.pressed + # Set the spot up to move + self.spot_agent.move_spot( + move_fwd=press[key.W], + move_back=press[key.S], + move_up=press[key.Z], + move_down=press[key.X], + slide_left=press[key.Q], + slide_right=press[key.E], + turn_left=press[key.A], + turn_right=press[key.D], + ) + + def save_scene(self, event: Application.KeyEvent, exit_scene: bool): + """ + Save current scene. Exit if shift is pressed + """ + + # Save spot's state and remove it + self.spot_agent.cache_transform_and_remove() + + # Save scene + self.obj_editor.save_current_scene() + + # save clutter + if len(self.removed_clutter) > 0: + with open("removed_clutter.txt", "a") as f: + for obj_name in self.removed_clutter: + f.write(obj_name + "\n") + # clear clutter + self.removed_clutter: Dict[str, str] = {} + # whether to exit scene + if exit_scene: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + # Restore spot at previous location + self.spot_agent.restore_at_previous_loc() + + def invert_gravity(self) -> None: + """ + Sets the gravity vector to the negative of it's previous value. This is + a good method for testing simulation functionality. + """ + gravity: mn.Vector3 = self.sim.get_gravity() * -1 + self.sim.set_gravity(gravity) + + def key_press_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key press by performing the corresponding functions. + If the key pressed is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the + key will be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + pressed = Application.Key + mod = Application.Modifier + + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + # warning: ctrl doesn't always pass through with other key-presses + if key == pressed.ESC: + # If shift_pressed then exit without save + if shift_pressed: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + + # Otherwise, save scene if it has been edited before exiting + self.save_scene(event, exit_scene=True) + return + elif key == pressed.ZERO: + # reset agent camera location + self.spot_agent.init_spot_cam() + + elif key == pressed.ONE: + # Toggle spot's clipping/restriction to navmesh + self.spot_agent.toggle_clip() + + elif key == pressed.TWO: + # Match target object's x dim + self.navmesh_dirty = self.obj_editor.match_x_dim(self.navmesh_dirty) + + elif key == pressed.THREE: + # Match target object's y dim + self.navmesh_dirty = self.obj_editor.match_y_dim(self.navmesh_dirty) + + elif key == pressed.FOUR: + # Match target object's z dim + self.navmesh_dirty = self.obj_editor.match_z_dim(self.navmesh_dirty) + + elif key == pressed.FIVE: + # Match target object's orientation + self.navmesh_dirty = self.obj_editor.match_orientation(self.navmesh_dirty) + + elif key == pressed.SIX: + # Select all items matching selected item. Shift to include all currently selected items + self.obj_editor.select_all_matching_objects(only_matches=not shift_pressed) + + elif key == pressed.H: + # Print help text to console + self.print_help_text() + + elif key == pressed.SPACE: + if not self.sim.config.sim_cfg.enable_physics: + logger.warn("Warning: physics was not enabled during setup") + else: + self.simulating = not self.simulating + logger.info(f"Command: physics simulating set to {self.simulating}") + + elif key == pressed.PERIOD: + if self.simulating: + logger.warn("Warning: physics simulation already running") + else: + self.simulate_single_step = True + logger.info("Command: physics step taken") + + elif key == pressed.COMMA: + self.debug_bullet_draw = not self.debug_bullet_draw + logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") + + elif key == pressed.LEFT: + # Move or rotate selected object(s) left + self.navmesh_dirty = self.obj_editor.edit_left(self.navmesh_dirty) + + elif key == pressed.RIGHT: + # Move or rotate selected object(s) right + self.navmesh_dirty = self.obj_editor.edit_right(self.navmesh_dirty) + + elif key == pressed.UP: + # Move or rotate selected object(s) up + self.navmesh_dirty = self.obj_editor.edit_up( + self.navmesh_dirty, toggle=alt_pressed + ) + + elif key == pressed.DOWN: + # Move or rotate selected object(s) down + self.navmesh_dirty = self.obj_editor.edit_down( + self.navmesh_dirty, toggle=alt_pressed + ) + + elif key == pressed.BACKSPACE or key == pressed.Y: + # 'Remove' all selected objects by moving them out of view. + # Removal only becomes permanent when scene is saved + # If shift pressed, undo removals + if shift_pressed: + restored_obj_handles = self.obj_editor.restore_removed_objects() + if key == pressed.Y: + for handle in restored_obj_handles: + obj_name = handle.split("/")[-1].split("_:")[0] + self.removed_clutter.pop(obj_name, None) + # select restored objects + self.obj_editor.sel_obj_list(restored_obj_handles) + else: + removed_obj_handles = self.obj_editor.remove_sel_objects() + if key == pressed.Y: + for handle in removed_obj_handles: + # Mark removed clutter + obj_name = handle.split("/")[-1].split("_:")[0] + self.removed_clutter[obj_name] = "" + self.navmesh_config_and_recompute() + + elif key == pressed.B: + # Cycle through available edit amount values + self.obj_editor.change_edit_vals(toggle=shift_pressed) + + elif key == pressed.C: + # Display contacts + self.contact_debug_draw = not self.contact_debug_draw + log_str = f"Command: toggle contact debug draw: {self.contact_debug_draw}" + if self.contact_debug_draw: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + # TODO: add a nice log message with concise contact pair naming. + log_str = f"{log_str}: performing discrete collision detection and visualize active contacts." + self.sim.perform_discrete_collision_detection() + logger.info(log_str) + + elif key == pressed.G: + # cycle through edit modes + self.obj_editor.change_edit_mode(toggle=shift_pressed) + + elif key == pressed.I: + # Save scene, exiting if shift has been pressed + self.save_scene(event=event, exit_scene=shift_pressed) + + elif key == pressed.J: + # If shift pressed then open, otherwise close + # If alt pressed then selected, otherwise all + self.obj_editor.set_ao_joint_states( + do_open=shift_pressed, selected=alt_pressed + ) + if not shift_pressed: + # if closing then redo navmesh + self.navmesh_config_and_recompute() + + elif key == pressed.K: + # Cycle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + elif key == pressed.L: + # Cycle through types of objects to draw highlight box around - aos, rigids, both, none + self.obj_editor.change_draw_box_types(toggle=shift_pressed) + + elif key == pressed.N: + # (default) - toggle navmesh visualization + # NOTE: (+ALT) - re-sample the agent position on the NavMesh + # NOTE: (+SHIFT) - re-compute the NavMesh + if alt_pressed: + logger.info("Command: resample agent state from navmesh") + self.spot_agent.place_on_navmesh() + elif shift_pressed: + logger.info("Command: recompute navmesh") + self.navmesh_config_and_recompute() + else: + if self.sim.pathfinder.is_loaded: + self.sim.navmesh_visualization = not self.sim.navmesh_visualization + logger.info("Command: toggle navmesh") + else: + logger.warning("Warning: recompute navmesh first") + elif key == pressed.P: + # Toggle whether showing performance data on screen or not + self.do_draw_text = not self.do_draw_text + + elif key == pressed.T: + self.remove_outdoor_objects() + + elif key == pressed.U: + # Undo all edits on selected objects 1 step, or redo undone, if shift + if shift_pressed: + self.obj_editor.redo_sel_edits() + else: + self.obj_editor.undo_sel_edits() + + elif key == pressed.V: + # Duplicate all the selected objects and place them in the scene + # or inject a new object by queried handle substring in front of + # the agent if no objects selected + + # Use shift to play object at most recent hit location, if it exists + if shift_pressed and self.last_hit_details is not None: + build_loc = self.last_hit_details.point + else: + build_loc = self.spot_agent.get_point_in_front( + disp_in_front=[1.5, 0.0, 0.0] + ) + + new_obj_list, self.navmesh_dirty = self.obj_editor.build_objects( + self.navmesh_dirty, + build_loc=build_loc, + ) + if len(new_obj_list) == 0: + print("Failed to add any new objects.") + else: + print(f"Finished adding {len(new_obj_list)} object(s).") + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = True + event.accepted = True + self.redraw() + + def key_release_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key release. When a key is released, if it + is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the key will + be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = False + event.accepted = True + self.redraw() + + def calc_mouse_cast_results(self, screen_location: mn.Vector3) -> None: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(screen_location)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + self.mouse_cast_has_hits = ( + mouse_cast_results is not None and mouse_cast_results.has_hits() + ) + self.mouse_cast_results = mouse_cast_results + + def is_left_mse_btn( + self, event: Union[Application.PointerEvent, Application.PointerMoveEvent] + ) -> bool: + """ + Returns whether the left mouse button is pressed + """ + if isinstance(event, Application.PointerEvent): + return event.pointer == Application.Pointer.MOUSE_LEFT + elif isinstance(event, Application.PointerMoveEvent): + return event.pointers & Application.Pointer.MOUSE_LEFT + else: + return False + + def is_right_mse_btn( + self, event: Union[Application.PointerEvent, Application.PointerMoveEvent] + ) -> bool: + """ + Returns whether the right mouse button is pressed + """ + if isinstance(event, Application.PointerEvent): + return event.pointer == Application.Pointer.MOUSE_RIGHT + elif isinstance(event, Application.PointerMoveEvent): + return event.pointers & Application.Pointer.MOUSE_RIGHT + else: + return False + + def pointer_move_event(self, event: Application.PointerMoveEvent) -> None: + """ + Handles `Application.PointerMoveEvent`. When in LOOK mode, enables the left + mouse button to steer the agent's facing direction. When in GRAB mode, + continues to update the grabber's object position with our agents position. + """ + + # if interactive mode -> LOOK MODE + if self.is_left_mse_btn(event): + shift_pressed = bool(event.modifiers & Application.Modifier.SHIFT) + alt_pressed = bool(event.modifiers & Application.Modifier.ALT) + self.spot_agent.mod_spot_cam( + mse_rel_pos=event.relative_position, + shift_pressed=shift_pressed, + alt_pressed=alt_pressed, + ) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def pointer_press_event(self, event: Application.PointerEvent) -> None: + """ + Handles `Application.PointerEvent`. When in GRAB mode, click on + objects to drag their position. (right-click for fixed constraints) + """ + physics_enabled = self.sim.get_physics_simulation_library() + # is_left_mse_btn = self.is_left_mse_btn(event) + is_right_mse_btn = self.is_right_mse_btn(event) + mod = Application.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + # alt_pressed = bool(event.modifiers & mod.ALT) + self.calc_mouse_cast_results(event.position) + + # select an object with RIGHT-click + if physics_enabled and self.mouse_cast_has_hits: + mouse_cast_hit_results = self.mouse_cast_results.hits + if is_right_mse_btn: + # Find object being clicked + obj_found = False + obj = None + # find first non-stage object + hit_idx = 0 + while hit_idx < len(mouse_cast_hit_results) and not obj_found: + self.last_hit_details = mouse_cast_hit_results[hit_idx] + hit_obj_id = mouse_cast_hit_results[hit_idx].object_id + obj = hsim_physics.get_obj_from_id(self.sim, hit_obj_id) + if obj is None: + hit_idx += 1 + else: + obj_found = True + if obj_found: + print( + f"Object: {obj.handle} is {'Articulated' if obj.is_articulated else 'Rigid'} Object at {obj.translation}" + ) + else: + print("This is the stage.") + + if not shift_pressed: + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(obj) + elif obj_found: + # add or remove object from selected objects, depending on whether it is already selected or not + self.obj_editor.toggle_sel_obj(obj) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def scroll_event(self, event: Application.ScrollEvent) -> None: + """ + Handles `Application.ScrollEvent`. When in LOOK mode, enables camera + zooming (fine-grained zoom using shift) When in GRAB mode, adjusts the depth + of the grabber's object. (larger depth change rate using shift) + """ + scroll_mod_val = ( + event.offset.y + if abs(event.offset.y) > abs(event.offset.x) + else event.offset.x + ) + if not scroll_mod_val: + return + + # use shift to scale action response + mod = Application.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + # ctrl_pressed = bool(event.modifiers & mod.CTRL) + + # LOOK MODE + # use shift for fine-grained zooming + self.spot_agent.mod_spot_cam( + scroll_mod_val=scroll_mod_val, + shift_pressed=shift_pressed, + alt_pressed=alt_pressed, + ) + + self.redraw() + event.accepted = True + + def pointer_release_event(self, event: Application.PointerEvent) -> None: + """ + Release any existing constraints. + """ + event.accepted = True + + def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> mn.Vector2i: + """ + This function will get a screen-space mouse position appropriately + scaled based on framebuffer size and window size. Generally these would be + the same value, but on certain HiDPI displays (Retina displays) they may be + different. + """ + scaling = mn.Vector2i(self.framebuffer_size) / mn.Vector2i(self.window_size) + return mouse_event_position * scaling + + def navmesh_config_and_recompute(self) -> None: + """ + This method is setup to be overridden in for setting config accessibility + in inherited classes. + """ + self.navmesh_settings = habitat_sim.NavMeshSettings() + self.navmesh_settings.set_defaults() + self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height + self.navmesh_settings.agent_radius = self.cfg.agents[self.agent_id].radius + self.navmesh_settings.include_static_objects = True + + # first cache AO motion types and set to STATIC for navmesh + ao_motion_types = [] + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + # ignore the robot + if "hab_spot" not in ao.handle: + ao_motion_types.append((ao, ao.motion_type)) + ao.motion_type = habitat_sim.physics.MotionType.STATIC + + self.sim.recompute_navmesh(self.sim.pathfinder, self.navmesh_settings) + + # reset AO motion types from cache + for ao, ao_orig_motion_type in ao_motion_types: + ao.motion_type = ao_orig_motion_type + + def exit_event(self, event: Application.ExitEvent): + """ + Overrides exit_event to properly close the Simulator before exiting the + application. + """ + for i in range(self.num_env): + self.tiled_sims[i].close(destroy=True) + event.accepted = True + exit(0) + + def draw_text(self, sensor_spec): + # make magnum text background transparent for text + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + + self.shader.bind_vector_texture(self.glyph_cache.texture) + self.shader.transformation_projection_matrix = self.window_text_transform + self.shader.color = [1.0, 1.0, 1.0] + + sensor_type_string = str(sensor_spec.sensor_type.name) + sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + edit_string = self.obj_editor.edit_disp_str() + self.window_text.render( + f""" +{self.fps} FPS +Scene ID : {os.path.split(self.cfg.sim_cfg.scene_id)[1].split('.scene_instance')[0]} +Sensor Type: {sensor_type_string} +Sensor Subtype: {sensor_subtype_string} +{edit_string} + """ + ) + self.shader.draw(self.window_text.mesh) + + # Disable blending for text + mn.gl.Renderer.disable(mn.gl.Renderer.Feature.BLENDING) + + def print_help_text(self) -> None: + """ + Print the Key Command help text. + """ + logger.info( + """ +===================================================== +Welcome to the Habitat-sim Python Spot Viewer application! +===================================================== +Mouse Functions +---------------- + LEFT: + Click and drag to rotate the view around Spot. + RIGHT: + Select an object(s) to modify. If multiple objects selected all will be modified equally + (+SHIFT) add/remove object from selected set. Most recently selected object (with yellow box) will be target object. + WHEEL: + Zoom in and out on Spot view. + (+ALT): Raise/Lower the camera's target above Spot. + + +Key Commands: +------------- + esc: Exit the application. + 'h': Display this help message. + 'p': Toggle the display of on-screen data + + Spot/Camera Controls: + 'wasd': Move Spot's body forward/backward and rotate left/right. + 'qe': Move Spot's body in strafe left/right. + + '0': Reset the camera around Spot (after raising/lowering) + '1' : Disengage/re-engage the navmesh constraints (no-clip toggle). When toggling back on, + before collision/navmesh is re-engaged, the closest point to the navmesh is searched + for. If found, spot is snapped to it, but if not found, spot will stay in no-clip + mode and a message will display. + + Scene Object Modification UI: + 'g' : Change Edit mode to either Move or Rotate the selected object(s) + 'b' (+ SHIFT) : Increment (Decrement) the current edit amounts. + Editing : + - With 1 or more objects selected: + - When Move Object Edit mode is selected : + - LEFT/RIGHT arrow keys: move the selected object(s) along global X axis. + - UP/DOWN arrow keys: move the selected object(s) along global Z axis. + (+ALT): move the selected object(s) up/down (global Y axis) + - When Rotate Object Edit mode is selected : + - LEFT/RIGHT arrow keys: rotate the selected object(s) around global Y axis. + - UP/DOWN arrow keys: rotate the selected object(s) around global Z axis. + (+ALT): rotate the selected object(s) around global X axis. + - BACKSPACE: delete the selected object(s) + 'y': delete the selected object and record it as clutter. + - Matching target selected object(s) (rendered with yellow box) specified dimension : + - '2': all selected objects match selected 'target''s x value + - '3': all selected objects match selected 'target''s y value + - '4': all selected objects match selected 'target''s z value + - '5': all selected objects match selected 'target''s orientation + 'u': Undo Edit + - With an object selected: Undo a single modification step for the selected object(s) + (+SHIFT) : redo modification to the selected object(s) + + '6': Select all objects that match the type of the current target/highlit (yellow box) object + + 'i': Save the current, modified, scene_instance file. Also save removed_clutter.txt containing object names of all removed clutter objects. + - With Shift : also close the viewer. + + 'j': Modify AO link states : + (+SHIFT) : Open Selected/All AOs + (-SHIFT) : Close Selected/All AOs + (+ALT) : Modify Selected AOs + (-ALT) : Modify All AOs + + 'l' : Toggle types of objects to display boxes around : None, AOs, Rigids, Both + + + 'v': + - With object(s) selected : Duplicate the selected object(s) + - With no object selected : Load an object by providing a uniquely identifying substring of the object's name + + + Utilities: + 'r': Reset the simulator with the most recently loaded scene. + ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). + 'c': Toggle the contact point debug render overlay on/off. If toggled to true, + then run a discrete collision detection pass and render a debug wireframe overlay + showing active contact points and normals (yellow=fixed length normals, red=collision distances). + 'k' Toggle Semantic visualization bounds (currently only Semantic Region annotations) + 'n': Show/hide NavMesh wireframe. + (+SHIFT) Recompute NavMesh with Spot settings (already done). + (+ALT) Re-sample Spot's position from the NavMesh. + + + Simulation: + SPACE: Toggle physics simulation on/off. + '.': Take a single simulation step if not simulating continuously. +===================================================== +""" + ) + + +class Timer: + """ + Timer class used to keep track of time between buffer swaps + and guide the display frame rate. + """ + + start_time = 0.0 + prev_frame_time = 0.0 + prev_frame_duration = 0.0 + running = False + + @staticmethod + def start() -> None: + """ + Starts timer and resets previous frame time to the start time. + """ + Timer.running = True + Timer.start_time = time.time() + Timer.prev_frame_time = Timer.start_time + Timer.prev_frame_duration = 0.0 + + @staticmethod + def stop() -> None: + """ + Stops timer and erases any previous time data, resetting the timer. + """ + Timer.running = False + Timer.start_time = 0.0 + Timer.prev_frame_time = 0.0 + Timer.prev_frame_duration = 0.0 + + @staticmethod + def next_frame() -> None: + """ + Records previous frame duration and updates the previous frame timestamp + to the current time. If the timer is not currently running, perform nothing. + """ + if not Timer.running: + return + Timer.prev_frame_duration = time.time() - Timer.prev_frame_time + Timer.prev_frame_time = time.time() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + # optional arguments + parser.add_argument( + "--scene", + default="./data/test_assets/scenes/simple_room.glb", + type=str, + help='scene/stage file to load (default: "./data/test_assets/scenes/simple_room.glb")', + ) + parser.add_argument( + "--dataset", + default="./data/objects/ycb/ycb.scene_dataset_config.json", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "./data/objects/ycb/ycb.scene_dataset_config.json")', + ) + parser.add_argument( + "--disable-physics", + action="store_true", + help="disable physics simulation (default: False)", + ) + parser.add_argument( + "--use-default-lighting", + action="store_true", + help="Override configured lighting to use default lighting for the stage.", + ) + parser.add_argument( + "--hbao", + action="store_true", + help="Enable horizon-based ambient occlusion, which provides soft shadows in corners and crevices.", + ) + parser.add_argument( + "--enable-batch-renderer", + action="store_true", + help="Enable batch rendering mode. The number of concurrent environments is specified with the num-environments parameter.", + ) + parser.add_argument( + "--num-environments", + default=1, + type=int, + help="Number of concurrent environments to batch render. Note that only the first environment simulates physics and can be controlled.", + ) + parser.add_argument( + "--composite-files", + type=str, + nargs="*", + help="Composite files that the batch renderer will use in-place of simulation assets to improve memory usage and performance. If none is specified, the original scene files will be loaded from disk.", + ) + parser.add_argument( + "--width", + default=1080, + type=int, + help="Horizontal resolution of the window.", + ) + parser.add_argument( + "--height", + default=720, + type=int, + help="Vertical resolution of the window.", + ) + + args = parser.parse_args() + + if args.num_environments < 1: + parser.error("num-environments must be a positive non-zero integer.") + if args.width < 1: + parser.error("width must be a positive non-zero integer.") + if args.height < 1: + parser.error("height must be a positive non-zero integer.") + + # Setting up sim_settings + sim_settings: Dict[str, Any] = default_sim_settings + sim_settings["scene"] = args.scene + sim_settings["scene_dataset_config_file"] = args.dataset + sim_settings["enable_physics"] = not args.disable_physics + sim_settings["use_default_lighting"] = args.use_default_lighting + sim_settings["enable_batch_renderer"] = args.enable_batch_renderer + sim_settings["num_environments"] = args.num_environments + sim_settings["composite_files"] = args.composite_files + sim_settings["window_width"] = args.width + sim_settings["window_height"] = args.height + sim_settings["sensor_height"] = 0 + sim_settings["enable_hbao"] = args.hbao + + # start the application + HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/examples/viewer.py b/examples/viewer.py index ad1af834c1..9db670393e 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. @@ -459,10 +461,9 @@ def move_and_look(self, repetitions: int) -> None: if repetitions == 0: return - key = Application.Key agent = self.sim.agents[self.agent_id] - press: Dict[key.key, bool] = self.pressed - act: Dict[key.key, str] = self.key_to_action + press: Dict[Application.Key.key, bool] = self.pressed + act: Dict[Application.Key.key, str] = self.key_to_action action_queue: List[str] = [act[k] for k, v in press.items() if v] diff --git a/src_python/habitat_sim/utils/__init__.py b/src_python/habitat_sim/utils/__init__.py index e1c3f2418e..dcd157737f 100755 --- a/src_python/habitat_sim/utils/__init__.py +++ b/src_python/habitat_sim/utils/__init__.py @@ -9,14 +9,9 @@ # TODO @maksymets: remove after habitat-lab/examples/new_actions.py will be # fixed - -from habitat_sim.utils import common, manager_utils, settings, validators, viz_utils -from habitat_sim.utils.common import quat_from_angle_axis, quat_rotate_vector +from habitat_sim.utils import manager_utils, settings, validators, viz_utils __all__ = [ - "quat_from_angle_axis", - "quat_rotate_vector", - "common", "viz_utils", "validators", "settings", diff --git a/src_python/habitat_sim/utils/classes/__init__.py b/src_python/habitat_sim/utils/classes/__init__.py new file mode 100755 index 0000000000..435063cc04 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/__init__.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +from habitat_sim.utils.classes.markersets_editor import MarkerSetsEditor +from habitat_sim.utils.classes.object_editor import ObjectEditor +from habitat_sim.utils.classes.semantic_display import SemanticDisplay + +__all__ = [ + "ObjectEditor", + "MarkerSetsEditor", + "SemanticDisplay", +] diff --git a/src_python/habitat_sim/utils/classes/markersets_editor.py b/src_python/habitat_sim/utils/classes/markersets_editor.py new file mode 100644 index 0000000000..e545410667 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/markersets_editor.py @@ -0,0 +1,436 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import os +from typing import Any, Dict, Set + +import magnum as mn +import numpy as np + +import habitat_sim +from habitat_sim.utils.namespace.hsim_physics import ( + get_obj_from_handle, + get_obj_from_id, +) + + +class MarkerSetsEditor: + def __init__( + self, sim: habitat_sim.simulator.Simulator, task_names_set: Set = None + ): + # Handle default being none + if task_names_set is None: + task_names_set = set() + self.sim = sim + self.markerset_taskset_names = list(task_names_set) + self.update_markersets() + + def update_markersets(self): + """ + Call when created and when a new AO is added or removed + """ + task_names_set = set(self.markerset_taskset_names) + + self.marker_sets_per_obj = self.get_all_markersets() + + # initialize list of possible taskSet names along with those being added specifically + for sub_dict in self.marker_sets_per_obj.values(): + for _obj_handle, MarkerSet in sub_dict.items(): + task_names_set.update(MarkerSet.get_all_taskset_names()) + + # Necessary class-level variables. + self.markerset_taskset_names = list(task_names_set) + # remove trailing s from taskset name for each markerset label prefix + self.markerset_label_prefix = [ + s[:-1] if s.endswith("s") else s for s in self.markerset_taskset_names + ] + + self.current_markerset_taskset_idx = 0 + + self.marker_sets_changed: Dict[str, Dict[str, bool]] = {} + # Hierarchy of colors to match the markerset hierarchy + self.marker_debug_random_colors: Dict[str, Dict[str, Any]] = {} + for sub_key, sub_dict in self.marker_sets_per_obj.items(): + tmp_dict_changed = {} + tmp_dict_colors: Dict[str, Any] = {} + for key in sub_dict: + print(f"subkey : {sub_key} | key : {key}") + tmp_dict_changed[key] = False + tmp_dict_colors[key] = {} + self.marker_sets_changed[sub_key] = tmp_dict_changed + self.marker_debug_random_colors[sub_key] = tmp_dict_colors + + # for debugging + self.glbl_marker_point_dicts_per_obj = self.get_all_global_markers() + + def get_current_taskname(self): + """ + Retrieve the name of the currently used markerset taskname, as specified in the current object's markersets + """ + return self.markerset_taskset_names[self.current_markerset_taskset_idx] + + def cycle_current_taskname(self, forward: bool = True): + mod_val = 1 if forward else -1 + self.current_markerset_taskset_idx = ( + self.current_markerset_taskset_idx + + len(self.markerset_taskset_names) + + mod_val + ) % len(self.markerset_taskset_names) + + def set_current_taskname(self, taskname: str): + if taskname in self.markerset_taskset_names: + self.current_markerset_taskset_idx = self.markerset_taskset_names.index( + taskname + ) + else: + print( + f"Specified taskname {taskname} not valid, so taskname is remaining {self.get_current_taskname()}" + ) + + def get_all_markersets(self): + """ + Get all the markersets defined in the currently loaded objects and articulated objects. + Note : any modified/saved markersets may require their owning Configurations + to be reloaded before this function would expose them. + """ + print("Start getting all markersets") + # marker set cache of existing markersets for all objs in scene, keyed by object name + marker_sets_per_obj = {} + ro_marker_sets = {} + rom = self.sim.get_rigid_object_manager() + obj_dict = rom.get_objects_by_handle_substring("") + for handle, obj in obj_dict.items(): + print(f"setting rigid markersets for {handle}") + ro_marker_sets[handle] = obj.marker_sets + ao_marker_sets = {} + aom = self.sim.get_articulated_object_manager() + ao_obj_dict = aom.get_objects_by_handle_substring("") + for handle, obj in ao_obj_dict.items(): + print(f"setting ao markersets for {handle}") + ao_marker_sets[handle] = obj.marker_sets + print("Done getting all markersets") + + marker_sets_per_obj["ao"] = ao_marker_sets + marker_sets_per_obj["ro"] = ro_marker_sets + + return marker_sets_per_obj + + def place_marker_at_hit_location( + self, hit_info, ao_link_map: Dict[int, int], add_marker: bool + ): + selected_object = None + + # object or ao at hit location. If none, hit stage + obj = get_obj_from_id(self.sim, hit_info.object_id, ao_link_map) + print( + f"Marker : Mouse click object : {hit_info.object_id} : Point : {hit_info.point} " + ) + # TODO these values need to be modifiable + task_set_name = self.get_current_taskname() + marker_set_name = ( + f"{self.markerset_label_prefix[self.current_markerset_taskset_idx]}_000" + ) + if obj is None: + print( + f"Currently can't add a marker to the stage : ID : ({hit_info.object_id})." + ) + # TODO get stage's marker_sets properly + obj_marker_sets = None + obj_handle = "stage" + link_ix = -1 + link_name = "root" + # TODO need to support stage properly including root transformation + local_hit_point = hit_info.point + else: + selected_object = obj + # get a reference to the object/ao 's markerSets + obj_marker_sets = obj.marker_sets + obj_handle = obj.handle + if obj.is_articulated: + obj_type = "articulated object" + # this is an ArticulatedLink, so we can add markers' + link_ix = obj.link_object_ids[hit_info.object_id] + link_name = obj.get_link_name(link_ix) + obj_type_key = "ao" + + else: + obj_type = "rigid object" + # this is an ArticulatedLink, so we can add markers' + link_ix = -1 + link_name = "root" + obj_type_key = "ro" + + local_hit_point_list = obj.transform_world_pts_to_local( + [hit_info.point], link_ix + ) + # get location in local space + local_hit_point = local_hit_point_list[0] + + print( + f"Marker on this {obj_type} : {obj_handle} link Idx : {link_ix} : link name : {link_name} world point : {hit_info.point} local_hit_point : {local_hit_point}" + ) + + if obj_marker_sets is not None: + # add marker if left button clicked + if add_marker: + # if the desired hierarchy does not exist, create it + if not obj_marker_sets.has_task_link_markerset( + task_set_name, link_name, marker_set_name + ): + obj_marker_sets.init_task_link_markerset( + task_set_name, link_name, marker_set_name + ) + # get points for current task_set i.e. ("faucets"), link_name, marker_set_name i.e.("faucet_000") + curr_markers = obj_marker_sets.get_task_link_markerset_points( + task_set_name, link_name, marker_set_name + ) + # add point to list + curr_markers.append(local_hit_point) + # save list + obj_marker_sets.set_task_link_markerset_points( + task_set_name, link_name, marker_set_name, curr_markers + ) + else: + # right click is remove marker + print( + f"About to check obj {obj_handle} if it has points in MarkerSet : {marker_set_name}, LinkSet :{link_name}, TaskSet :{task_set_name} so removal aborted." + ) + if obj_marker_sets.has_task_link_markerset( + task_set_name, link_name, marker_set_name + ): + # Non-empty markerset so find closest point to target and delete + curr_markers = obj_marker_sets.get_task_link_markerset_points( + task_set_name, link_name, marker_set_name + ) + # go through every point to find closest + closest_marker_index = None + closest_marker_dist = 999999 + for m_idx in range(len(curr_markers)): + m_dist = (local_hit_point - curr_markers[m_idx]).length() + if m_dist < closest_marker_dist: + closest_marker_dist = m_dist + closest_marker_index = m_idx + if closest_marker_index is not None: + del curr_markers[closest_marker_index] + # save new list + obj_marker_sets.set_task_link_markerset_points( + task_set_name, link_name, marker_set_name, curr_markers + ) + else: + print( + f"There are no points in MarkerSet : {marker_set_name}, LinkSet :{link_name}, TaskSet :{task_set_name} so removal aborted." + ) + self.marker_sets_per_obj[obj_type_key][obj_handle] = obj_marker_sets + self.marker_sets_changed[obj_type_key][obj_handle] = True + self.save_markerset_attributes(obj) + return selected_object + + def draw_marker_sets_debug( + self, debug_line_render: Any, camera_position: mn.Vector3 + ) -> None: + """ + Draw the global state of all configured marker sets. + """ + for obj_type_key, sub_dict in self.marker_sets_per_obj.items(): + color_dict = self.marker_debug_random_colors[obj_type_key] + for obj_handle, obj_markerset in sub_dict.items(): + marker_points_dict = obj_markerset.get_all_marker_points() + if obj_markerset.num_tasksets > 0: + obj = get_obj_from_handle(self.sim, obj_handle) + for task_name, task_set_dict in marker_points_dict.items(): + if task_name not in color_dict[obj_handle]: + color_dict[obj_handle][task_name] = {} + for link_name, link_set_dict in task_set_dict.items(): + if link_name not in color_dict[obj_handle][task_name]: + color_dict[obj_handle][task_name][link_name] = {} + if link_name in ["root", "body"]: + link_id = -1 + else: + link_id = obj.get_link_id_from_name(link_name) + + for ( + markerset_name, + marker_pts_list, + ) in link_set_dict.items(): + # print(f"markerset_name : {markerset_name} : marker_pts_list : {marker_pts_list} type : {type(marker_pts_list)} : len : {len(marker_pts_list)}") + if ( + markerset_name + not in color_dict[obj_handle][task_name][link_name] + ): + color_dict[obj_handle][task_name][link_name][ + markerset_name + ] = mn.Color4(mn.Vector3(np.random.random(3))) + marker_set_color = color_dict[obj_handle][task_name][ + link_name + ][markerset_name] + global_points = obj.transform_local_pts_to_world( + marker_pts_list, link_id + ) + for global_marker_pos in global_points: + debug_line_render.draw_circle( + translation=global_marker_pos, + radius=0.005, + color=marker_set_color, + normal=camera_position - global_marker_pos, + ) + + def save_all_dirty_markersets(self) -> None: + # save config for object handle's markersets + for subdict in self.marker_sets_changed.values(): + for obj_handle, is_dirty in subdict.items(): + if is_dirty: + obj = get_obj_from_handle(self.sim, obj_handle) + self.save_markerset_attributes(obj) + + def save_markerset_attributes(self, obj) -> None: + """ + Modify the attributes for the passed object to include the + currently edited markersets and save those attributes to disk + """ + # get the name of the attrs used to initialize the object + obj_init_attr_handle = obj.creation_attributes.handle + + if obj.is_articulated: + # save AO config + attrMgr = self.sim.metadata_mediator.ao_template_manager + subdict = "ao" + else: + # save obj config + attrMgr = self.sim.metadata_mediator.object_template_manager + subdict = "ro" + # get copy of initialization attributes as they were in manager, + # unmodified by scene instance values such as scale + init_attrs = attrMgr.get_template_by_handle(obj_init_attr_handle) + # TEMP TODO Remove this when fixed in Simulator + # Clean up sub-dirs being added to asset handles. + if obj.is_articulated: + init_attrs.urdf_filepath = init_attrs.urdf_filepath.split(os.sep)[-1] + init_attrs.render_asset_handle = init_attrs.render_asset_handle.split( + os.sep + )[-1] + else: + init_attrs.render_asset_handle = init_attrs.render_asset_handle.split( + os.sep + )[-1] + init_attrs.collision_asset_handle = init_attrs.collision_asset_handle.split( + os.sep + )[-1] + # put edited subconfig into initial attributes' markersets + markersets = init_attrs.get_marker_sets() + # manually copying because the markersets type is getting lost from markersets + edited_markersets = self.marker_sets_per_obj[subdict][obj.handle] + if edited_markersets.top_level_num_entries == 0: + # if all subconfigs of edited_markersets are gone, clear out those in + # markersets ref within attributes. + for subconfig_key in markersets.get_subconfig_keys(): + markersets.remove_subconfig(subconfig_key) + else: + # Copy subconfigs from local copy of markersets to init attributes' copy + for subconfig_key in edited_markersets.get_subconfig_keys(): + markersets.save_subconfig( + subconfig_key, edited_markersets.get_subconfig(subconfig_key) + ) + + # reregister template + attrMgr.register_template(init_attrs, init_attrs.handle, True) + # save to original location - uses saved location in attributes + attrMgr.save_template_by_handle(init_attrs.handle, True) + # clear out dirty flag + self.marker_sets_changed[subdict][obj.handle] = False + + def get_all_global_markers(self): + """ + Debug function. Get all markers in global space, in nested hierarchy + """ + + def get_points_as_global(obj, marker_points_dict): + new_markerset_dict = {} + # for every task + for task_name, task_dict in marker_points_dict.items(): + new_task_dict = {} + # for every link + for link_name, link_dict in task_dict.items(): + if link_name in ["root", "body"]: + link_id = -1 + else: + # articulated object + link_id = obj.get_link_id_from_name(link_name) + new_link_dict = {} + # for every markerset + for subset, markers_list in link_dict.items(): + new_markers_list = obj.transform_local_pts_to_world( + markers_list, link_id + ) + new_link_dict[subset] = new_markers_list + new_task_dict[link_name] = new_link_dict + new_markerset_dict[task_name] = new_task_dict + return new_markerset_dict + + # marker set cache of existing markersets for all objs in scene, keyed by object name + marker_set_global_dicts_per_obj = {} + marker_set_dict_ao = {} + aom = self.sim.get_articulated_object_manager() + ao_obj_dict = aom.get_objects_by_handle_substring("") + for handle, obj in ao_obj_dict.items(): + marker_set_dict_ao[handle] = get_points_as_global( + obj, obj.marker_sets.get_all_marker_points() + ) + marker_set_dict_ro = {} + rom = self.sim.get_rigid_object_manager() + obj_dict = rom.get_objects_by_handle_substring("") + for handle, obj in obj_dict.items(): + marker_set_dict_ro[handle] = get_points_as_global( + obj, obj.marker_sets.get_all_marker_points() + ) + marker_set_global_dicts_per_obj["ao"] = marker_set_dict_ao + marker_set_global_dicts_per_obj["ro"] = marker_set_dict_ro + return marker_set_global_dicts_per_obj + + def _draw_markersets_glbl_debug_objtype( + self, obj_type_key: str, debug_line_render: Any, camera_position: mn.Vector3 + ) -> None: + obj_dict = self.glbl_marker_point_dicts_per_obj[obj_type_key] + color_dict = self.marker_debug_random_colors[obj_type_key] + for ( + obj_handle, + marker_points_dict, + ) in obj_dict.items(): + for task_name, task_set_dict in marker_points_dict.items(): + if task_name not in color_dict[obj_handle]: + color_dict[obj_handle][task_name] = {} + for link_name, link_set_dict in task_set_dict.items(): + if link_name not in color_dict[obj_handle][task_name]: + color_dict[obj_handle][task_name][link_name] = {} + + for markerset_name, global_points in link_set_dict.items(): + # print(f"markerset_name : {markerset_name} : marker_pts_list : {marker_pts_list} type : {type(marker_pts_list)} : len : {len(marker_pts_list)}") + if ( + markerset_name + not in color_dict[obj_handle][task_name][link_name] + ): + color_dict[obj_handle][task_name][link_name][ + markerset_name + ] = mn.Color4(mn.Vector3(np.random.random(3))) + marker_set_color = color_dict[obj_handle][task_name][link_name][ + markerset_name + ] + for global_marker_pos in global_points: + debug_line_render.draw_circle( + translation=global_marker_pos, + radius=0.005, + color=marker_set_color, + normal=camera_position - global_marker_pos, + ) + + def draw_markersets_glbl_debug( + self, debug_line_render: Any, camera_position: mn.Vector3 + ) -> None: + self._draw_markersets_glbl_debug_objtype( + "ao", debug_line_render=debug_line_render, camera_position=camera_position + ) + self._draw_markersets_glbl_debug_objtype( + "ro", debug_line_render=debug_line_render, camera_position=camera_position + ) diff --git a/src_python/habitat_sim/utils/classes/object_editor.py b/src_python/habitat_sim/utils/classes/object_editor.py new file mode 100644 index 0000000000..a7b26e1925 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/object_editor.py @@ -0,0 +1,973 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +from collections import defaultdict +from enum import Enum +from typing import Any, Dict, List, Optional + +import magnum as mn +from numpy import pi + +import habitat_sim +from habitat_sim import physics as HSim_Phys +from habitat_sim.physics import MotionType as HSim_Phys_MT +from habitat_sim.utils.namespace.hsim_physics import ( + get_ao_root_bb, + get_obj_from_handle, + open_link, +) + + +# Class to control editing objects +# Create an instance and then map various keyboard keys/interactive inputs to appropriate functions. +# Be sure to always specify selected object when it changes. +class ObjectEditor: + # Describe edit type + class EditMode(Enum): + MOVE = 0 + ROTATE = 1 + NUM_VALS = 2 # last value + + EDIT_MODE_NAMES = ["Move object", "Rotate object"] + + # Describe edit distance values + class DistanceMode(Enum): + TINY = 0 + VERY_SMALL = 1 + SMALL = 2 + MEDIUM = 3 + LARGE = 4 + HUGE = 5 + NUM_VALS = 6 + + # What kind of objects to display boxes around + class ObjectTypeToDraw(Enum): + NONE = 0 + ARTICULATED = 1 + RIGID = 2 + BOTH = 3 + NUM_VALS = 4 + + OBJECT_TYPE_NAMES = ["", "Articulated", "Rigid", "Both"] + + # distance values in m + DISTANCE_MODE_VALS = [0.001, 0.01, 0.02, 0.05, 0.1, 0.5] + # angle value multipliers (in degrees) - multiplied by conversion + ROTATION_MULT_VALS = [1.0, 10.0, 30.0, 45.0, 60.0, 90.0] + # 1 radian + BASE_EDIT_ROT_AMT = pi / 180.0 + # Vector to displace removed objects + REMOVAL_DISP_VEC = mn.Vector3(0.0, -20.0, 0.0) + + def __init__(self, sim: habitat_sim.simulator.Simulator): + self.sim = sim + # Set up object and edit collections/caches + self._init_obj_caches() + # Edit mode + self.curr_edit_mode = ObjectEditor.EditMode.MOVE + # Edit distance/amount + self.curr_edit_multiplier = ObjectEditor.DistanceMode.VERY_SMALL + # Type of objects to draw highlight aabb boxes around + self.obj_type_to_draw = ObjectEditor.ObjectTypeToDraw.NONE + # Set initial values + self.set_edit_vals() + + def _init_obj_caches(self): + # Internal: Dict of currently selected object ids to index in + self._sel_obj_ids: Dict[int, int] = {} + # list of current objects selected. Last object in list is "target" object + self.sel_objs: List[Any] = [] + # Complete list of per-objec transformation edits, for undo chaining, + # keyed by object id, value is before and after transform + self.obj_transform_edits: Dict[ + int, List[tuple[mn.Matrix4, mn.Matrix4, bool]] + ] = defaultdict(list) + # Dictionary by object id of transformation when object was most recently saved + self.obj_last_save_transform: Dict[int, mn.Matrix4] = {} + + # Complete list of undone transformation edits, for redo chaining, + # keyed by object id, value is before and after transform. + # Cleared when any future edits are performed. + self.obj_transform_undone_edits: Dict[ + int, List[tuple[mn.Matrix4, mn.Matrix4, bool]] + ] = defaultdict(list) + + # Cache removed objects in dictionary + # These objects should be hidden/moved out of vieew until the scene is + # saved, when they should be actually removed (to allow for undo). + # First key is whether they are articulated or not, value is dict with key == object id, value is object, + self._removed_objs: Dict[bool, Dict[int, Any]] = defaultdict(dict) + # Initialize a flag tracking if the scene is dirty or not + self.modified_scene: bool = False + + def set_edit_mode_rotate(self): + self.curr_edit_mode = ObjectEditor.EditMode.ROTATE + + def set_edit_vals(self): + # Set current scene object edit values for translation and rotation + # 1 cm * multiplier + self.edit_translation_dist = ObjectEditor.DISTANCE_MODE_VALS[ + self.curr_edit_multiplier.value + ] + # 1 radian * multiplier + self.edit_rotation_amt = ( + ObjectEditor.BASE_EDIT_ROT_AMT + * ObjectEditor.ROTATION_MULT_VALS[self.curr_edit_multiplier.value] + ) + + def get_target_sel_obj(self): + """ + Retrieve the primary/target selected object. If none then will return none + """ + if len(self.sel_objs) == 0: + return None + return self.sel_objs[-1] + + def edit_disp_str(self): + """ + Specify display quantities for editing + """ + + edit_mode_string = ObjectEditor.EDIT_MODE_NAMES[self.curr_edit_mode.value] + + dist_mode_substr = ( + f"Translation: {self.edit_translation_dist}m" + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE + else f"Rotation:{ObjectEditor.ROTATION_MULT_VALS[self.curr_edit_multiplier.value]} deg " + ) + edit_distance_mode_string = f"{dist_mode_substr}" + obj_str = self.edit_obj_disp_str() + obj_type_disp_str = ( + "" + if self.obj_type_to_draw == ObjectEditor.ObjectTypeToDraw.NONE + else f"\nObject Types Being Displayed :{ObjectEditor.OBJECT_TYPE_NAMES[self.obj_type_to_draw.value]}" + ) + disp_str = f"""Edit Mode: {edit_mode_string} +Edit Value: {edit_distance_mode_string} +Scene Is Modified: {self.modified_scene} +Num Sel Objs: {len(self.sel_objs)}{obj_str}{obj_type_disp_str} + """ + return disp_str + + def edit_obj_disp_str(self): + """ + Specify primary selected object display quantities + """ + if len(self.sel_objs) == 0: + return "" + sel_obj = self.sel_objs[-1] + if sel_obj.is_articulated: + tar_str = ( + f"Articulated Object : {sel_obj.handle} with {sel_obj.num_links} links." + ) + else: + tar_str = f"Rigid Object : {sel_obj.handle}" + return f"\nTarget Object is {tar_str}" + + def _clear_sel_objs(self): + """ + Internal: clear object selection structure(s) + """ + self._sel_obj_ids.clear() + self.sel_objs.clear() + + def _add_obj_to_sel(self, obj): + """ + Internal : add object to selection structure(s) + """ + self._sel_obj_ids[obj.object_id] = len(self.sel_objs) + # Add most recently selected object to list of selected objects + self.sel_objs.append(obj) + + def _remove_obj_from_sel(self, obj): + """ + Internal : remove object from selection structure(s) + """ + new_objs: List[Any] = [] + # Rebuild selected object structures without the removed object + self._sel_obj_ids.clear() + for old_obj in self.sel_objs: + if old_obj.object_id != obj.object_id: + self._sel_obj_ids[old_obj.object_id] = len(new_objs) + new_objs.append(old_obj) + self.sel_objs = new_objs + + def set_sel_obj(self, obj): + """ + Set the selected objects to just be the passed obj + """ + self._clear_sel_objs() + if obj is not None: + self._add_obj_to_sel(obj) + + def toggle_sel_obj(self, obj): + """ + Remove or add the passed object to the selected objects dict, depending on whether it is present, or not. + """ + if obj is not None: + if obj.object_id in self._sel_obj_ids: + # Remove object from obj selected dict + self._remove_obj_from_sel(obj) + else: + # Add object to selected dict + self._add_obj_to_sel(obj) + + def sel_obj_list(self, obj_handle_list: List[str]): + """ + Select all objects whose handles are in passed list + """ + self._clear_sel_objs() + sim = self.sim + for obj_handle in obj_handle_list: + obj = get_obj_from_handle(sim, obj_handle) + if obj is not None: + self._add_obj_to_sel(obj) + else: + print(f"Unable to find object with handle : {obj_handle}, so skipping.") + + def set_ao_joint_states( + self, do_open: bool, selected: bool, agent_name: str = "hab_spot" + ): + """ + Set either the selected articulated object states to either fully open or fully closed, or all the articulated object states (except for any robots) + """ + if selected: + # Only selected objs if they are articulated + ao_objs = [ao for ao in self.sel_objs if ao.is_articulated] + else: + # all articulated objs that do not contain agent's name + ao_objs = ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring(search_str=agent_name, contains=False) + .values() + ) + + if do_open: + # Open AOs + for ao in ao_objs: + # open the selected receptacle(s) + for link_ix in ao.get_link_ids(): + if ao.get_link_joint_type(link_ix) in [ + HSim_Phys.JointType.Prismatic, + HSim_Phys.JointType.Revolute, + ]: + open_link(ao, link_ix) + else: + # Close AOs + for ao in ao_objs: + j_pos = ao.joint_positions + ao.joint_positions = [0.0 for _ in range(len(j_pos))] + j_vel = ao.joint_velocities + ao.joint_velocities = [0.0 for _ in range(len(j_vel))] + + def _set_scene_dirty(self): + """ + Set whether the scene is currently modified from saved version or + not. If there are objects to be deleted or cached transformations + in the undo stack, this flag should be true. + """ + # Set scene to be modified if any aos or rigids have been marked + # for deletion, or any objects have been transformed + self.modified_scene = (len(self._removed_objs[True]) > 0) or ( + len(self._removed_objs[False]) > 0 + ) + + # only check transforms if still false + if not self.modified_scene: + # check all object transformations match most recent edit transform + for obj_id, transform_list in self.obj_transform_edits.items(): + if (obj_id not in self.obj_last_save_transform) or ( + len(transform_list) == 0 + ): + continue + curr_transform = transform_list[-1][1] + if curr_transform != self.obj_last_save_transform[obj_id]: + self.modified_scene = True + + def _move_one_object( + self, + obj, + navmesh_dirty: bool, + translation: Optional[mn.Vector3] = None, + rotation: Optional[mn.Quaternion] = None, + removal: bool = False, + ) -> bool: + """ + Internal. Move a single object with a given modification and save the resulting state to the buffer. + Returns whether the navmesh should be rebuilt due to an object changing position, or previous edits. + """ + if obj is None: + print("No object is selected so ignoring move request") + return navmesh_dirty + action_str = ( + f"{'Articulated' if obj.is_articulated else 'Rigid'} Object {obj.handle}" + ) + # If object is marked for deletion, don't allow it to be further moved + if obj.object_id in self._removed_objs[obj.is_articulated]: + print( + f"{action_str} already marked for deletion so cannot be moved. Restore object to change its transformation." + ) + return navmesh_dirty + # Move object if transforms exist + if translation is not None or rotation is not None: + orig_transform = obj.transformation + # First time save of original transformation for objects being moved + if obj.object_id not in self.obj_last_save_transform: + self.obj_last_save_transform[obj.object_id] = orig_transform + orig_mt = obj.motion_type + obj.motion_type = HSim_Phys_MT.KINEMATIC + if translation is not None: + obj.translation = obj.translation + translation + action_str = f"{action_str} translated to {obj.translation};" + if rotation is not None: + obj.rotation = rotation * obj.rotation + action_str = f"{action_str} rotated to {obj.rotation};" + print(action_str) + obj.motion_type = orig_mt + # Save transformation for potential undoing later + trans_tuple = (orig_transform, obj.transformation, removal) + self.obj_transform_edits[obj.object_id].append(trans_tuple) + # Clear entries for redo since we have a new edit + self.obj_transform_undone_edits[obj.object_id] = [] + # Set whether scene has been modified or not + self._set_scene_dirty() + return True + return navmesh_dirty + + def move_sel_objects( + self, + navmesh_dirty: bool, + translation: Optional[mn.Vector3] = None, + rotation: Optional[mn.Quaternion] = None, + removal: bool = False, + ) -> bool: + """ + Move all selected objects with a given modification and save the resulting state to the buffer. + Returns whether the navmesh should be rebuilt due to an object's transformation changing, or previous edits. + """ + for obj in self.sel_objs: + new_navmesh_dirty = self._move_one_object( + obj, + navmesh_dirty, + translation=translation, + rotation=rotation, + removal=removal, + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + return navmesh_dirty + + def _remove_obj(self, obj): + """ + Move and mark the passed object for removal from the scene. + """ + # move selected object outside of direct render area -20 m below current location + translation = ObjectEditor.REMOVAL_DISP_VEC + # ignore navmesh result; removal always recomputes + self._move_one_object(obj, True, translation=translation, removal=True) + # record removed object for eventual deletion upon scene save + self._removed_objs[obj.is_articulated][obj.object_id] = obj + + def _restore_obj(self, obj): + """ + Restore the passed object from the removal queue + """ + # move selected object back to where it was before - back 20m up + translation = -ObjectEditor.REMOVAL_DISP_VEC + # remove object from deletion record + self._removed_objs[obj.is_articulated].pop(obj.object_id, None) + # ignore navmesh result; restoration always recomputes + self._move_one_object(obj, True, translation=translation, removal=False) + + def remove_sel_objects(self): + """ + 'Removes' all selected objects from the scene by hiding them and putting them in queue to be deleted on + scene save update. Returns list of the handles of all the objects removed if successful + + Note : removal is not permanent unless scene is saved. + """ + removed_obj_handles = [] + for obj in self.sel_objs: + if obj is None: + continue + self._remove_obj(obj) + print( + f"Moved {obj.handle} out of view and marked for removal. Removal becomes permanent when scene is saved." + ) + # record handle of removed objects, for return + removed_obj_handles.append(obj.handle) + # unselect all objects, since they were all 'removed' + self._clear_sel_objs() + # retain all object selected transformations. + return removed_obj_handles + + def restore_removed_objects(self): + """ + Undo removals that have not been saved yet via scene instance. Will put object back where it was before marking it for removal + """ + restored_obj_handles = [] + obj_rem_dict = self._removed_objs[True] + removed_obj_keys = list(obj_rem_dict.keys()) + for obj_id in removed_obj_keys: + obj = obj_rem_dict.pop(obj_id, None) + if obj is not None: + self._restore_obj(obj) + restored_obj_handles.append(obj.handle) + obj_rem_dict = self._removed_objs[False] + removed_obj_keys = list(obj_rem_dict.keys()) + for obj_id in removed_obj_keys: + obj = obj_rem_dict.pop(obj_id, None) + if obj is not None: + self._restore_obj(obj) + restored_obj_handles.append(obj.handle) + + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + return restored_obj_handles + + def delete_removed_objs(self): + """ + Delete all the objects in the scene marked for removal. Call before saving a scene instance. + """ + ao_removed_objs = self._removed_objs[True] + if len(ao_removed_objs) > 0: + ao_mgr = self.sim.get_articulated_object_manager() + for obj_id in ao_removed_objs: + ao_mgr.remove_object_by_id(obj_id) + # Get rid of all recorded transforms of specified object + self.obj_transform_edits.pop(obj_id, None) + ro_removed_objs = self._removed_objs[False] + if len(ro_removed_objs) > 0: + ro_mgr = self.sim.get_rigid_object_manager() + for obj_id in ro_removed_objs: + ro_mgr.remove_object_by_id(obj_id) + # Get rid of all recorded transforms of specified object + self.obj_transform_edits.pop(obj_id, None) + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + + def _undo_obj_transform_edit( + self, obj, transform_tuple: tuple[mn.Matrix4, mn.Matrix4, bool] + ): + """ + Changes the object's current transformation to the passed, previous transformation (in idx 0). + Different than a move, only called by undo/redo procedure + """ + old_transform = transform_tuple[0] + orig_mt = obj.motion_type + obj.motion_type = HSim_Phys_MT.KINEMATIC + obj.transformation = old_transform + obj.motion_type = orig_mt + + def _redo_single_obj_edit(self, obj): + """ + Redo edit that has been undone on a single object one step + """ + obj_id = obj.object_id + # Verify there are transforms to redo for this object + if len(self.obj_transform_undone_edits[obj_id]) > 0: + # Last undo state is last element in transforms list + # In tuple idxs : 0 : previous transform, 1 : current transform, 2 : whether was a removal op + # Retrieve and remove last undo + transform_tuple = self.obj_transform_undone_edits[obj_id].pop() + if len(self.obj_transform_undone_edits[obj_id]) == 0: + self.obj_transform_undone_edits.pop(obj_id, None) + # If this had been a removal that had been undone, redo removal + remove_str = "" + if transform_tuple[2]: + # Restore object to removal queue for eventual deletion upon scene save + self._removed_objs[obj.is_articulated][obj.object_id] = obj + remove_str = ", being re-marked for removal," + self._undo_obj_transform_edit(obj, transform_tuple) + # Save transformation tuple for subsequent undoing + # Swap order of transforms since they were redon, for potential undo + undo_tuple = ( + transform_tuple[1], + transform_tuple[0], + transform_tuple[2], + ) + self.obj_transform_edits[obj_id].append(undo_tuple) + print( + f"REDO : Sel Obj : {obj.handle} : Current object{remove_str} transformation : \n{transform_tuple[1]}\nReplaced by saved transformation : \n{transform_tuple[0]}" + ) + + def redo_sel_edits(self): + """ + Internal only. Redo edits that have been undone on all currently selected objects one step + NOTE : does not address scene being dirty or not + """ + if len(self.sel_objs) == 0: + return + # For every object in selected object + for obj in self.sel_objs: + self._redo_single_obj_edit(obj) + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + + def _undo_single_obj_edit(self, obj): + """ + Internal only. Undo any edits on the passed object one step, (including removal marks) + NOTE : does not address scene being dirty or not + """ + obj_id = obj.object_id + # Verify there are transforms to undo for this object + if len(self.obj_transform_edits[obj_id]) > 0: + # Last edit state is last element in transforms list + # In tuple idxs : 0 : previous transform, 1 : current transform, 2 : whether was a removal op + # Retrieve and remove last edit + transform_tuple = self.obj_transform_edits[obj_id].pop() + # If all object edits have been removed, also remove entry + if len(self.obj_transform_edits[obj_id]) == 0: + self.obj_transform_edits.pop(obj_id, None) + # If this was a removal, remove object from removal queue + remove_str = "" + if transform_tuple[2]: + # Remove object from removal queue if there - undo removal + self._removed_objs[obj.is_articulated].pop(obj_id, None) + remove_str = ", being restored from removal list," + self._undo_obj_transform_edit(obj, transform_tuple) + # Save transformation tuple for redoing + # Swap order of transforms since they were undone, for potential redo + redo_tuple = ( + transform_tuple[1], + transform_tuple[0], + transform_tuple[2], + ) + self.obj_transform_undone_edits[obj_id].append(redo_tuple) + print( + f"UNDO : Sel Obj : {obj.handle} : Current object{remove_str} transformation : \n{transform_tuple[1]}\nReplaced by saved transformation : \n{transform_tuple[0]}" + ) + + def undo_sel_edits(self): + """ + Undo the edits that have been performed on all the currently selected objects one step, (including removal marks) + """ + if len(self.sel_objs) == 0: + return + # For every object in selected object + for obj in self.sel_objs: + self._undo_single_obj_edit(obj) + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + + def select_all_matching_objects(self, only_matches: bool): + """ + Selects all objects matching currently selected object (or first object selected) + only_matches : only select objects that match type of first selected object (deselects all others) + """ + if len(self.sel_objs) == 0: + return + # primary object is always at idx -1 + match_obj = self.sel_objs[-1] + obj_is_articulated = match_obj.is_articulated + if only_matches: + # clear out existing objects + self._clear_sel_objs() + + attr_mgr = ( + self.sim.get_articulated_object_manager() + if obj_is_articulated + else self.sim.get_rigid_object_manager() + ) + match_obj_handle = match_obj.handle.split("_:")[0] + new_sel_objs_dict = attr_mgr.get_objects_by_handle_substring( + search_str=match_obj_handle, contains=True + ) + for obj in new_sel_objs_dict.values(): + self._add_obj_to_sel(obj) + # reset match_obj as selected object by first unselected and then re-selecting + self.toggle_sel_obj(match_obj) + self.toggle_sel_obj(match_obj) + + def match_dim_sel_objects( + self, + navmesh_dirty: bool, + new_val: float, + axis: mn.Vector3, + ) -> bool: + """ + Set all selected objects to have the same specified translation dimension value. + new_val : new value to set the location of the object + axis : the dimension's axis to match the value of + """ + trans_vec = new_val * axis + for obj in self.sel_objs: + obj_mod_vec = obj.translation.projected(axis) + new_navmesh_dirty = self._move_one_object( + obj, + navmesh_dirty, + translation=trans_vec - obj_mod_vec, + removal=False, + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + return navmesh_dirty + + def match_x_dim(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's x value + """ + if len(self.sel_objs) == 0: + return None + match_val = self.sel_objs[-1].translation.x + return self.match_dim_sel_objects(navmesh_dirty, match_val, mn.Vector3.x_axis()) + + def match_y_dim(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's y value + """ + if len(self.sel_objs) == 0: + return None + match_val = self.sel_objs[-1].translation.y + return self.match_dim_sel_objects(navmesh_dirty, match_val, mn.Vector3.y_axis()) + + def match_z_dim(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's z value + """ + if len(self.sel_objs) == 0: + return None + match_val = self.sel_objs[-1].translation.z + return self.match_dim_sel_objects(navmesh_dirty, match_val, mn.Vector3.z_axis()) + + def match_orientation(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's orientation + """ + if len(self.sel_objs) == 0: + return None + match_rotation = self.sel_objs[-1].rotation + local_navmesh_dirty = False + for obj in self.sel_objs: + obj_mod_rot = match_rotation * obj.rotation.inverted() + local_navmesh_dirty = self._move_one_object( + obj, + navmesh_dirty, + rotation=obj_mod_rot, + removal=False, + ) + navmesh_dirty = navmesh_dirty or local_navmesh_dirty + return navmesh_dirty + + def edit_left(self, navmesh_dirty: bool) -> bool: + """ + Edit selected objects for left key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=mn.Vector3.x_axis() * self.edit_translation_dist, + ) + # if rotation mode : rotate around y axis + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation( + mn.Rad(self.edit_rotation_amt), mn.Vector3.y_axis() + ), + ) + + def edit_right(self, navmesh_dirty: bool): + """ + Edit selected objects for right key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=-mn.Vector3.x_axis() * self.edit_translation_dist, + ) + # if rotation mode : rotate around y axis + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation( + -mn.Rad(self.edit_rotation_amt), mn.Vector3.y_axis() + ), + ) + return navmesh_dirty + + def edit_up(self, navmesh_dirty: bool, toggle: bool): + """ + Edit selected objects for up key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + trans_axis = mn.Vector3.y_axis() if toggle else mn.Vector3.z_axis() + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=trans_axis * self.edit_translation_dist, + ) + + # if rotation mode : rotate around x or z axis + rot_axis = mn.Vector3.x_axis() if toggle else mn.Vector3.z_axis() + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation(mn.Rad(self.edit_rotation_amt), rot_axis), + ) + + def edit_down(self, navmesh_dirty: bool, toggle: bool): + """ + Edit selected objects for down key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + trans_axis = -mn.Vector3.y_axis() if toggle else -mn.Vector3.z_axis() + + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=trans_axis * self.edit_translation_dist, + ) + # if rotation mode : rotate around x or z axis + rot_axis = mn.Vector3.x_axis() if toggle else mn.Vector3.z_axis() + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation(-mn.Rad(self.edit_rotation_amt), rot_axis), + ) + + def save_current_scene(self): + if self.modified_scene: + # update scene with removals before saving + self.delete_removed_objs() + + # clear out cache of removed objects by resetting dictionary + self._removed_objs = defaultdict(dict) + # Reset all AOs to be 0 + self.set_ao_joint_states(do_open=False, selected=False) + # Save current scene + self.sim.save_current_scene_config(overwrite=True) + # Specify most recent edits for each object that has an undo queue + self.obj_last_save_transform = {} + obj_ids = list(self.obj_transform_edits.keys()) + for obj_id in obj_ids: + transform_list = self.obj_transform_edits[obj_id] + if len(transform_list) == 0: + # if transform list is empty, delete it and skip + self.obj_transform_edits.pop(obj_id, None) + continue + self.obj_last_save_transform[obj_id] = transform_list[-1][1] + + # Clear edited flag + self.modified_scene = False + # + print("Saved modified scene instance JSON to original location.") + else: + print("Nothing modified in scene so save aborted.") + + def load_from_substring( + self, navmesh_dirty: bool, obj_substring: str, build_loc: mn.Vector3 + ): + sim = self.sim + mm = sim.metadata_mediator + template_mgr = mm.ao_template_manager + template_handles = template_mgr.get_template_handles(obj_substring) + build_ao = False + print(f"Attempting to find {obj_substring} as an articulated object") + if len(template_handles) == 1: + print(f"{obj_substring} found as an AO!") + # Specific AO template found + obj_mgr = sim.get_articulated_object_manager() + base_motion_type = HSim_Phys_MT.DYNAMIC + build_ao = True + else: + print(f"Attempting to find {obj_substring} as a rigid object instead") + template_mgr = mm.object_template_manager + template_handles = template_mgr.get_template_handles(obj_substring) + if len(template_handles) != 1: + print( + f"No distinct Rigid or Articulated Object handle found matching substring: '{obj_substring}'. Aborting" + ) + return [], navmesh_dirty + print(f"{obj_substring} found as an RO!") + # Specific Rigid template found + obj_mgr = sim.get_rigid_object_manager() + base_motion_type = HSim_Phys_MT.STATIC + + obj_temp_handle = template_handles[0] + # Build an object using obj_temp_handle, getting template from attr_mgr and object manager obj_mgr + temp = template_mgr.get_template_by_handle(obj_temp_handle) + + if build_ao: + obj_type = "Articulated" + temp.base_type = "FIXED" + template_mgr.register_template(temp) + new_obj = obj_mgr.add_articulated_object_by_template_handle(obj_temp_handle) + else: + # If any changes to template, put them here and re-register template + # template_mgr.register_template(temp) + obj_type = "Rigid" + new_obj = obj_mgr.add_object_by_template_handle(obj_temp_handle) + + if new_obj is not None: + # set new object location to be above location of selected object + new_obj.motion_type = base_motion_type + self.set_sel_obj(new_obj) + # move new object to appropriate location + new_navmesh_dirty = self._move_one_object( + new_obj, navmesh_dirty, translation=build_loc + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + else: + print( + f"Failed to load/create {obj_type} Object from template named {obj_temp_handle}." + ) + # creation failing would have its own message + return [], navmesh_dirty + return [new_obj], navmesh_dirty + + def build_objects(self, navmesh_dirty: bool, build_loc: mn.Vector3): + """ + Make a copy of the selected object(s), or load a named item at some distance away + """ + sim = self.sim + if len(self.sel_objs) > 0: + # Copy all selected objects + res_objs = [] + for obj in self.sel_objs: + # Duplicate object via object ID + if obj.is_articulated: + # duplicate articulated object + new_obj = sim.get_articulated_object_manager().duplicate_articulated_object_by_id( + obj.object_id + ) + else: + # duplicate rigid object + new_obj = sim.get_rigid_object_manager().duplicate_object_by_id( + obj.object_id + ) + + if new_obj is not None: + # set new object location to be above location of copied object + new_obj_translation = mn.Vector3(0.0, 1.0, 0.0) + # set new object rotation to match copied object's rotation + new_obj_rotation = obj.rotation * new_obj.rotation.inverted() + new_obj.motion_type = obj.motion_type + # move new object to appropriate location + new_navmesh_dirty = self._move_one_object( + new_obj, + navmesh_dirty, + translation=new_obj_translation, + rotation=new_obj_rotation, + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + res_objs.append(new_obj) + # duplicated all currently selected objects + # clear currently set selected objects + self._clear_sel_objs() + # Select all new objects + for new_obj in res_objs: + # add object to selected objects + self._add_obj_to_sel(new_obj) + return res_objs, navmesh_dirty + + else: + # No objects selected, get user input to load a single object + obj_substring = input( + "Load Object or AO. Enter a Rigid Object or AO handle substring, first match will be added:" + ).strip() + + if len(obj_substring) == 0: + print("No valid name given. Aborting") + return [], navmesh_dirty + return self.load_from_substring( + navmesh_dirty=navmesh_dirty, + obj_substring=obj_substring, + build_loc=build_loc, + ) + + def change_edit_mode(self, toggle: bool): + # toggle edit mode + mod_val = -1 if toggle else 1 + self.curr_edit_mode = ObjectEditor.EditMode( + (self.curr_edit_mode.value + ObjectEditor.EditMode.NUM_VALS.value + mod_val) + % ObjectEditor.EditMode.NUM_VALS.value + ) + + def change_edit_vals(self, toggle: bool): + # cycle through edit dist/amount multiplier + mod_val = -1 if toggle else 1 + self.curr_edit_multiplier = ObjectEditor.DistanceMode( + ( + self.curr_edit_multiplier.value + + ObjectEditor.DistanceMode.NUM_VALS.value + + mod_val + ) + % ObjectEditor.DistanceMode.NUM_VALS.value + ) + # update the edit values + self.set_edit_vals() + + def change_draw_box_types(self, toggle: bool): + # Cycle through types of objects to display with highlight box + mod_val = -1 if toggle else 1 + self.obj_type_to_draw = ObjectEditor.ObjectTypeToDraw( + ( + self.obj_type_to_draw.value + + ObjectEditor.ObjectTypeToDraw.NUM_VALS.value + + mod_val + ) + % ObjectEditor.ObjectTypeToDraw.NUM_VALS.value + ) + + def _draw_selected_obj(self, obj, debug_line_render, box_color): + """ + Draw a selection box around and axis frame at the origin of a single object + """ + aabb = get_ao_root_bb(obj) if obj.is_articulated else obj.collision_shape_aabb + debug_line_render.push_transform(obj.transformation) + debug_line_render.draw_box(aabb.min, aabb.max, box_color) + debug_line_render.pop_transform() + + def draw_selected_objects(self, debug_line_render): + if len(self.sel_objs) == 0: + return + obj_list = self.sel_objs + sel_obj = obj_list[-1] + if sel_obj.is_alive: + # Last object selected is target object + self._draw_selected_obj( + sel_obj, + debug_line_render=debug_line_render, + box_color=mn.Color4.yellow(), + ) + debug_line_render.draw_axes(sel_obj.translation) + + mag_color = mn.Color4.magenta() + # draw all but last/target object + for i in range(len(obj_list) - 1): + obj = obj_list[i] + if obj.is_alive: + self._draw_selected_obj( + obj, debug_line_render=debug_line_render, box_color=mag_color + ) + debug_line_render.draw_axes(obj.translation) + + def draw_box_around_objs(self, debug_line_render, agent_name: str = "hab_spot"): + """ + Draw a box of an object-type-specific color around every object in the scene (green for AOs and cyan for Rigids) + """ + if self.obj_type_to_draw.value % 2 == 1: + # draw aos if 1 or 3 + attr_mgr = self.sim.get_articulated_object_manager() + # Get all aos excluding the agent if present + new_sel_objs_dict = attr_mgr.get_objects_by_handle_substring( + search_str=agent_name, contains=False + ) + obj_clr = mn.Color4.green() + for obj in new_sel_objs_dict.values(): + if obj.is_alive: + self._draw_selected_obj( + obj, debug_line_render=debug_line_render, box_color=obj_clr + ) + + if self.obj_type_to_draw.value > 1: + # draw rigis if 2 or 3 + attr_mgr = self.sim.get_rigid_object_manager() + new_sel_objs_dict = attr_mgr.get_objects_by_handle_substring(search_str="") + obj_clr = mn.Color4.cyan() + for obj in new_sel_objs_dict.values(): + if obj.is_alive: + self._draw_selected_obj( + obj, debug_line_render=debug_line_render, box_color=obj_clr + ) diff --git a/src_python/habitat_sim/utils/classes/semantic_display.py b/src_python/habitat_sim/utils/classes/semantic_display.py new file mode 100644 index 0000000000..27e0db94f3 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/semantic_display.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# 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 Any, Dict + +import magnum as mn +import numpy as np + +import habitat_sim + + +# Class to display semantic settings in a scene +class SemanticDisplay: + def __init__(self, sim: habitat_sim.simulator.Simulator): + self.sim = sim + # Descriptive strings for semantic region debug draw possible choices + self.semantic_region_debug_draw_choices = ["None", "Kitchen Only", "All"] + # draw semantic region debug visualizations if present : should be [0 : len(semantic_region_debug_draw_choices)-1] + self.semantic_region_debug_draw_state = 0 + # Colors to use for each region's semantic rendering. + self.debug_semantic_colors: Dict[str, mn.Color4] = {} + + def cycle_semantic_region_draw(self): + new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( + self.semantic_region_debug_draw_choices + ) + info_str = f"Change Region Draw from {self.semantic_region_debug_draw_choices[self.semantic_region_debug_draw_state]} to {self.semantic_region_debug_draw_choices[new_state_idx]}" + + # Increment visualize semantic bboxes. Currently only regions supported + self.semantic_region_debug_draw_state = new_state_idx + return info_str + + def draw_region_debug(self, debug_line_render: Any) -> None: + """ + Draw the semantic region wireframes. + """ + if self.semantic_region_debug_draw_state == 0: + return + if len(self.debug_semantic_colors) != len(self.sim.semantic_scene.regions): + self.debug_semantic_colors = {} + for region in self.sim.semantic_scene.regions: + self.debug_semantic_colors[region.id] = mn.Color4( + mn.Vector3(np.random.random(3)) + ) + if self.semantic_region_debug_draw_state == 1: + for region in self.sim.semantic_scene.regions: + if "kitchen" not in region.id.lower(): + continue + color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) + for edge in region.volume_edges: + debug_line_render.draw_transformed_line( + edge[0], + edge[1], + color, + ) + else: + # Draw all + for region in self.sim.semantic_scene.regions: + color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) + for edge in region.volume_edges: + debug_line_render.draw_transformed_line( + edge[0], + edge[1], + color, + ) diff --git a/src_python/habitat_sim/utils/common/__init__.py b/src_python/habitat_sim/utils/common/__init__.py new file mode 100755 index 0000000000..e40b207297 --- /dev/null +++ b/src_python/habitat_sim/utils/common/__init__.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +# List functions in __all__ to make available from this namespace level + + +from habitat_sim._ext.habitat_sim_bindings.core import orthonormalize_rotation_shear +from habitat_sim.utils.common.common import d3_40_colors_hex, d3_40_colors_rgb +from habitat_sim.utils.common.quaternion_utils import ( + angle_between_quats, + quat_from_angle_axis, + quat_from_coeffs, + quat_from_magnum, + quat_from_two_vectors, + quat_rotate_vector, + quat_to_angle_axis, + quat_to_coeffs, + quat_to_magnum, + random_quaternion, +) + +__all__ = [ + "angle_between_quats", + "orthonormalize_rotation_shear", + "quat_from_coeffs", + "quat_to_coeffs", + "quat_from_magnum", + "quat_to_magnum", + "quat_from_angle_axis", + "quat_to_angle_axis", + "quat_rotate_vector", + "quat_from_two_vectors", + "random_quaternion", + "d3_40_colors_hex", + "d3_40_colors_rgb", +] diff --git a/src_python/habitat_sim/utils/common/common.py b/src_python/habitat_sim/utils/common/common.py new file mode 100755 index 0000000000..a031b148d7 --- /dev/null +++ b/src_python/habitat_sim/utils/common/common.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# 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 List + +import numpy as np + + +def colorize_ids(ids): + out = np.zeros((ids.shape[0], ids.shape[1], 3), dtype=np.uint8) + for i in range(ids.shape[0]): + for j in range(ids.shape[1]): + object_index = ids[i, j] + if object_index >= 0: + out[i, j] = d3_40_colors_rgb[object_index % 40] + return out + + +d3_40_colors_rgb: np.ndarray = np.array( + [ + [31, 119, 180], + [174, 199, 232], + [255, 127, 14], + [255, 187, 120], + [44, 160, 44], + [152, 223, 138], + [214, 39, 40], + [255, 152, 150], + [148, 103, 189], + [197, 176, 213], + [140, 86, 75], + [196, 156, 148], + [227, 119, 194], + [247, 182, 210], + [127, 127, 127], + [199, 199, 199], + [188, 189, 34], + [219, 219, 141], + [23, 190, 207], + [158, 218, 229], + [57, 59, 121], + [82, 84, 163], + [107, 110, 207], + [156, 158, 222], + [99, 121, 57], + [140, 162, 82], + [181, 207, 107], + [206, 219, 156], + [140, 109, 49], + [189, 158, 57], + [231, 186, 82], + [231, 203, 148], + [132, 60, 57], + [173, 73, 74], + [214, 97, 107], + [231, 150, 156], + [123, 65, 115], + [165, 81, 148], + [206, 109, 189], + [222, 158, 214], + ], + dtype=np.uint8, +) + + +# [d3_40_colors_hex] +d3_40_colors_hex: List[str] = [ + "0x1f77b4", + "0xaec7e8", + "0xff7f0e", + "0xffbb78", + "0x2ca02c", + "0x98df8a", + "0xd62728", + "0xff9896", + "0x9467bd", + "0xc5b0d5", + "0x8c564b", + "0xc49c94", + "0xe377c2", + "0xf7b6d2", + "0x7f7f7f", + "0xc7c7c7", + "0xbcbd22", + "0xdbdb8d", + "0x17becf", + "0x9edae5", + "0x393b79", + "0x5254a3", + "0x6b6ecf", + "0x9c9ede", + "0x637939", + "0x8ca252", + "0xb5cf6b", + "0xcedb9c", + "0x8c6d31", + "0xbd9e39", + "0xe7ba52", + "0xe7cb94", + "0x843c39", + "0xad494a", + "0xd6616b", + "0xe7969c", + "0x7b4173", + "0xa55194", + "0xce6dbd", + "0xde9ed6", +] +# [/d3_40_colors_hex] diff --git a/src_python/habitat_sim/utils/common.py b/src_python/habitat_sim/utils/common/quaternion_utils.py old mode 100755 new mode 100644 similarity index 64% rename from src_python/habitat_sim/utils/common.py rename to src_python/habitat_sim/utils/common/quaternion_utils.py index 2e9e43b04b..43021d257d --- a/src_python/habitat_sim/utils/common.py +++ b/src_python/habitat_sim/utils/common/quaternion_utils.py @@ -5,19 +5,12 @@ # LICENSE file in the root directory of this source tree. import math -from io import BytesIO -from typing import List, Sequence, Tuple, Union -from urllib.request import urlopen -from zipfile import ZipFile +from typing import Sequence, Tuple, Union import magnum as mn import numpy as np import quaternion as qt -from habitat_sim._ext.habitat_sim_bindings.core import ( # noqa: F401 - orthonormalize_rotation_shear, -) - def quat_from_coeffs(coeffs: Union[Sequence[float], np.ndarray]) -> qt.quaternion: r"""Creates a quaternion from the coeffs returned by the simulator backend @@ -165,112 +158,3 @@ def random_quaternion(): ] ) return mn.Quaternion(qAxis, math.sqrt(1 - u[0]) * math.sin(2 * math.pi * u[1])) - - -def download_and_unzip(file_url, local_directory): - response = urlopen(file_url) - zipfile = ZipFile(BytesIO(response.read())) - zipfile.extractall(path=local_directory) - - -def colorize_ids(ids): - out = np.zeros((ids.shape[0], ids.shape[1], 3), dtype=np.uint8) - for i in range(ids.shape[0]): - for j in range(ids.shape[1]): - object_index = ids[i, j] - if object_index >= 0: - out[i, j] = d3_40_colors_rgb[object_index % 40] - return out - - -d3_40_colors_rgb: np.ndarray = np.array( - [ - [31, 119, 180], - [174, 199, 232], - [255, 127, 14], - [255, 187, 120], - [44, 160, 44], - [152, 223, 138], - [214, 39, 40], - [255, 152, 150], - [148, 103, 189], - [197, 176, 213], - [140, 86, 75], - [196, 156, 148], - [227, 119, 194], - [247, 182, 210], - [127, 127, 127], - [199, 199, 199], - [188, 189, 34], - [219, 219, 141], - [23, 190, 207], - [158, 218, 229], - [57, 59, 121], - [82, 84, 163], - [107, 110, 207], - [156, 158, 222], - [99, 121, 57], - [140, 162, 82], - [181, 207, 107], - [206, 219, 156], - [140, 109, 49], - [189, 158, 57], - [231, 186, 82], - [231, 203, 148], - [132, 60, 57], - [173, 73, 74], - [214, 97, 107], - [231, 150, 156], - [123, 65, 115], - [165, 81, 148], - [206, 109, 189], - [222, 158, 214], - ], - dtype=np.uint8, -) - - -# [d3_40_colors_hex] -d3_40_colors_hex: List[str] = [ - "0x1f77b4", - "0xaec7e8", - "0xff7f0e", - "0xffbb78", - "0x2ca02c", - "0x98df8a", - "0xd62728", - "0xff9896", - "0x9467bd", - "0xc5b0d5", - "0x8c564b", - "0xc49c94", - "0xe377c2", - "0xf7b6d2", - "0x7f7f7f", - "0xc7c7c7", - "0xbcbd22", - "0xdbdb8d", - "0x17becf", - "0x9edae5", - "0x393b79", - "0x5254a3", - "0x6b6ecf", - "0x9c9ede", - "0x637939", - "0x8ca252", - "0xb5cf6b", - "0xcedb9c", - "0x8c6d31", - "0xbd9e39", - "0xe7ba52", - "0xe7cb94", - "0x843c39", - "0xad494a", - "0xd6616b", - "0xe7969c", - "0x7b4173", - "0xa55194", - "0xce6dbd", - "0xde9ed6", -] -# [/d3_40_colors_hex] diff --git a/src_python/habitat_sim/utils/namespace/__init__.py b/src_python/habitat_sim/utils/namespace/__init__.py new file mode 100755 index 0000000000..0f0db8cad5 --- /dev/null +++ b/src_python/habitat_sim/utils/namespace/__init__.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. diff --git a/src_python/habitat_sim/utils/namespace/hsim_physics.py b/src_python/habitat_sim/utils/namespace/hsim_physics.py new file mode 100644 index 0000000000..c9b248c483 --- /dev/null +++ b/src_python/habitat_sim/utils/namespace/hsim_physics.py @@ -0,0 +1,405 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# 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, Union + +import magnum as mn + +import habitat_sim +from habitat_sim import physics as HSim_Phys + + +def get_link_normalized_joint_position( + object_a: HSim_Phys.ManagedArticulatedObject, link_ix: int +) -> float: + """ + Normalize the joint limit range [min, max] -> [0,1] and return the current joint state in this range. + + :param object_a: The parent ArticulatedObject of the link. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + + :return: normalized joint position [0,1] + """ + + assert object_a.get_link_joint_type(link_ix) in [ + HSim_Phys.JointType.Revolute, + HSim_Phys.JointType.Prismatic, + ], f"Invalid joint type '{object_a.get_link_joint_type(link_ix)}'. Open/closed not a valid check for multi-dimensional or fixed joints." + + joint_pos_ix = object_a.get_link_joint_pos_offset(link_ix) + joint_pos = object_a.joint_positions[joint_pos_ix] + limits = object_a.joint_position_limits + + # compute the normalized position [0,1] + n_pos = (joint_pos - limits[0][joint_pos_ix]) / ( + limits[1][joint_pos_ix] - limits[0][joint_pos_ix] + ) + return n_pos + + +def set_link_normalized_joint_position( + object_a: HSim_Phys.ManagedArticulatedObject, + link_ix: int, + normalized_pos: float, +) -> None: + """ + Set the joint's state within its limits from a normalized range [0,1] -> [min, max] + + Assumes the joint has valid joint limits. + + :param object_a: The parent ArticulatedObject of the link. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + :param normalized_pos: The normalized position [0,1] to set. + """ + + assert object_a.get_link_joint_type(link_ix) in [ + HSim_Phys.JointType.Revolute, + HSim_Phys.JointType.Prismatic, + ], f"Invalid joint type '{object_a.get_link_joint_type(link_ix)}'. Open/closed not a valid check for multi-dimensional or fixed joints." + + assert ( + normalized_pos <= 1.0 and normalized_pos >= 0 + ), "values outside the range [0,1] are by definition beyond the joint limits." + + joint_pos_ix = object_a.get_link_joint_pos_offset(link_ix) + limits = object_a.joint_position_limits + joint_positions = object_a.joint_positions + joint_positions[joint_pos_ix] = limits[0][joint_pos_ix] + ( + normalized_pos * (limits[1][joint_pos_ix] - limits[0][joint_pos_ix]) + ) + object_a.joint_positions = joint_positions + + +def link_is_open( + object_a: HSim_Phys.ManagedArticulatedObject, + link_ix: int, + threshold: float = 0.4, +) -> bool: + """ + Check whether a particular AO link is in the "open" state. + We assume that joint limits define the closed state (min) and open state (max). + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + :param threshold: The normalized threshold ratio of joint ranges which are considered "open". E.g. 0.8 = 80% + + :return: Whether or not the link is considered "open". + """ + + return get_link_normalized_joint_position(object_a, link_ix) >= threshold + + +def link_is_closed( + object_a: HSim_Phys.ManagedArticulatedObject, + link_ix: int, + threshold: float = 0.1, +) -> bool: + """ + Check whether a particular AO link is in the "closed" state. + We assume that joint limits define the closed state (min) and open state (max). + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + :param threshold: The normalized threshold ratio of joint ranges which are considered "closed". E.g. 0.1 = 10% + + :return: Whether or not the link is considered "closed". + """ + + return get_link_normalized_joint_position(object_a, link_ix) <= threshold + + +def open_link(object_a: HSim_Phys.ManagedArticulatedObject, link_ix: int) -> None: + """ + Set a link to the "open" state. Sets the joint position to the maximum joint limit. + + TODO: does not do any collision checking to validate the state or move any other objects which may be contained in or supported by this link. + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + """ + + set_link_normalized_joint_position(object_a, link_ix, 1.0) + + +def close_link(object_a: HSim_Phys.ManagedArticulatedObject, link_ix: int) -> None: + """ + Set a link to the "closed" state. Sets the joint position to the minimum joint limit. + + TODO: does not do any collision checking to validate the state or move any other objects which may be contained in or supported by this link. + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + """ + + set_link_normalized_joint_position(object_a, link_ix, 0) + + +def get_bb_corners(range3d: mn.Range3D) -> List[mn.Vector3]: + """ + Return a list of AABB (Range3D) corners in object local space. + """ + return [ + range3d.back_bottom_left, + range3d.back_bottom_right, + range3d.back_top_right, + range3d.back_top_left, + range3d.front_top_left, + range3d.front_top_right, + range3d.front_bottom_right, + range3d.front_bottom_left, + ] + + +def get_ao_root_bb( + ao: HSim_Phys.ManagedArticulatedObject, +) -> mn.Range3D: + """ + Get the local bounding box of all links of an articulated object in the root frame. + + :param ao: The ArticulatedObject instance. + """ + + # NOTE: we'd like to use SceneNode AABB, but this won't work because the links are not in the subtree of the root: + # ao.root_scene_node.compute_cumulative_bb() + + ao_local_part_bb_corners = [] + + link_nodes = [ao.get_link_scene_node(ix) for ix in range(-1, ao.num_links)] + for link_node in link_nodes: + local_bb_corners = get_bb_corners(link_node.cumulative_bb) + global_bb_corners = [ + link_node.absolute_transformation().transform_point(bb_corner) + for bb_corner in local_bb_corners + ] + ao_local_bb_corners = [ + ao.transformation.inverted().transform_point(p) for p in global_bb_corners + ] + ao_local_part_bb_corners.extend(ao_local_bb_corners) + + # get min and max of each dimension + # TODO: use numpy arrays for more elegance... + max_vec = mn.Vector3(ao_local_part_bb_corners[0]) + min_vec = mn.Vector3(ao_local_part_bb_corners[0]) + for point in ao_local_part_bb_corners: + for dim in range(3): + max_vec[dim] = max(max_vec[dim], point[dim]) + min_vec[dim] = min(min_vec[dim], point[dim]) + return mn.Range3D(min_vec, max_vec) + + +def get_ao_default_link( + ao: habitat_sim.physics.ManagedArticulatedObject, + compute_if_not_found: bool = False, +) -> Optional[int]: + """ + Get the "default" link index for a ManagedArticulatedObject. + The "default" link is the one link which should be used if only one joint can be actuated. For example, the largest or most accessible drawer or door. + + :param ao: The ManagedArticulatedObject instance. + :param compute_if_not_found: If true, try to compute the default link if it isn't found. + :return: The default link index or None if not found. Cannot be base link (-1). + + The default link is determined by: + + - must be "prismatic" or "revolute" joint type + - first look in the metadata Configuration for an annotated link. + - (if compute_if_not_found) - if not annotated, it is programmatically computed from a heuristic. + + Default link heuristic: the link with the lowest Y value in the bounding box with appropriate joint type. + """ + + # first look in metadata + default_link = ao.user_attributes.get("default_link") + + if default_link is None and compute_if_not_found: + valid_joint_types = [ + habitat_sim.physics.JointType.Revolute, + habitat_sim.physics.JointType.Prismatic, + ] + lowest_link = None + lowest_y: int = None + # compute the default link + for link_id in ao.get_link_ids(): + if ao.get_link_joint_type(link_id) in valid_joint_types: + # use minimum global keypoint Y value + link_lowest_y = min( + get_articulated_link_global_keypoints(ao, link_id), + key=lambda x: x[1], + )[1] + if lowest_y is None or link_lowest_y < lowest_y: + lowest_y = link_lowest_y + lowest_link = link_id + if lowest_link is not None: + default_link = lowest_link + # if found, set in metadata for next time + ao.user_attributes.set("default_link", default_link) + + return default_link + + +def get_ao_link_id_map(sim: habitat_sim.Simulator) -> Dict[int, int]: + """ + Construct a dict mapping ArticulatedLink object_id to parent ArticulatedObject object_id. + NOTE: also maps ao's root object id to itself for ease of use. + + :param sim: The Simulator instance. + + :return: dict mapping ArticulatedLink object ids to parent object ids. + """ + + aom = sim.get_articulated_object_manager() + ao_link_map: Dict[int, int] = {} + for ao in aom.get_objects_by_handle_substring().values(): + # add the ao itself for ease of use + ao_link_map[ao.object_id] = ao.object_id + # add the links + for link_id in ao.link_object_ids: + ao_link_map[link_id] = ao.object_id + + return ao_link_map + + +def get_global_keypoints_from_bb( + aabb: mn.Range3D, local_to_global: mn.Matrix4 +) -> List[mn.Vector3]: + """ + Get a list of bounding box keypoints in global space. + 0th point is the bounding box center, others are bounding box corners. + + :param aabb: The local bounding box. + :param local_to_global: The local to global transformation matrix. + + :return: A set of global 3D keypoints for the bounding box. + """ + local_keypoints = [aabb.center()] + local_keypoints.extend(get_bb_corners(aabb)) + global_keypoints = [ + local_to_global.transform_point(key_point) for key_point in local_keypoints + ] + return global_keypoints + + +def get_articulated_link_global_keypoints( + object_a: habitat_sim.physics.ManagedArticulatedObject, link_index: int +) -> List[mn.Vector3]: + """ + Get global bb keypoints for an ArticulatedLink. + + :param object_a: The parent ManagedArticulatedObject for the link. + :param link_index: The local index of the link within the parent ArticulatedObject. Not the object_id of the link. + + :return: A set of global 3D keypoints for the link. + """ + link_node = object_a.get_link_scene_node(link_index) + + return get_global_keypoints_from_bb( + link_node.cumulative_bb, link_node.absolute_transformation() + ) + + +def get_obj_from_id( + sim: habitat_sim.Simulator, + obj_id: int, + ao_link_map: Optional[Dict[int, int]] = None, +) -> Union[HSim_Phys.ManagedRigidObject, HSim_Phys.ManagedArticulatedObject,]: + """ + Get a ManagedRigidObject or ManagedArticulatedObject from an object_id. + + ArticulatedLink object_ids will return the ManagedArticulatedObject. + If you want link id, use ManagedArticulatedObject.link_object_ids[obj_id]. + + :param sim: The Simulator instance. + :param obj_id: object id for which ManagedObject is desired. + :param ao_link_map: A pre-computed map from link object ids to their parent ArticulatedObject's object id. + + :return: a ManagedObject or None + """ + + if ao_link_map is None: + # Note: better to pre-compute this and pass it around + ao_link_map = get_ao_link_id_map(sim) + + aom = sim.get_articulated_object_manager() + if obj_id in ao_link_map: + return aom.get_object_by_id(ao_link_map[obj_id]) + + rom = sim.get_rigid_object_manager() + if rom.get_library_has_id(obj_id): + return rom.get_object_by_id(obj_id) + + return None + + +def get_obj_from_handle( + sim: habitat_sim.Simulator, obj_handle: str +) -> Union[HSim_Phys.ManagedRigidObject, HSim_Phys.ManagedArticulatedObject,]: + """ + Get a ManagedRigidObject or ManagedArticulatedObject from its instance handle. + + :param sim: The Simulator instance. + :param obj_handle: object instance handle for which ManagedObject is desired. + + :return: a ManagedObject or None + """ + aom = sim.get_articulated_object_manager() + if aom.get_library_has_handle(obj_handle): + return aom.get_object_by_handle(obj_handle) + + rom = sim.get_rigid_object_manager() + if rom.get_library_has_handle(obj_handle): + return rom.get_object_by_handle(obj_handle) + + return None + + +def get_all_ao_objects( + sim: habitat_sim.Simulator, +) -> List[HSim_Phys.ManagedArticulatedObject]: + """ + Get a list of all ManagedArticulatedObjects in the scene. + + :param sim: The Simulator instance. + + :return: a list of ManagedObject wrapper instances containing all articulated objects currently instantiated in the scene. + """ + return ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().values() + ) + + +def get_all_rigid_objects( + sim: habitat_sim.Simulator, +) -> List[HSim_Phys.ManagedArticulatedObject]: + """ + Get a list of all ManagedRigidObjects in the scene. + + :param sim: The Simulator instance. + + :return: a list of ManagedObject wrapper instances containing all rigid objects currently instantiated in the scene. + """ + return sim.get_rigid_object_manager().get_objects_by_handle_substring().values() + + +def get_all_objects( + sim: habitat_sim.Simulator, +) -> List[Union[HSim_Phys.ManagedRigidObject, HSim_Phys.ManagedArticulatedObject,]]: + """ + Get a list of all ManagedRigidObjects and ManagedArticulatedObjects in the scene. + + :param sim: The Simulator instance. + + :return: a list of ManagedObject wrapper instances containing all objects currently instantiated in the scene. + """ + + managers = [ + sim.get_rigid_object_manager(), + sim.get_articulated_object_manager(), + ] + all_objects = [] + for mngr in managers: + all_objects.extend(mngr.get_objects_by_handle_substring().values()) + return all_objects diff --git a/src_python/habitat_sim/utils/sim_utils.py b/src_python/habitat_sim/utils/sim_utils.py new file mode 100644 index 0000000000..79c34592f9 --- /dev/null +++ b/src_python/habitat_sim/utils/sim_utils.py @@ -0,0 +1,421 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +import os +from typing import Callable, List + +import magnum as mn +import numpy as np +from habitat.articulated_agents.robots.spot_robot import SpotRobot +from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index +from omegaconf import DictConfig + +import habitat_sim +from habitat_sim import physics as HSim_Phys + + +# Class to instantiate and maneuver spot from a viewer +# DEPENDENT ON HABITAT-LAB - +class SpotAgent: + SPOT_DIR = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" + if not os.path.isfile(SPOT_DIR): + # support other layout + SPOT_DIR = "data/scene_datasets/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" + + class ExtractedBaseVelNonCylinderAction: + def __init__(self, sim, spot): + self._sim = sim + self.spot = spot + self.base_vel_ctrl = HSim_Phys.VelocityControl() + self.base_vel_ctrl.controlling_lin_vel = True + self.base_vel_ctrl.lin_vel_is_local = True + self.base_vel_ctrl.controlling_ang_vel = True + self.base_vel_ctrl.ang_vel_is_local = True + self._allow_dyn_slide = True + self._allow_back = True + self._longitudinal_lin_speed = 10.0 + self._lateral_lin_speed = 10.0 + self._ang_speed = 10.0 + self._navmesh_offset = [[0.0, 0.0], [0.25, 0.0], [-0.25, 0.0]] + self._enable_lateral_move = True + self._collision_threshold = 1e-5 + self._noclip = False + # If we just changed from noclip to clip - make sure + # that if spot is not on the navmesh he gets snapped to it + self._transition_to_clip = False + + def collision_check( + self, trans, target_trans, target_rigid_state, compute_sliding + ): + """ + trans: the transformation of the current location of the robot + target_trans: the transformation of the target location of the robot given the center original Navmesh + target_rigid_state: the target state of the robot given the center original Navmesh + compute_sliding: if we want to compute sliding or not + """ + + def step_spot( + num_check_cylinder: int, + cur_height: float, + pos_calc: Callable[[np.ndarray, np.ndarray], np.ndarray], + cur_pos: List[np.ndarray], + goal_pos: List[np.ndarray], + ): + end_pos = [] + for i in range(num_check_cylinder): + pos = pos_calc(cur_pos[i], goal_pos[i]) + # Sanitize the height + pos[1] = cur_height + cur_pos[i][1] = cur_height + goal_pos[i][1] = cur_height + end_pos.append(pos) + return end_pos + + # Get the offset positions + num_check_cylinder = len(self._navmesh_offset) + nav_pos_3d = [np.array([xz[0], 0.0, xz[1]]) for xz in self._navmesh_offset] + cur_pos: List[np.ndarray] = [ + trans.transform_point(xyz) for xyz in nav_pos_3d + ] + goal_pos: List[np.ndarray] = [ + target_trans.transform_point(xyz) for xyz in nav_pos_3d + ] + + # For step filter of offset positions + end_pos = [] + + no_filter_step = lambda _, val: val + if self._noclip: + cur_height = self.spot.base_pos[1] + # ignore navmesh + end_pos = step_spot( + num_check_cylinder=num_check_cylinder, + cur_height=cur_height, + pos_calc=no_filter_step, + cur_pos=cur_pos, + goal_pos=goal_pos, + ) + else: + cur_height = self._sim.pathfinder.snap_point(self.spot.base_pos)[1] + # constrain to navmesh + end_pos = step_spot( + num_check_cylinder=num_check_cylinder, + cur_height=cur_height, + pos_calc=self._sim.step_filter, + cur_pos=cur_pos, + goal_pos=goal_pos, + ) + + # Planar move distance clamped by NavMesh + move = [] + for i in range(num_check_cylinder): + move.append((end_pos[i] - goal_pos[i]).length()) + + # For detection of linear or angualr velocities + # There is a collision if the difference between the clamped NavMesh position and target position is too great for any point. + diff = len([v for v in move if v > self._collision_threshold]) + + if diff > 0: + # Wrap the move direction if we use sliding + # Find the largest diff moving direction, which means that there is a collision in that cylinder + if compute_sliding: + max_idx = np.argmax(move) + move_vec = end_pos[max_idx] - cur_pos[max_idx] + new_end_pos = trans.translation + move_vec + return True, mn.Matrix4.from_( + target_rigid_state.rotation.to_matrix(), new_end_pos + ) + return True, trans + else: + return False, target_trans + + def update_base(self, if_rotation): + """ + Update the base of the robot + if_rotation: if the robot is rotating or not + """ + # Get the control frequency + inv_ctrl_freq = 1.0 / 60.0 + # Get the current transformation + trans = self.spot.sim_obj.transformation + # Get the current rigid state + rigid_state = habitat_sim.RigidState( + mn.Quaternion.from_matrix(trans.rotation()), trans.translation + ) + # Integrate to get target rigid state + target_rigid_state = self.base_vel_ctrl.integrate_transform( + inv_ctrl_freq, rigid_state + ) + # Get the traget transformation based on the target rigid state + target_trans = mn.Matrix4.from_( + target_rigid_state.rotation.to_matrix(), + target_rigid_state.translation, + ) + # We do sliding only if we allow the robot to do sliding and current + # robot is not rotating + compute_sliding = self._allow_dyn_slide and not if_rotation + # Check if there is a collision + did_coll, new_target_trans = self.collision_check( + trans, target_trans, target_rigid_state, compute_sliding + ) + # Update the base + self.spot.sim_obj.transformation = new_target_trans + + if self.spot._base_type == "leg": + # Fix the leg joints + self.spot.leg_joint_pos = self.spot.params.leg_init_params + + def toggle_clip(self, largest_island_ix: int): + """ + Handle transition to/from no clipping/navmesh disengaged. + """ + # Transitioning to clip from no clip or vice versa + self._transition_to_clip = self._noclip + self._noclip = not self._noclip + + spot_cur_point = self.spot.base_pos + # Find reasonable location to return to navmesh + if self._transition_to_clip and not self._sim.pathfinder.is_navigable( + spot_cur_point + ): + # Clear transition flag - only transition once + self._transition_to_clip = False + # Find closest point on navmesh to snap spot to + print( + f"Trying to find closest navmesh point to spot_cur_point: {spot_cur_point}" + ) + new_point = self._sim.pathfinder.snap_point( + spot_cur_point, largest_island_ix + ) + if not np.any(np.isnan(new_point)): + print( + f"Closest navmesh point to spot_cur_point: {spot_cur_point} is {new_point} on largest island {largest_island_ix}. Snapping to it." + ) + # Move spot to this point + self.spot.base_pos = new_point + else: + # try again to any island + new_point = self._sim.pathfinder.snap_point(spot_cur_point) + if not np.any(np.isnan(new_point)): + print( + f"Closest navmesh point to spot_cur_point: {spot_cur_point} is {new_point} not on largest island. Snapping to it." + ) + # Move spot to this point + self.spot.base_pos = new_point + else: + print( + "Unable to leave no-clip mode, too far from navmesh. Try again when closer." + ) + self._noclip = True + return self._noclip + + def step(self, forward, lateral, angular): + """ + provide forward, lateral, and angular velocities as [-1,1]. + """ + longitudinal_lin_vel = forward + lateral_lin_vel = lateral + ang_vel = angular + longitudinal_lin_vel = ( + np.clip(longitudinal_lin_vel, -1, 1) * self._longitudinal_lin_speed + ) + lateral_lin_vel = np.clip(lateral_lin_vel, -1, 1) * self._lateral_lin_speed + ang_vel = np.clip(ang_vel, -1, 1) * self._ang_speed + if not self._allow_back: + longitudinal_lin_vel = np.maximum(longitudinal_lin_vel, 0) + + self.base_vel_ctrl.linear_velocity = mn.Vector3( + longitudinal_lin_vel, 0, -lateral_lin_vel + ) + self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0) + + if longitudinal_lin_vel != 0.0 or lateral_lin_vel != 0.0 or ang_vel != 0.0: + self.update_base(ang_vel != 0.0) + + def __init__(self, sim: habitat_sim.Simulator): + self.sim = sim + # changed when spot is put on navmesh + self.largest_island_ix = -1 + self.spot_forward = 0.0 + self.spot_lateral = 0.0 + self.spot_angular = 0.0 + self.load_and_init() + # angle and azimuth of camera orientation + self.camera_angles = mn.Vector2() + self.init_spot_cam() + + self.spot_rigid_state = self.spot.sim_obj.rigid_state + self.spot_motion_type = self.spot.sim_obj.motion_type + + def load_and_init(self): + # add the robot to the world via the wrapper + robot_path = SpotAgent.SPOT_DIR + agent_config = DictConfig({"articulated_agent_urdf": robot_path}) + self.spot: SpotRobot = SpotRobot(agent_config, self.sim, fixed_base=True) + self.spot.reconfigure() + self.spot.update() + self.spot_action: SpotAgent.ExtractedBaseVelNonCylinderAction = ( + SpotAgent.ExtractedBaseVelNonCylinderAction(self.sim, self.spot) + ) + + def init_spot_cam(self): + # Camera relative to spot + self.camera_distance = 2.0 + # height above spot to target lookat + self.lookat_height = 0.0 + + def mod_spot_cam( + self, + scroll_mod_val: float = 0, + mse_rel_pos: List = None, + shift_pressed: bool = False, + alt_pressed: bool = False, + ): + """ + Modify the camera agent's orientation, distance and lookat target relative to spot via UI input + """ + # use shift for fine-grained zooming + if scroll_mod_val != 0: + mod_val = 0.3 if shift_pressed else 0.15 + scroll_delta = scroll_mod_val * mod_val + if alt_pressed: + # lookat going up and down + self.lookat_height -= scroll_delta + else: + self.camera_distance -= scroll_delta + if mse_rel_pos is not None: + self.camera_angles[0] -= mse_rel_pos[1] * 0.01 + self.camera_angles[1] -= mse_rel_pos[0] * 0.01 + self.camera_angles[0] = max(-1.55, min(0.5, self.camera_angles[0])) + self.camera_angles[1] = np.fmod(self.camera_angles[1], np.pi * 2.0) + + def set_agent_camera_transform(self, agent_node): + # set camera agent position relative to spot + x_rot = mn.Quaternion.rotation( + mn.Rad(self.camera_angles[0]), mn.Vector3(1, 0, 0) + ) + y_rot = mn.Quaternion.rotation( + mn.Rad(self.camera_angles[1]), mn.Vector3(0, 1, 0) + ) + local_camera_vec = mn.Vector3(0, 0, 1) + local_camera_position = y_rot.transform_vector( + x_rot.transform_vector(local_camera_vec * self.camera_distance) + ) + spot_pos = self.base_pos() + lookat_disp = mn.Vector3(0, self.lookat_height, 0) + lookat_pos = spot_pos + lookat_disp + camera_position = local_camera_position + lookat_pos + agent_node.transformation = mn.Matrix4.look_at( + camera_position, + lookat_pos, + mn.Vector3(0, 1, 0), + ) + + def base_pos(self): + return self.spot.base_pos + + def place_on_navmesh(self): + if self.sim.pathfinder.is_loaded: + self.largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + print(f"Largest indoor island index = {self.largest_island_ix}") + valid_spot_point = None + max_attempts = 1000 + attempt = 0 + while valid_spot_point is None and attempt < max_attempts: + spot_point = self.sim.pathfinder.get_random_navigable_point( + island_index=self.largest_island_ix + ) + if self.sim.pathfinder.distance_to_closest_obstacle(spot_point) >= 0.25: + valid_spot_point = spot_point + attempt += 1 + if valid_spot_point is not None: + self.spot.base_pos = valid_spot_point + else: + print( + f"Unable to find a valid spot for Spot on the navmesh after {max_attempts} attempts" + ) + + def toggle_clip(self): + # attempt to turn on or off noclip + clipstate = self.spot_action.toggle_clip(self.largest_island_ix) + + # Turn off dynamics if spot is being moved kinematically + # self.spot.sim_obj.motion_type = HSim_MT.KINEMATIC if clipstate else self.spot_motion_type + print(f"After toggle, clipstate is {clipstate}") + + def move_spot( + self, + move_fwd: bool, + move_back: bool, + move_up: bool, + move_down: bool, + slide_left: bool, + slide_right: bool, + turn_left: bool, + turn_right: bool, + ): + inc = 0.02 + min_val = 0.1 + + if move_fwd and not move_back: + self.spot_forward = max(min_val, self.spot_forward + inc) + elif move_back and not move_fwd: + self.spot_forward = min(-min_val, self.spot_forward - inc) + else: + self.spot_forward *= 0.5 + if abs(self.spot_forward) < min_val: + self.spot_forward = 0 + + if slide_left and not slide_right: + self.spot_lateral = max(min_val, self.spot_lateral + inc) + elif slide_right and not slide_left: + self.spot_lateral = min(-min_val, self.spot_lateral - inc) + else: + self.spot_lateral *= 0.5 + if abs(self.spot_lateral) < min_val: + self.spot_lateral = 0 + + if turn_left and not turn_right: + self.spot_angular = max(min_val, self.spot_angular + inc) + elif turn_right and not turn_left: + self.spot_angular = min(-min_val, self.spot_angular - inc) + else: + self.spot_angular *= 0.5 + if abs(self.spot_angular) < min_val: + self.spot_angular = 0 + + self.spot_action.step( + forward=self.spot_forward, + lateral=self.spot_lateral, + angular=self.spot_angular, + ) + + def cache_transform_and_remove(self): + """ + Save spot's current location and remove from scene, for saving scene instance. + """ + aom = self.sim.get_articulated_object_manager() + self.spot_rigid_state = self.spot.sim_obj.rigid_state + aom.remove_object_by_id(self.spot.sim_obj.object_id) + + def restore_at_previous_loc(self): + """ + Reload spot and restore from saved location. + """ + # rebuild spot + self.load_and_init() + # put em back + self.spot.sim_obj.rigid_state = self.spot_rigid_state + + def get_point_in_front(self, disp_in_front: mn.Vector3 = None): + if disp_in_front is None: + disp_in_front = [1.5, 0.0, 0.0] + return self.spot.base_transformation.transform_point(disp_in_front)