--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

@@ -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.

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:
 -------------

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,
+                    )