unoccluded navmesh snap multiple ignores (#1943)

* refactor navmesh_utils to allow a list of target_object_ids and correct some typos

* refactor downstream code to pass a list instead of a single int. Does not semantically address the expansion to multiple ids (e.g. for AOs).

habitat-lab/habitat/datasets/rearrange/navmesh_utils.py

@@ -16,7 +16,7 @@ def snap_point_is_occluded(
     height: float,
     sim: habitat_sim.Simulator,
     granularity: float = 0.2,
-    target_object_id: Optional[int] = None,
+    target_object_ids: Optional[List[int]] = None,
 ) -> bool:
     """
     Uses raycasting to check whether a target is occluded given a navmesh snap point.
@@ -26,9 +26,9 @@ def snap_point_is_occluded(
     :property height: The height of the agent above the navmesh. Assumes the navmesh snap point is on the ground. Should be the maximum relative distance from navmesh ground to which a visibility check should indicate non-occlusion. The first check starts from this height. (E.g. agent_eyes_y - agent_base_y)
     :property sim: The Simulator instance.
     :property granularity: The distance between raycast samples. Finer granularity is more accurate, but more expensive.
-    :property target_object_id: An optional object id which should be ignored in occlusion check.
+    :property target_object_ids: An optional set of object ids which should be ignored in occlusion check.
 
-    NOTE: If agent's eye height is known and only that height should be considered, provide eye height and granulatiry > height for fastest check.
+    NOTE: If agent's eye height is known and only that height should be considered, provide eye height and granularity > height for fastest check.
 
     :return: whether or not the target is considered occluded from the snap_point.
     """
@@ -46,8 +46,8 @@ def snap_point_is_occluded(
             and raycast_results.hits[0].ray_distance < 1
         ):
             if (
-                target_object_id is not None
-                and raycast_results.hits[0].object_id == target_object_id
+                target_object_ids is not None
+                and raycast_results.hits[0].object_id in target_object_ids
             ):
                 # we hit an allowed object (i.e., the target object), so not occluded
                 return False
@@ -64,7 +64,7 @@ def unoccluded_navmesh_snap(
     height: float,
     pathfinder: habitat_sim.nav.PathFinder,
     sim: habitat_sim.Simulator,
-    target_object_id: Optional[int] = None,
+    target_object_ids: Optional[List[int]] = None,
     island_id: int = -1,
     search_offset: float = 1.5,
     test_batch_size: int = 20,
@@ -78,7 +78,7 @@ def unoccluded_navmesh_snap(
     :property height: The height of the agent above the navmesh. Assumes the navmesh snap point is on the ground. Should be the maximum relative distance from navmesh ground to which a visibility check should indicate non-occlusion. The first check starts from this height. (E.g. agent_eyes_y - agent_base_y)
     :property pathfinder: The PathFinder defining the NavMesh to use.
     :property sim: The Simulator instance.
-    :property target_object_id: An optional object_id which should be ignored in the occlusion check. For example, when pos is an object's COM, that object should not occlude the point.
+    :property target_object_ids: An optional set of object ids which should be ignored in occlusion check. For example, when pos is an object's COM, that object should not occlude the point.
     :property island_id: Optionally restrict the search to a single navmesh island. Default -1 is the full navmesh.
     :property search_offset: The additional radius to search for navmesh points around the target position. Added to the minimum distance from pos to navmesh.
     :property test_batch_size: The number of sample navmesh points to consider when testing for occlusion.
@@ -98,7 +98,7 @@ def unoccluded_navmesh_snap(
         snap_point=snap_point,
         height=height,
         sim=sim,
-        target_object_id=target_object_id,
+        target_object_ids=target_object_ids,
     )
 
     # now sample and try different snap options
@@ -140,7 +140,7 @@ def unoccluded_navmesh_snap(
                 batch_sample[0],
                 height,
                 sim,
-                target_object_id=target_object_id,
+                target_object_ids=target_object_ids,
             ):
                 return batch_sample[0]
 
@@ -275,13 +275,13 @@ def record_robot_nav_debug_image(
     obs_cache: List[Any],
 ) -> None:
     """
-    Render a single frame 3rd person view of the robot embodiement approximation following a path with DebugVizualizer and cache it in obs_cache.
+    Render a single frame 3rd person view of the robot embodiment approximation following a path with DebugVizualizer and cache it in obs_cache.
 
     :param curr_path_points: List of current path points.
     :param robot_transformation: Current transformation of the robot.
-    :param robot_navmesh_offsets: Robot embodiement approximation. List of 2D points XZ in robot local space.
-    :param robot_navmesh_radius: The radius of each point approximating the robot embodiement.
-    :param in_collision: Whether or not the robot is in collision with the environment. If so, embodiement is rendered red.
+    :param robot_navmesh_offsets: Robot embodiment approximation. List of 2D points XZ in robot local space.
+    :param robot_navmesh_radius: The radius of each point approximating the robot embodiment.
+    :param in_collision: Whether or not the robot is in collision with the environment. If so, embodiment is rendered red.
     :param dbv: The DebugVisualizer instance.
     :param obs_cache: The observation cache for later video rendering.
     """
@@ -298,12 +298,12 @@ def record_robot_nav_debug_image(
             )
     dbv.render_debug_lines(debug_lines=path_point_render_lines)
 
-    # draw the local coordinate axis ofthe robot
+    # draw the local coordinate axis of the robot
     dbv.render_debug_frame(
         axis_length=0.3, transformation=robot_transformation
     )
 
-    # render the robot embodiement
+    # render the robot embodiment
     nav_pos_3d = [
         np.array([xz[0], 0.0, xz[1]]) for xz in robot_navmesh_offsets
     ]
@@ -353,7 +353,7 @@ def path_is_navigable_given_robot(
     """
     Compute the ratio of time-steps for which there were collisions detected while the robot navigated from start_pos to goal_pos given the configuration of the sim navmesh.
 
-    :param sim: Habitat Simulaton instance.
+    :param sim: Habitat Simulator instance.
     :param start_pos: Initial translation of the robot's transform. The start of the navigation path.
     :param goal_pos: Target translation of the robot's transform. The end of the navigation path.
     :param robot_navmesh_offsets: The list of 2D points XZ in robot local space which will be used represent the robot's shape. Used to query the navmesh for navigability as a collision heuristic.
@@ -522,19 +522,17 @@ def is_accessible(
     height: float,
     nav_to_min_distance: float,
     nav_island: int = -1,
-    target_object_id: Optional[int] = None,
+    target_object_ids: Optional[List[int]] = None,
 ) -> bool:
     """
     Return True if the point is within a threshold distance (in XZ plane) of the nearest unoccluded navigable point on the selected island.
 
-    :param sim: Habitat Simulaton instance.
+    :param sim: Habitat Simulator instance.
     :param point: The query point.
     :property height: The height of the agent. Given navmesh snap point is grounded, the maximum height from which a visibility check should indicate non-occlusion. First check starts from this height.
-    :param nav_to_min_distance: Minimum distance threshold. -1 opts out of the test and returns True (i.e. no minumum distance).
+    :param nav_to_min_distance: Minimum distance threshold. -1 opts out of the test and returns True (i.e. no minimum distance).
     :param nav_island: The NavMesh island on which to check accessibility. Default -1 is the full NavMesh.
-    :param target_object_id: An optional object_id which should be ignored in the occlusion check. For example, when checking accessibility of an object's COM, that object should not occlude.
-
-    TODO: target_object_id should be a list to correctly support ArticulatedObjects (e.g. the fridge body should not occlude the fridge drawer for this check.)
+    :param target_object_id: An optional set of object ids which should be ignored in occlusion check. For example, when checking accessibility of an object's COM, that object should not occlude.
 
     :return: Whether or not the point is accessible.
     """
@@ -546,7 +544,7 @@ def is_accessible(
         height=height,
         pathfinder=sim.pathfinder,
         sim=sim,
-        target_object_id=target_object_id,
+        target_object_ids=target_object_ids,
         island_id=nav_island,
         search_offset=nav_to_min_distance,
     )

habitat-lab/habitat/datasets/rearrange/samplers/object_sampler.py

@@ -339,7 +339,7 @@ def sample_placement(
                         height=1.3,
                         nav_to_min_distance=self.nav_to_min_distance,
                         nav_island=self.largest_island_id,
-                        target_object_id=new_object.object_id,
+                        target_object_ids=[new_object.object_id],
                     ):
                         logger.info(
                             "   - object is not accessible from navmesh, rejecting placement."
@@ -358,7 +358,7 @@ def sample_placement(
                     height=1.3,
                     nav_to_min_distance=self.nav_to_min_distance,
                     nav_island=self.largest_island_id,
-                    target_object_id=new_object.object_id,
+                    target_object_ids=[new_object.object_id],
                 ):
                     logger.info(
                         "   - object is not accessible from navmesh, rejecting placement."

habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py

@@ -1219,7 +1219,7 @@ def get_navigable_receptacles(
                     height=max_access_height,
                     nav_to_min_distance=nav_to_min_distance,
                     nav_island=nav_island,
-                    target_object_id=receptacle_obj.object_id,
+                    target_object_ids=[receptacle_obj.object_id],
                 )
                 for point in recep_points
             )