From 4983d7975831e1329a1df010e1f3ece20d0aec57 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Mon, 17 Jun 2024 12:20:32 -0400 Subject: [PATCH] --add contact and region display to spot viewer Also move region/semantic display utility to sim_utils for eventual expansion to other semantic debug displays --- examples/spot_viewer.py | 93 ++++++++++++----------- examples/viewer.py | 61 +++------------ src_python/habitat_sim/utils/sim_utils.py | 64 +++++++++++++++- 3 files changed, 125 insertions(+), 93 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 11ee72bd22..e3ba4cfd4a 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -24,7 +24,12 @@ from habitat_sim import ReplayRenderer, ReplayRendererConfiguration from habitat_sim.logging import LoggingContext, logger from habitat_sim.utils.settings import default_sim_settings, make_cfg -from habitat_sim.utils.sim_utils import ObjectEditor, SpotAgent, get_obj_from_id +from habitat_sim.utils.sim_utils import ( + ObjectEditor, + SemanticManager, + SpotAgent, + get_obj_from_id, +) class HabitatSimInteractiveViewer(Application): @@ -184,6 +189,9 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # Editing self.obj_editor = ObjectEditor(self.sim) + # Semantics + self.dbg_semantics = SemanticManager(self.sim) + # create spot right after reconfigure self.spot_agent = SpotAgent(self.sim) # set for spot's radius @@ -299,6 +307,9 @@ def debug_draw(self): 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, @@ -611,9 +622,9 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.navmesh_dirty, toggle=alt_pressed ) - elif key == pressed.BACKSPACE or key == pressed.C: + elif key == pressed.BACKSPACE or key == pressed.Y: if self.selected_object is not None: - if key == pressed.C: + if key == pressed.Y: obj_name = self.selected_object.handle.split("/")[-1].split("_:")[0] self.removed_clutter.append(obj_name) print(f"Removed {self.selected_object.handle}") @@ -631,9 +642,27 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.obj_editor.set_sel_obj(self.selected_object) 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.E: + # Cyle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + elif key == pressed.G: + # cycle through edit modes self.obj_editor.change_edit_mode(toggle=shift_pressed) elif key == pressed.I: @@ -701,35 +730,8 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.obj_editor.undo_edit() elif key == pressed.V: - # # inject a new AO by handle substring in front of the agent - - # # get user input - # ao_substring = input( - # "Load ArticulatedObject. Enter an AO handle substring, first match will be added:" - # ).strip() - - # aotm = self.sim.metadata_mediator.ao_template_manager - # aom = self.sim.get_articulated_object_manager() - # ao_handles = aotm.get_template_handles(ao_substring) - # if len(ao_handles) == 0: - # print(f"No AO found matching substring: '{ao_substring}'") - # return - # elif len(ao_handles) > 1: - # print(f"Multiple AOs found matching substring: '{ao_substring}'.") - # matching_ao_handle = ao_handles[0] - # print(f"Adding AO: '{matching_ao_handle}'") - # aot = aotm.get_template_by_handle(matching_ao_handle) - # aot.base_type = "FIXED" - # aotm.register_template(aot) - # ao = aom.add_articulated_object_by_template_handle(matching_ao_handle) - # if ao is not None: - # recompute_ao_bbs(ao) - # in_front_of_spot = self.spot_agent.get_point_in_front( - # disp_in_front=[1.5, 0.0, 0.0] - # ) - # ao.translation = in_front_of_spot - # else: - # print("Failed to load AO.") + # inject a new object by handle substring in front of the agent + # or else using the currently selected object # press shift if we want to load if no object selected new_obj, base_transformation = self.obj_editor.build_object( @@ -991,17 +993,17 @@ def print_help_text(self) -> None: Scene Object Modification UI: '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 Move Object mode is selected : - - LEFT/RIGHT arrow keys: move the object along global X axis. - - UP/DOWN arrow keys: move the object along global Z axis. - (+ALT): move the object up/down (global Y axis) - When Rotate Object mode is selected : - - LEFT/RIGHT arrow keys: rotate the object around global Y axis. - - UP/DOWN arrow keys: rotate the object around global Z axis. - (+ALT): rotate the object around global X axis. - - BACKSPACE: delete the selected object - - 'c': delete the selected object and record it as clutter. + - With an object selected: + When Move Object Edit mode is selected : + - LEFT/RIGHT arrow keys: move the object along global X axis. + - UP/DOWN arrow keys: move the object along global Z axis. + (+ALT): move the object up/down (global Y axis) + When Rotate Object Edit mode is selected : + - LEFT/RIGHT arrow keys: rotate the object around global Y axis. + - UP/DOWN arrow keys: rotate the object around global Z axis. + (+ALT): rotate the object around global X axis. + - BACKSPACE: delete the selected object + - 'y': delete the selected object and record it as clutter. '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. @@ -1011,6 +1013,11 @@ def print_help_text(self) -> None: (+SHIFT) Recompute NavMesh with Spot settings (already done). (+ALT) Re-sample Spot's position 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). + 'e' Toggle Semantic visualization bounds (currently only Semantic Region annotations) + Object Interactions: SPACE: Toggle physics simulation on/off. diff --git a/examples/viewer.py b/examples/viewer.py index cb5b1a41b5..b3e91987b2 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -38,6 +38,7 @@ from habitat_sim.utils.sim_utils import ( MarkerSetsInfo, ObjectEditor, + SemanticManager, get_all_ao_objects, get_bb_corners, get_obj_from_handle, @@ -225,8 +226,6 @@ def __init__( # variables that track app data and CPU/GPU usage self.num_frames_to_track = 60 - # Descriptive strings for semantic region debug draw possible choices - self.semantic_region_debug_draw_choices = ["None", "Kitchen Only", "All"] global _cpo self._cpo = _cpo @@ -246,10 +245,6 @@ def __init__( self.debug_bullet_draw = False # draw active contact point debug line visualizations self.contact_debug_draw = False - # 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 = {} # cache most recently loaded URDF file for quick-reload self.cached_urdf = "" @@ -312,6 +307,9 @@ def __init__( # Editing self.obj_editor = ObjectEditor(self.sim) + # Semantics + self.dbg_semantics = SemanticManager(self.sim) + # sys.exit(0) # load appropriate filter file for scene self.load_scene_filter_file() @@ -651,32 +649,6 @@ def draw_contact_debug(self, debug_line_render: Any): normal=camera_position - cp.position_on_b_in_ws, ) - def draw_region_debug(self, debug_line_render: Any) -> None: - """ - Draw the semantic region wireframes. - """ - 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, - ) - def draw_receptacles(self, debug_line_render): if self.rec_filter_data is None and self.cpo_initialized: self.compute_rec_filter_state( @@ -830,14 +802,10 @@ def debug_draw(self): if self.contact_debug_draw: self.draw_contact_debug(debug_line_render) - if self.semantic_region_debug_draw_state != 0: - 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)) - ) - self.draw_region_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, @@ -1350,14 +1318,9 @@ def key_press_event(self, event: Application.KeyEvent) -> None: logger.info(log_str) elif key == pressed.E: - new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( - self.semantic_region_debug_draw_choices - ) - logger.info( - 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 + # 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 @@ -2178,7 +2141,7 @@ def print_help_text(self) -> None: - '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 + - 'u': delete the selected object Key Commands: ------------- diff --git a/src_python/habitat_sim/utils/sim_utils.py b/src_python/habitat_sim/utils/sim_utils.py index 5680706afc..d9d7efc3ee 100644 --- a/src_python/habitat_sim/utils/sim_utils.py +++ b/src_python/habitat_sim/utils/sim_utils.py @@ -838,7 +838,7 @@ def build_object(self, shift_pressed: bool): print( f"No distinct Rigid or Articulated Object handle found matching substring: '{obj_substring}'" ) - return None + return None, None attr_mgr = rotm obj_mgr = self.sim.get_rigid_object_manager() obj_temp_handle = ro_handles[0] @@ -850,6 +850,12 @@ def build_object(self, shift_pressed: bool): obj_temp_handle = ao_handles[0] build_ao = True base_motion_type = habitat_sim.physics.MotionType.STATIC + else: + # no object selected and no name input + print( + "No object was selected to copy and shift was not pressed so no known object handle was input. Aborting" + ) + return None, None # Build an object using obj_temp_handle, getting template from attr_mgr and object manager obj_mgr temp = attr_mgr.get_template_by_handle(obj_temp_handle) @@ -1353,3 +1359,59 @@ 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) + + +# Class to manage semantic interaction and display +class SemanticManager: + 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, + )