From 9ba9854e61c79537eae847756b0b4022b2762e13 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 30 Jan 2023 09:34:36 -0800
Subject: [PATCH 01/27] updates to rearrange episode generation to support mesh
 receptacles and improved debugging

---
 .../configs/all_receptacles_test.yaml         |  53 +++
 .../datasets/rearrange/rearrange_generator.py | 135 ++++++--
 .../rearrange/run_episode_generator.py        |   2 +
 .../rearrange/samplers/object_sampler.py      |  13 +
 .../datasets/rearrange/samplers/receptacle.py | 318 ++++++++++++++++--
 .../habitat_simulator/debug_visualizer.py     | 117 +++++--
 6 files changed, 547 insertions(+), 91 deletions(-)
 create mode 100644 habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml

diff --git a/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml b/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
new file mode 100644
index 0000000000..f7eb811c50
--- /dev/null
+++ b/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
@@ -0,0 +1,53 @@
+---
+## All receptacles and objects are used.
+# Define your own dataset path,
+#dataset_path: "data/replica_cad/replicaCAD.scene_dataset_config.json"
+dataset_path: "/run/media/alexclegg/Extreme_SSD/datasets/floorplanner/fp_mini_rec_automation/fp_mini.scene_dataset_config.json"
+additional_object_paths:
+  - "data/objects/ycb/configs/"
+correct_unstable_results: True
+scene_sets:
+  -
+    name: "all_scenes"
+    included_substrings:
+      - ""
+    excluded_substrings: ["NONE"]
+
+object_sets:
+  -
+    name: "simple_objects"
+    included_substrings:
+      - "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"
+    excluded_substrings: []
+receptacle_sets:
+  -
+    name: "all_receptacles"
+    included_object_substrings:
+      - ""
+    excluded_object_substrings: []
+    included_receptacle_substrings:
+      - ""
+    excluded_receptacle_substrings: []
+
+scene_sampler:
+  type: "subset"
+  params:
+    scene_sets: ["all_scenes"]
+
+object_samplers:
+  -
+    name: "simple_objects_sample"
+    type: "uniform"
+    params:
+      object_sets: ["simple_objects"]
+      receptacle_sets: ["all_receptacles"]
+      num_samples: [10, 200]
+      orientation_sampling: "up"
diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
index 853a61ffa0..3801f70579 100644
--- a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
@@ -5,6 +5,7 @@
 # LICENSE file in the root directory of this source tree.
 
 import os.path as osp
+import time
 from collections import defaultdict
 
 try:
@@ -26,6 +27,7 @@
 from habitat.datasets.rearrange.rearrange_dataset import RearrangeEpisode
 from habitat.datasets.rearrange.samplers.receptacle import (
     OnTopOfReceptacle,
+    Receptacle,
     ReceptacleSet,
     ReceptacleTracker,
     find_receptacles,
@@ -391,29 +393,15 @@ def generate_scene(self) -> str:
 
     def visualize_scene_receptacles(self) -> None:
         """
-        Generate a wireframe bounding box for each receptacle in the scene, aim the camera at it and record 1 observation.
+        Generate a debug line representation for each receptacle in the scene, aim the camera at it and record 1 observation.
         """
         logger.info("visualize_scene_receptacles processing")
         receptacles = find_receptacles(self.sim)
         for receptacle in receptacles:
             logger.info("receptacle processing")
-            viz_objects = receptacle.add_receptacle_visualization(self.sim)
-
-            # sample points in the receptacles to display
-            # for sample in range(25):
-            #     sample_point = receptacle.sample_uniform_global(self.sim, 1.0)
-            #     sutils.add_viz_sphere(self.sim, 0.025, sample_point)
-
-            if viz_objects:
-                # point the camera at the 1st viz_object for the Receptacle
-                self.vdb.look_at(
-                    viz_objects[0].root_scene_node.absolute_translation
-                )
-                self.vdb.get_observation()
-            else:
-                logger.warning(
-                    f"visualize_scene_receptacles: no visualization object generated for Receptacle '{receptacle.name}'."
-                )
+            receptacle.debug_draw(self.sim)
+            self.vdb.look_at(receptacle.sample_uniform_global(self.sim, 1.0))
+            self.vdb.get_observation()
 
     def generate_episodes(
         self, num_episodes: int = 1, verbose: bool = False
@@ -545,7 +533,7 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]:
             self.vdb.make_debug_video(prefix="receptacles_")
 
         # sample object placements
-        object_to_containing_receptacle = {}
+        self.object_to_containing_receptacle = {}
         for sampler_name, obj_sampler in self._obj_samplers.items():
             object_sample_data = obj_sampler.sample(
                 self.sim,
@@ -558,7 +546,7 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]:
                 return None
             new_objects, receptacles = zip(*object_sample_data)
             for obj, rec in zip(new_objects, receptacles):
-                object_to_containing_receptacle[obj.handle] = rec
+                self.object_to_containing_receptacle[obj.handle] = rec
             if sampler_name not in self.episode_data["sampled_objects"]:
                 self.episode_data["sampled_objects"][
                     sampler_name
@@ -574,9 +562,13 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]:
             )
             # debug visualization showing each newly added object
             if self._render_debug_obs:
+                logger.info(
+                    f"Generating debug images for {len(new_objects)} objects..."
+                )
                 for new_object in new_objects:
                     self.vdb.look_at(new_object.translation)
                     self.vdb.get_observation()
+                logger.info("   ... done")
 
         # simulate the world for a few seconds to validate the placements
         if not self.settle_sim():
@@ -613,7 +605,7 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]:
                 vdb=self.vdb,
                 target_receptacles=target_receptacles[obj_sampler_name],
                 goal_receptacles=goal_receptacles[sampler_name],
-                object_to_containing_receptacle=object_to_containing_receptacle,
+                object_to_containing_receptacle=self.object_to_containing_receptacle,
             )
             if new_target_objects is None:
                 return None
@@ -694,7 +686,7 @@ def extract_recep_info(recep):
         ]
 
         name_to_receptacle = {
-            k: v.name for k, v in object_to_containing_receptacle.items()
+            k: v.name for k, v in self.object_to_containing_receptacle.items()
         }
 
         return RearrangeEpisode(
@@ -787,9 +779,12 @@ def initialize_sim(self, scene_name: str, dataset_path: str) -> None:
         self.sim.agents[0].scene_node.translation = scene_bb.center()
 
         # initialize the debug visualizer
-        self.vdb = DebugVisualizer(
-            self.sim, output_path="rearrange_ep_gen_output/"
+        output_path = (
+            "rearrange_ep_gen_output/"
+            if self.vdb is None
+            else self.vdb.output_path
         )
+        self.vdb = DebugVisualizer(self.sim, output_path=output_path)
 
     def settle_sim(
         self, duration: float = 5.0, make_video: bool = True
@@ -800,7 +795,9 @@ def settle_sim(
         """
         if len(self.ep_sampled_objects) == 0:
             return True
-        # assert len(self.ep_sampled_objects) > 0
+
+        settle_start_time = time.time()
+        logger.info("Running placement stability analysis...")
 
         scene_bb = (
             self.sim.get_active_scene_graph().get_root_node().cumulative_bb
@@ -824,11 +821,13 @@ def settle_sim(
             if self._render_debug_obs:
                 self.vdb.get_observation(obs_cache=settle_db_obs)
 
+        logger.info(f"   ...done in {time.time()-settle_start_time} seconds.")
         # check stability of placements
         logger.info("Computing placement stability report:")
+        logger.info("----------------------------------------")
         max_settle_displacement = 0
         error_eps = 0.1
-        unstable_placements = []
+        unstable_placements: List[str] = []  # list of unstable object handles
         for new_object in self.ep_sampled_objects:
             error = (
                 spawn_positions[new_object.handle] - new_object.translation
@@ -839,6 +838,21 @@ def settle_sim(
                 logger.info(
                     f"    Object '{new_object.handle}' unstable. Moved {error} units from placement."
                 )
+                if self._render_debug_obs:
+                    self.vdb.peek_rigid_object(
+                        obj=new_object,
+                        peek_all_axis=True,
+                        additional_savefile_prefix="unstable_",
+                        debug_lines=[
+                            (
+                                [
+                                    spawn_positions[new_object.handle],
+                                    new_object.translation,
+                                ],
+                                mn.Color4.red(),
+                            )
+                        ],
+                    )
         logger.info(
             f" : unstable={len(unstable_placements)}|{len(self.ep_sampled_objects)} ({len(unstable_placements)/len(self.ep_sampled_objects)*100}%) : {unstable_placements}."
         )
@@ -852,5 +866,72 @@ def settle_sim(
                 prefix="settle_", fps=30, obs_cache=settle_db_obs
             )
 
+        # detailed receptacle stability report
+        logger.info("  Detailed sampling stats:")
+
+        # receptacle: [num_objects, num_unstable_objects]
+        rec_num_obj_vs_unstable: Dict[Receptacle, List[int]] = {}
+        for obj_name, rec in self.object_to_containing_receptacle.items():
+            if rec not in rec_num_obj_vs_unstable:
+                rec_num_obj_vs_unstable[rec] = [0, 0]
+            rec_num_obj_vs_unstable[rec][0] += 1
+            if obj_name in unstable_placements:
+                rec_num_obj_vs_unstable[rec][1] += 1
+        for rec, details in rec_num_obj_vs_unstable.items():
+            logger.info(
+                f"      receptacle '{rec.name}': ({details[1]}/{details[0]}) (unstable/total) objects."
+            )
+
+        success = len(unstable_placements) == 0
+
+        # optionally salvage the episode by removing unstable objects
+        if self.cfg.correct_unstable_results and not success:
+            logger.info("  attempting to correct unstable placements...")
+            for sampler_name, objects in self.episode_data[
+                "sampled_objects"
+            ].items():
+                obj_names = [obj.handle for obj in objects]
+                sampler = self._obj_samplers[sampler_name]
+                unstable_subset = [
+                    obj_name
+                    for obj_name in unstable_placements
+                    if obj_name in obj_names
+                ]
+                # check that we have freedom to reject some objects
+                if (
+                    len(objects) - len(unstable_subset)
+                    >= sampler.num_objects[0]
+                ):
+                    # remove the unstable objects from datastructures
+                    self.episode_data["sampled_objects"][sampler_name] = [
+                        obj
+                        for obj in self.episode_data["sampled_objects"][
+                            sampler_name
+                        ]
+                        if obj.handle not in unstable_subset
+                    ]
+                    self.ep_sampled_objects = [
+                        obj
+                        for obj in self.ep_sampled_objects
+                        if obj.handle not in unstable_subset
+                    ]
+                else:
+                    logger.info(
+                        f"  ... could not remove all unstable placements without violating minimum object sampler requirements for {sampler_name}"
+                    )
+                    logger.info("----------------------------------------")
+                    return False
+            logger.info(
+                f"  ... corrected unstable placements successfully. Final object count = {len(self.ep_sampled_objects)}"
+            )
+            # we removed all unstable placements
+            success = True
+
+        logger.info("----------------------------------------")
+
+        if self._render_debug_obs and success:
+            for obj in self.ep_sampled_objects:
+                self.vdb.peek_rigid_object(obj, peek_all_axis=True)
+
         # return success or failure
-        return len(unstable_placements) == 0
+        return success
diff --git a/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py b/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py
index 00f5835969..1a62703404 100644
--- a/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py
@@ -50,6 +50,8 @@ class RearrangeEpisodeGeneratorConfig:
     additional_object_paths: List[str] = field(
         default_factory=lambda: ["data/objects/ycb/"]
     )
+    # optionally correct unstable states by removing extra unstable objects (within minimum samples limitations)
+    correct_unstable_results: bool = False
     # ----- resource set definitions ------
     # Define the sets of scenes, objects, and receptacles which can be sampled from.
     # The SceneDataset will be searched for resources of each type with handles containing ANY "included" substrings and NO "excluded" substrings.
diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/object_sampler.py b/habitat-lab/habitat/datasets/rearrange/samplers/object_sampler.py
index 8c55f2ade6..356fdde3d9 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/object_sampler.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/object_sampler.py
@@ -6,6 +6,7 @@
 
 import math
 import random
+import time
 from collections import defaultdict
 from typing import Dict, List, Optional, Tuple
 
@@ -388,6 +389,8 @@ def sample(
             f"    Trying to sample {self.target_objects_number} from range {self.num_objects}"
         )
 
+        sampling_start_time = time.time()
+        pairing_start_time = sampling_start_time
         while (
             len(new_objects) < self.target_objects_number
             and num_pairing_tries < self.max_sample_attempts
@@ -415,8 +418,18 @@ def sample(
                     self.receptacle_candidates = None
 
             if new_object is not None:
+                # when an object placement is successful, reset the try counter.
+                logger.info(
+                    f"    found obj|receptacle pairing ({len(new_objects)}/{self.target_objects_number}) in {num_pairing_tries} attempts ({time.time()-pairing_start_time}sec)."
+                )
+                num_pairing_tries = 0
+                pairing_start_time = time.time()
                 new_objects.append((new_object, receptacle))
 
+        logger.info(
+            f"    Sampling process completed in ({time.time()-sampling_start_time}sec)."
+        )
+
         if len(new_objects) >= self.num_objects[0]:
             return new_objects
 
diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index c5b3d8e6ad..6f220aef3d 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -4,15 +4,18 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
+import os
+import random
 from abc import ABC, abstractmethod
 from copy import deepcopy
 from dataclasses import dataclass
-from typing import Dict, List, Optional, Union
+from typing import Any, Dict, List, Optional, Tuple, Union
 
 import magnum as mn
 import numpy as np
 
 import habitat_sim
+from habitat.core.logging import logger
 from habitat.sims.habitat_simulator.sim_utilities import add_wire_box
 
 
@@ -66,11 +69,24 @@ def sample_uniform_local(
         :param sample_region_scale: defines a XZ scaling of the sample region around its center. For example to constrain object spawning toward the center of a receptacle.
         """
 
-    @abstractmethod
     def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
         """
         Isolates boilerplate necessary to extract receptacle global transform of the Receptacle at the current state.
         """
+        if self.parent_object_handle is None:
+            # global identify by default
+            return mn.Matrix4.identity_init()
+        elif not self.is_parent_object_articulated:
+            obj_mgr = sim.get_rigid_object_manager()
+            obj = obj_mgr.get_object_by_handle(self.parent_object_handle)
+            # NOTE: we use absolute transformation from the 2nd visual node (scaling node) and root of all render assets to correctly account for any COM shifting, re-orienting, or scaling which has been applied.
+            return obj.visual_scene_nodes[1].absolute_transformation()
+        else:
+            ao_mgr = sim.get_articulated_object_manager()
+            obj = ao_mgr.get_object_by_handle(self.parent_object_handle)
+            return obj.get_link_scene_node(
+                self.parent_link
+            ).absolute_transformation()
 
     def sample_uniform_global(
         self, sim: habitat_sim.Simulator, sample_region_scale: float
@@ -91,6 +107,14 @@ def add_receptacle_visualization(
         """
         return []
 
+    @abstractmethod
+    def debug_draw(self, sim, color=None) -> None:
+        """
+        Render the Receptacle with DebugLineRender utility at the current frame.
+        Simulator must be provided. If color is provided, the debug render will use it.
+        Must be called after each frame is rendered, before querying the image data.
+        """
+
 
 class OnTopOfReceptacle(Receptacle):
     def __init__(self, name: str, places: List[str]):
@@ -112,6 +136,14 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
 
         return mn.Matrix4([[targ_T[j][i] for j in range(4)] for i in range(4)])
 
+    def debug_draw(self, sim, color=None) -> None:
+        """
+        Render the Receptacle with DebugLineRender utility at the current frame.
+        Simulator must be provided. If color is provided, the debug render will use it.
+        Must be called after each frame is rendered, before querying the image data.
+        """
+        # TODO:
+
 
 class AABBReceptacle(Receptacle):
     """
@@ -161,6 +193,7 @@ def sample_uniform_local(
     def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
         """
         Isolates boilerplate necessary to extract receptacle global transform of the Receptacle at the current state.
+        This specialization adds override rotation handling for global bounding box Receptacles.
         """
         if self.parent_object_handle is None:
             # this is a global stage receptacle
@@ -192,17 +225,8 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
             l2w4 = l2w4.__matmul__(T.__matmul__(R).__matmul__(T.inverted()))
             return l2w4
 
-        elif not self.is_parent_object_articulated:
-            obj_mgr = sim.get_rigid_object_manager()
-            obj = obj_mgr.get_object_by_handle(self.parent_object_handle)
-            # NOTE: we use absolute transformation from the 2nd visual node (scaling node) and root of all render assets to correctly account for any COM shifting, re-orienting, or scaling which has been applied.
-            return obj.visual_scene_nodes[1].absolute_transformation()
-        else:
-            ao_mgr = sim.get_articulated_object_manager()
-            obj = ao_mgr.get_object_by_handle(self.parent_object_handle)
-            return obj.get_link_scene_node(
-                self.parent_link
-            ).absolute_transformation()
+        # base class implements getting transform from attached objects
+        return super().get_global_transform
 
     def add_receptacle_visualization(
         self, sim: habitat_sim.Simulator
@@ -241,6 +265,143 @@ def add_receptacle_visualization(
             )
         return [box_obj]
 
+    def debug_draw(self, sim, color=None):
+        """
+        Render the AABBReceptacle with DebugLineRender utility at the current frame.
+        Simulator must be provided. If color is provided, the debug render will use it.
+        Must be called after each frame is rendered, before querying the image data.
+        """
+        # draw the box
+        if color is None:
+            color = mn.Color4.magenta()
+        dblr = sim.get_debug_line_render()
+        dblr.push_transform(self.get_global_transform(sim))
+        dblr.draw_box(self.bounds.min, self.bounds.max, color)
+        dblr.pop_transform()
+        # TODO: test this
+
+
+class TriangleMeshReceptacle(Receptacle):
+    """
+    Defines a Receptacle surface as a triangle mesh.
+    TODO: configurable maximum height.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        mesh_data: Tuple[List[Any], List[Any]],  # vertices, indices
+        parent_object_handle: str = None,
+        parent_link: Optional[int] = None,
+        up: Optional[mn.Vector3] = None,
+    ) -> None:
+        """
+        :param name: The name of the Receptacle. Should be unique and descriptive for any one object.
+        :param up: The "up" direction of the Receptacle in local AABB space. Used for optionally culling receptacles in un-supportive states such as inverted surfaces.
+        :param parent_object_handle: The rigid or articulated object instance handle for the parent object to which the Receptacle is attached. None for globally defined stage Receptacles.
+        :param parent_link: Index of the link to which the Receptacle is attached if the parent is an ArticulatedObject. -1 denotes the base link. None for rigid objects and stage Receptables.
+        """
+        super().__init__(name, parent_object_handle, parent_link, up)
+        self.mesh_data = mesh_data
+        self.area_weighted_accumulator = (
+            []
+        )  # normalized float weights for each triangle for sampling
+        assert len(mesh_data[1]) % 3 == 0, "must be triangles"
+        self.total_area = 0
+        for f_ix in range(int(len(mesh_data[1]) / 3)):
+            v = self.get_face_verts(f_ix)
+            w1 = v[1] - v[0]
+            w2 = v[2] - v[1]
+            self.area_weighted_accumulator.append(
+                0.5 * np.linalg.norm(np.cross(w1, w2))
+            )
+            self.total_area += self.area_weighted_accumulator[-1]
+        for f_ix in range(len(self.area_weighted_accumulator)):
+            self.area_weighted_accumulator[f_ix] = (
+                self.area_weighted_accumulator[f_ix] / self.total_area
+            )
+            if f_ix > 0:
+                self.area_weighted_accumulator[
+                    f_ix
+                ] += self.area_weighted_accumulator[f_ix - 1]
+        # print(self.area_weighted_accumulator)
+
+    def get_face_verts(self, f_ix):
+        verts = []
+        for ix in range(3):
+            verts.append(
+                np.array(
+                    self.mesh_data[0][self.mesh_data[1][int(f_ix * 3 + ix)]]
+                )
+            )
+        return verts
+
+    def sample_area_weighted_triangle(self):
+        """
+        Isolates the area weighted triangle sampling code.
+        """
+
+        def find_ge(a, x):
+            "Find leftmost item greater than or equal to x"
+            from bisect import bisect_left
+
+            i = bisect_left(a, x)
+            if i != len(a):
+                return i
+            raise ValueError
+
+        # first area weighted sampling of a triangle
+        sample_val = random.random()
+        tri_index = find_ge(self.area_weighted_accumulator, sample_val)
+        return tri_index
+
+    def sample_uniform_local(
+        self, sample_region_scale: float = 1.0
+    ) -> mn.Vector3:
+        """
+        Sample a uniform random point from the mesh.
+
+        :param sample_region_scale: defines a XZ scaling of the sample region around its center. For example to constrain object spawning toward the center of a receptacle.
+        """
+
+        if sample_region_scale != 1.0:
+            logger.warning(
+                "TriangleMeshReceptacle does not support 'sample_region_scale' != 1.0."
+            )
+
+        tri_index = self.sample_area_weighted_triangle()
+
+        # then sample a random point in the triangle
+        # https://math.stackexchange.com/questions/538458/how-to-sample-points-on-a-triangle-surface-in-3d
+        coef1 = random.random()
+        coef2 = random.random()
+        if coef1 + coef2 >= 1:
+            coef1 = 1 - coef1
+            coef2 = 1 - coef2
+        v = self.get_face_verts(f_ix=tri_index)
+        rand_point = v[0] + coef1 * (v[1] - v[0]) + coef2 * (v[2] - v[0])
+
+        return rand_point
+
+    def debug_draw(self, sim, color=None):
+        """
+        Render the Receptacle with DebugLineRender utility at the current frame.
+        Draws the Receptacle mesh.
+        Simulator must be provided. If color is provided, the debug render will use it.
+        Must be called after each frame is rendered, before querying the image data.
+        """
+        # draw all mesh triangles
+        if color is None:
+            color = mn.Color4.magenta()
+        dblr = sim.get_debug_line_render()
+        assert len(self.mesh_data[1]) % 3 == 0, "must be triangles"
+        for face in range(int(len(self.mesh_data[1]) / 3)):
+            verts = self.get_face_verts(f_ix=face)
+            for edge in range(3):
+                dblr.draw_transformed_line(
+                    verts[edge], verts[(edge + 1) % 3], color
+                )
+
 
 def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]:
     """
@@ -262,6 +423,9 @@ def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]:
         stage_template = stm.get_template_by_handle(template_handle)
         for item in stage_template.get_user_config().get_subconfig_keys():
             if item.startswith("receptacle_"):
+                print(
+                    f"template file_directory = {stage_template.file_directory}"
+                )
                 if template_handle not in receptacles["stage"]:
                     receptacles["stage"][template_handle] = []
                 receptacles["stage"][template_handle].append(item)
@@ -272,6 +436,9 @@ def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]:
         obj_template = rotm.get_template_by_handle(template_handle)
         for item in obj_template.get_user_config().get_subconfig_keys():
             if item.startswith("receptacle_"):
+                print(
+                    f"template file_directory = {obj_template.file_directory}"
+                )
                 if template_handle not in receptacles["rigid"]:
                     receptacles["rigid"][template_handle] = []
                 receptacles["rigid"][template_handle].append(item)
@@ -290,9 +457,55 @@ def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]:
     return receptacles
 
 
+def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
+    """
+    Returns a Tuple of (verts,indices) from a ply mesh.
+    NOTE: the input PLY must contain only triangles.
+    TODO: This could be replaced by a standard importer, but I didn't want to add additional dependencies for such as small feature.
+    """
+    mesh_data: Tuple[List[mn.Vector3], List[int]] = ([], [])
+    with open(ply_file) as f:
+        lines = [line.rstrip() for line in f]
+        assert lines[0] == "ply", f"Must be PLY format. '{ply_file}'"
+        assert "format ascii" in lines[1], f"Must be ascii PLY. '{ply_file}'"
+        # parse the header
+        line_index = 2
+        num_verts = 0
+        num_faces = 0
+        while line_index < len(lines):
+            if lines[line_index].startswith("element vertex"):
+                num_verts = int(lines[line_index][14:])
+                print(f"num_verts = {num_verts}")
+            elif lines[line_index].startswith("element face"):
+                num_faces = int(lines[line_index][12:])
+                print(f"num_faces = {num_faces}")
+            elif lines[line_index] == "end_header":
+                # done parsing header
+                line_index += 1
+                break
+            line_index += 1
+        assert (
+            len(lines) - line_index == num_verts + num_faces
+        ), f"Lines after header ({len(lines) - line_index}) should agree with forward declared content. {num_verts} verts and {num_faces} faces expected. '{ply_file}'"
+        # parse the verts
+        for vert_line in range(line_index, num_verts + line_index):
+            coords = [float(x) for x in lines[vert_line].split(" ")]
+            mesh_data[0].append(mn.Vector3(coords))
+        line_index += num_verts
+        for face_line in range(line_index, num_faces + line_index):
+            assert (
+                int(lines[face_line][0]) == 3
+            ), f"Faces must be triangles. '{ply_file}'"
+            indices = [int(x) for x in lines[face_line].split(" ")[1:]]
+            mesh_data[1].extend(indices)
+
+    return mesh_data
+
+
 def parse_receptacles_from_user_config(
     user_subconfig: habitat_sim._ext.habitat_sim_bindings.Configuration,
     parent_object_handle: Optional[str] = None,
+    parent_template_directory: str = "",
     valid_link_names: Optional[List[str]] = None,
     ao_uniform_scaling: float = 1.0,
 ) -> List[Union[Receptacle, AABBReceptacle]]:
@@ -307,11 +520,18 @@ def parse_receptacles_from_user_config(
 
     Construct and return a list of Receptacle objects. Multiple Receptacles can be defined in a single user subconfig.
     """
-    receptacles: List[Union[Receptacle, AABBReceptacle]] = []
+    receptacles: List[
+        Union[Receptacle, AABBReceptacle, TriangleMeshReceptacle]
+    ] = []
+
+    # pre-define unique specifier strings for parsing receptacle types
+    receptacle_prefix_string = "receptacle_"
+    mesh_receptacle_id_string = "receptacle_mesh_"
+    aabb_receptacle_id_string = "receptacle_aabb_"
 
     # search the generic user subconfig metadata looking for receptacles
     for sub_config_key in user_subconfig.get_subconfig_keys():
-        if sub_config_key.startswith("receptacle_"):
+        if sub_config_key.startswith(receptacle_prefix_string):
             sub_config = user_subconfig.get_subconfig(sub_config_key)
             # this is a receptacle, parse it
             assert sub_config.has_value("position")
@@ -363,20 +583,43 @@ def parse_receptacles_from_user_config(
             )
             receptacle_scale = ao_uniform_scaling * sub_config.get("scale")
 
-            # TODO: adding more receptacle types will require additional logic here
-            receptacles.append(
-                AABBReceptacle(
-                    name=receptacle_name,
-                    bounds=mn.Range3D.from_center(
-                        receptacle_position,
-                        receptacle_scale,
-                    ),
-                    rotation=rotation,
-                    up=up,
-                    parent_object_handle=parent_object_handle,
-                    parent_link=parent_link_ix,
+            if aabb_receptacle_id_string in sub_config_key:
+                receptacles.append(
+                    AABBReceptacle(
+                        name=receptacle_name,
+                        bounds=mn.Range3D.from_center(
+                            receptacle_position,
+                            receptacle_scale,
+                        ),
+                        rotation=rotation,
+                        up=up,
+                        parent_object_handle=parent_object_handle,
+                        parent_link=parent_link_ix,
+                    )
+                )
+            elif mesh_receptacle_id_string in sub_config_key:
+                mesh_file = os.path.join(
+                    parent_template_directory, sub_config.get("mesh_filepath")
+                )
+                assert os.path.exists(
+                    mesh_file
+                ), f"Configured receptacle mesh asset '{mesh_file}' not found."
+                # TODO: build the mesh_data entry from scale and mesh
+                mesh_data = import_tri_mesh_ply(mesh_file)
+
+                receptacles.append(
+                    TriangleMeshReceptacle(
+                        name=receptacle_name,
+                        mesh_data=mesh_data,
+                        up=up,
+                        parent_object_handle=parent_object_handle,
+                        parent_link=parent_link_ix,
+                    )
+                )
+            else:
+                raise AssertionError(
+                    f"Receptacle detected without a subtype specifier: '{mesh_receptacle_id_string}'"
                 )
-            )
 
     return receptacles
 
@@ -391,32 +634,45 @@ def find_receptacles(
     obj_mgr = sim.get_rigid_object_manager()
     ao_mgr = sim.get_articulated_object_manager()
 
-    receptacles: List[Union[Receptacle, AABBReceptacle]] = []
+    receptacles: List[
+        Union[Receptacle, AABBReceptacle, TriangleMeshReceptacle]
+    ] = []
 
     # search for global receptacles included with the stage
     stage_config = sim.get_stage_initialization_template()
     if stage_config is not None:
         stage_user_attr = stage_config.get_user_config()
-        receptacles.extend(parse_receptacles_from_user_config(stage_user_attr))
+        receptacles.extend(
+            parse_receptacles_from_user_config(
+                stage_user_attr,
+                parent_template_directory=stage_config.file_directory,
+            )
+        )
 
     # rigid object receptacles
     for obj_handle in obj_mgr.get_object_handles():
         obj = obj_mgr.get_object_by_handle(obj_handle)
+        source_template_file = obj.creation_attributes.file_directory
         user_attr = obj.user_attributes
         receptacles.extend(
             parse_receptacles_from_user_config(
-                user_attr, parent_object_handle=obj_handle
+                user_attr,
+                parent_object_handle=obj_handle,
+                parent_template_directory=source_template_file,
             )
         )
 
     # articulated object receptacles
     for obj_handle in ao_mgr.get_object_handles():
         obj = ao_mgr.get_object_by_handle(obj_handle)
+        # TODO: no way to get filepath from AO currently. Add this API.
+        source_template_file = ""
         user_attr = obj.user_attributes
         receptacles.extend(
             parse_receptacles_from_user_config(
                 user_attr,
                 parent_object_handle=obj_handle,
+                parent_template_directory=source_template_file,
                 valid_link_names=[
                     obj.get_link_name(link)
                     for link in range(-1, obj.num_links)
diff --git a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
index 162a19b865..0752685edc 100644
--- a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
+++ b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
@@ -4,7 +4,8 @@
 # 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, List, Optional, Union
+import os
+from typing import Any, List, Optional, Tuple, Union
 
 import magnum as mn
 import numpy as np
@@ -109,17 +110,40 @@ def save_observation(
 
         # filename format "prefixmonth_day_year_hourminutesecondmicrosecond.png"
         date_time = datetime.now().strftime("%m_%d_%Y_%H%M%S%f")
-        file_path = output_path + prefix + date_time + ".png"
+        file_path = os.path.join(output_path, prefix + date_time + ".png")
         image.save(file_path)
         if show:
             image.show()
         return file_path
 
+    def render_debug_lines(
+        self,
+        debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+    ):
+        """
+        Draw a set of debug lines with accomanying colors.
+        """
+        # support None input to make useage easier elsewhere
+        if debug_lines is not None:
+            for points, color in debug_lines:
+                for p_ix, point in enumerate(points):
+                    if p_ix == 0:
+                        continue
+                    prev_point = points[p_ix - 1]
+                    self.debug_line_render.draw_transformed_line(
+                        prev_point,
+                        point,
+                        color,
+                    )
+
     def peek_rigid_object(
         self,
         obj: habitat_sim.physics.ManagedRigidObject,
         cam_local_pos: Optional[mn.Vector3] = None,
         peek_all_axis: bool = False,
+        additional_savefile_prefix="",
+        debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        show: bool = False,
     ) -> str:
         """
         Specialization to peek a rigid object.
@@ -131,6 +155,9 @@ def peek_rigid_object(
             obj.root_scene_node.cumulative_bb,
             cam_local_pos,
             peek_all_axis,
+            additional_savefile_prefix,
+            debug_lines,
+            show,
         )
 
     def peek_articulated_object(
@@ -138,6 +165,9 @@ def peek_articulated_object(
         obj: habitat_sim.physics.ManagedArticulatedObject,
         cam_local_pos: Optional[mn.Vector3] = None,
         peek_all_axis: bool = False,
+        additional_savefile_prefix="",
+        debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        show: bool = False,
     ) -> str:
         """
         Specialization to peek an articulated object.
@@ -149,7 +179,15 @@ def peek_articulated_object(
 
         obj_bb = get_ao_global_bb(obj)
 
-        return self._peek_object(obj, obj_bb, cam_local_pos, peek_all_axis)
+        return self._peek_object(
+            obj,
+            obj_bb,
+            cam_local_pos,
+            peek_all_axis,
+            additional_savefile_prefix,
+            debug_lines,
+            show,
+        )
 
     def _peek_object(
         self,
@@ -160,11 +198,15 @@ def _peek_object(
         obj_bb: mn.Range3D,
         cam_local_pos: Optional[mn.Vector3] = None,
         peek_all_axis: bool = False,
+        additional_savefile_prefix="",
+        debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        show: bool = False,
     ) -> str:
         """
-        Compute a camera placement to view an ArticulatedObject and show/save an observation.
+        Compute a camera placement to view an object and show/save an observation.
         Return the filepath.
         If peek_all_axis, then create a merged 3x2 matrix of images looking at the object from all angles.
+        debug_lines: optionally provide a list of debug line render tuples, each with a sequence of points and a color. These will be displayed in all peek images.
         """
         obj_abs_transform = obj.root_scene_node.absolute_transformation()
         look_at = obj_abs_transform.translation
@@ -187,42 +229,51 @@ def _peek_object(
                 * distance
                 + look_at
             )
+            self.render_debug_lines(debug_lines)
             return self.save_observation(
-                prefix="peek_" + obj.handle,
+                prefix=additional_savefile_prefix + "peek_" + obj.handle,
                 look_at=look_at,
                 look_from=look_from,
+                show=show,
             )
-        else:
-            # collect axis observations
-            axis_obs: List[Any] = []
-            for axis in range(6):
-                axis_vec = mn.Vector3()
-                axis_vec[axis % 3] = 1 if axis // 3 == 0 else -1
-                look_from = (
-                    obj_abs_transform.transform_vector(axis_vec).normalized()
-                    * distance
-                    + look_at
-                )
-                self.get_observation(look_at, look_from, axis_obs)
-            # stitch images together
-            stitched_image = None
-            from PIL import Image
 
-            from habitat_sim.utils import viz_utils as vut
+        # collect axis observations
+        axis_obs: List[Any] = []
+        for axis in range(6):
+            axis_vec = mn.Vector3()
+            axis_vec[axis % 3] = 1 if axis // 3 == 0 else -1
+            look_from = (
+                obj_abs_transform.transform_vector(axis_vec).normalized()
+                * distance
+                + look_at
+            )
+            self.render_debug_lines(debug_lines)
+            self.get_observation(look_at, look_from, axis_obs)
+        # stitch images together
+        stitched_image = None
+        from PIL import Image
 
-            for ix, obs in enumerate(axis_obs):
-                image = vut.observation_to_image(obs["rgb"], "color")
-                if stitched_image is None:
-                    stitched_image = Image.new(
-                        image.mode, (image.size[0] * 3, image.size[1] * 2)
-                    )
-                location = (
-                    image.size[0] * (ix % 3),
-                    image.size[1] * (0 if ix // 3 == 0 else 1),
+        from habitat_sim.utils import viz_utils as vut
+
+        for ix, obs in enumerate(axis_obs):
+            image = vut.observation_to_image(obs["rgb"], "color")
+            if stitched_image is None:
+                stitched_image = Image.new(
+                    image.mode, (image.size[0] * 3, image.size[1] * 2)
                 )
-                stitched_image.paste(image, location)
+            location = (
+                image.size[0] * (ix % 3),
+                image.size[1] * (0 if ix // 3 == 0 else 1),
+            )
+            stitched_image.paste(image, location)
+        if show:
             stitched_image.show()
-        return ""
+        save_path = os.path.join(
+            self.output_path,
+            additional_savefile_prefix + "peek_6x_" + obj.handle + ".png",
+        )
+        stitched_image.save(save_path)
+        return save_path
 
     def make_debug_video(
         self,
@@ -249,7 +300,7 @@ def make_debug_video(
 
         from habitat_sim.utils import viz_utils as vut
 
-        file_path = output_path + prefix + date_time
+        file_path = os.path.join(output_path, prefix + date_time)
         logger.info(f"DebugVisualizer: Saving debug video to {file_path}")
         vut.make_video(
             obs_cache, self.default_sensor_uuid, "color", file_path, fps=fps

From 307bcc521d50941f8d33a92c38c1953ea5ef93b9 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 30 Jan 2023 10:31:32 -0800
Subject: [PATCH 02/27] add some comments

---
 .../datasets/rearrange/configs/all_receptacles_test.yaml   | 7 ++++---
 .../habitat/datasets/rearrange/rearrange_generator.py      | 2 ++
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml b/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
index f7eb811c50..fd2406cdca 100644
--- a/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
+++ b/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
@@ -1,8 +1,9 @@
 ---
-## All receptacles and objects are used.
+## This config is meant to test receptacle configurations and clutter generation for new datasets.
+## All receptacles used with a small batch of known YCB objects.
+
 # Define your own dataset path,
-#dataset_path: "data/replica_cad/replicaCAD.scene_dataset_config.json"
-dataset_path: "/run/media/alexclegg/Extreme_SSD/datasets/floorplanner/fp_mini_rec_automation/fp_mini.scene_dataset_config.json"
+dataset_path: "data/replica_cad/replicaCAD.scene_dataset_config.json"
 additional_object_paths:
   - "data/objects/ycb/configs/"
 correct_unstable_results: True
diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
index 3801f70579..a58553bc6e 100644
--- a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
@@ -869,6 +869,7 @@ def settle_sim(
         # detailed receptacle stability report
         logger.info("  Detailed sampling stats:")
 
+        # compute number of unstable objects for each receptacle
         # receptacle: [num_objects, num_unstable_objects]
         rec_num_obj_vs_unstable: Dict[Receptacle, List[int]] = {}
         for obj_name, rec in self.object_to_containing_receptacle.items():
@@ -929,6 +930,7 @@ def settle_sim(
 
         logger.info("----------------------------------------")
 
+        #generate debug images of all final object placements
         if self._render_debug_obs and success:
             for obj in self.ep_sampled_objects:
                 self.vdb.peek_rigid_object(obj, peek_all_axis=True)

From 9718a8ba997b0122f20853f5c815786db4ed6a91 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 30 Jan 2023 10:54:06 -0800
Subject: [PATCH 03/27] docstrings and typing

---
 .../datasets/rearrange/samplers/receptacle.py | 53 +++++++++++++------
 1 file changed, 38 insertions(+), 15 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index 6f220aef3d..bd67a94d12 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -108,11 +108,13 @@ def add_receptacle_visualization(
         return []
 
     @abstractmethod
-    def debug_draw(self, sim, color=None) -> None:
+    def debug_draw(self, sim: habitat_sim.Simulator, color:Optional[mn.Color4]=None) -> None:
         """
         Render the Receptacle with DebugLineRender utility at the current frame.
-        Simulator must be provided. If color is provided, the debug render will use it.
         Must be called after each frame is rendered, before querying the image data.
+
+        :param sim: Simulator must be provided. 
+        :param color: Optionally provide wireframe color, otherwise magenta.
         """
 
 
@@ -136,11 +138,13 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
 
         return mn.Matrix4([[targ_T[j][i] for j in range(4)] for i in range(4)])
 
-    def debug_draw(self, sim, color=None) -> None:
+    def debug_draw(self, sim: habitat_sim.Simulator, color:Optional[mn.Color4]=None) -> None:
         """
         Render the Receptacle with DebugLineRender utility at the current frame.
-        Simulator must be provided. If color is provided, the debug render will use it.
         Must be called after each frame is rendered, before querying the image data.
+
+        :param sim: Simulator must be provided. 
+        :param color: Optionally provide wireframe color, otherwise magenta.
         """
         # TODO:
 
@@ -265,11 +269,13 @@ def add_receptacle_visualization(
             )
         return [box_obj]
 
-    def debug_draw(self, sim, color=None):
+    def debug_draw(self, sim: habitat_sim.Simulator, color:Optional[mn.Color4]=None)->None:
         """
         Render the AABBReceptacle with DebugLineRender utility at the current frame.
-        Simulator must be provided. If color is provided, the debug render will use it.
         Must be called after each frame is rendered, before querying the image data.
+
+        :param sim: Simulator must be provided. 
+        :param color: Optionally provide wireframe color, otherwise magenta.
         """
         # draw the box
         if color is None:
@@ -296,10 +302,13 @@ def __init__(
         up: Optional[mn.Vector3] = None,
     ) -> None:
         """
+        Initialize the TriangleMeshReceptacle from mesh data and pre-compute the area weighted accumulator.
+
         :param name: The name of the Receptacle. Should be unique and descriptive for any one object.
-        :param up: The "up" direction of the Receptacle in local AABB space. Used for optionally culling receptacles in un-supportive states such as inverted surfaces.
+        :param mesh_data: The Receptacle's mesh data. A Tuple of two Lists, first vertex geometry (Vector3) and second topology (indicies of triangle corner verts(int) (len divisible by 3)).
         :param parent_object_handle: The rigid or articulated object instance handle for the parent object to which the Receptacle is attached. None for globally defined stage Receptacles.
         :param parent_link: Index of the link to which the Receptacle is attached if the parent is an ArticulatedObject. -1 denotes the base link. None for rigid objects and stage Receptables.
+        :param up: The "up" direction of the Receptacle in local AABB space. Used for optionally culling receptacles in un-supportive states such as inverted surfaces.
         """
         super().__init__(name, parent_object_handle, parent_link, up)
         self.mesh_data = mesh_data
@@ -307,6 +316,8 @@ def __init__(
             []
         )  # normalized float weights for each triangle for sampling
         assert len(mesh_data[1]) % 3 == 0, "must be triangles"
+
+        #pre-compute the normalized cumulative area of all triangle faces for later sampling
         self.total_area = 0
         for f_ix in range(int(len(mesh_data[1]) / 3)):
             v = self.get_face_verts(f_ix)
@@ -326,8 +337,13 @@ def __init__(
                 ] += self.area_weighted_accumulator[f_ix - 1]
         # print(self.area_weighted_accumulator)
 
-    def get_face_verts(self, f_ix):
-        verts = []
+    def get_face_verts(self, f_ix: int) -> List[np.array]:
+        """
+        Get all three vertices of a mesh triangle given it's face index as a list of numpy arrays.
+
+        :param f_ix: The index of the mesh triangle.
+        """
+        verts: List[np.array] = []
         for ix in range(3):
             verts.append(
                 np.array(
@@ -336,12 +352,14 @@ def get_face_verts(self, f_ix):
             )
         return verts
 
-    def sample_area_weighted_triangle(self):
+    def sample_area_weighted_triangle(self) -> int:
         """
         Isolates the area weighted triangle sampling code.
+
+        Returns a random triangle index sampled with area weighting.
         """
 
-        def find_ge(a, x):
+        def find_ge(a:List[Any], x)->Any:
             "Find leftmost item greater than or equal to x"
             from bisect import bisect_left
 
@@ -383,12 +401,14 @@ def sample_uniform_local(
 
         return rand_point
 
-    def debug_draw(self, sim, color=None):
+    def debug_draw(self, sim:habitat_sim.Simulator, color:Optional[mn.Color4]=None)->None:
         """
         Render the Receptacle with DebugLineRender utility at the current frame.
         Draws the Receptacle mesh.
-        Simulator must be provided. If color is provided, the debug render will use it.
         Must be called after each frame is rendered, before querying the image data.
+
+        :param sim: Simulator must be provided. 
+        :param color: Optionally provide wireframe color, otherwise magenta.
         """
         # draw all mesh triangles
         if color is None:
@@ -403,12 +423,14 @@ def debug_draw(self, sim, color=None):
                 )
 
 
-def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]:
+def get_all_scenedataset_receptacles(sim:habitat_sim.Simulator) -> Dict[str, Dict[str, List[str]]]:
     """
     Scrapes the active SceneDataset from a Simulator for all receptacle names defined in rigid/articulated object and stage templates for investigation and preview purposes.
     Note this will not include scene-specific overrides defined in scene_config.json files. Only receptacles defined in object_config.json, ao_config.json, and stage_config.json files or added programmatically to associated Attributes objects will be found.
 
     Returns a dict with keys {"stage", "rigid", "articulated"} mapping object template handles to lists of receptacle names.
+
+    :param sim: Simulator must be provided. 
     """
     # cache the rigid and articulated receptacles seperately
     receptacles: Dict[str, Dict[str, List[str]]] = {
@@ -460,8 +482,9 @@ def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]:
 def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
     """
     Returns a Tuple of (verts,indices) from a ply mesh.
-    NOTE: the input PLY must contain only triangles.
     TODO: This could be replaced by a standard importer, but I didn't want to add additional dependencies for such as small feature.
+
+    :param ply_file: The input PLY mesh file. NOTE: must contain only triangles.
     """
     mesh_data: Tuple[List[mn.Vector3], List[int]] = ([], [])
     with open(ply_file) as f:

From 352065eb55bcc06cd0d2cc0e3795998e64351659 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 30 Jan 2023 11:16:55 -0800
Subject: [PATCH 04/27] docstrings and typing for DebugVisualizer util

---
 .../habitat_simulator/debug_visualizer.py     | 72 ++++++++++++++++---
 1 file changed, 61 insertions(+), 11 deletions(-)

diff --git a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
index 0752685edc..1e19718118 100644
--- a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
+++ b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
@@ -30,6 +30,10 @@ def __init__(
         """
         Initialize the debugger provided a Simulator and the uuid of the debug sensor.
         NOTE: Expects the debug sensor attached to and coincident with agent 0's frame.
+
+        :param sim: Simulator instance must be provided for attachment.
+        :param output_path: Directory path for saving debug images and videos.
+        :param default_sensor_uuid: Which sensor uuid to use for debug image rendering.
         """
         self.sim = sim
         self.output_path = output_path
@@ -46,6 +50,11 @@ def look_at(
     ) -> None:
         """
         Point the debug camera at a target.
+        Standard look_at function syntax.
+
+        :param look_at: 3D global position to point the camera towards.
+        :param look_from: 3D global position of the camera.
+        :param look_up: 3D global "up" vector for aligning the camera roll.
         """
         agent = self.sim.get_agent(0)
         camera_pos = (
@@ -76,6 +85,10 @@ def get_observation(
         Render a debug observation of the current state and cache it.
         Optionally configure the camera transform.
         Optionally provide an alternative observation cache.
+
+        :param look_at: 3D global position to point the camera towards.
+        :param look_from: 3D global position of the camera.
+        :param obs_cache: Optioanlly provide an external observation cache datastructure in place of self._debug_obs.
         """
         if look_at is not None:
             self.look_at(look_at, look_from)
@@ -94,8 +107,15 @@ def save_observation(
         show: bool = True,
     ) -> str:
         """
-        Get an observation and save it to file.
+        Render an observation and save it to file.
         Return the filepath.
+
+        :param output_path: Optional directory path for saving debug images and videos. Otherwise use self.output_path.
+        :param prefix: Optional prefix for output filename. Filename format: "<prefix>month_day_year_hourminutesecondmicrosecond.png"
+        :param look_at: 3D global position to point the camera towards.
+        :param look_from: 3D global position of the camera.
+        :param obs_cache: Optioanlly provide an external observation cache datastructure in place of self._debug_obs.
+        :param show: If True, open the image immediately.
         """
         obs_cache = []
         self.get_observation(look_at, look_from, obs_cache)
@@ -119,9 +139,11 @@ def save_observation(
     def render_debug_lines(
         self,
         debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
-    ):
+    ) -> None:
         """
         Draw a set of debug lines with accomanying colors.
+
+        :param debug_lines: A set of debug line strips with accompanying colors. Each list entry contains a list of points and a color.
         """
         # support None input to make useage easier elsewhere
         if debug_lines is not None:
@@ -146,8 +168,16 @@ def peek_rigid_object(
         show: bool = False,
     ) -> str:
         """
-        Specialization to peek a rigid object.
-        See _peek_object.
+        Helper function to generate image(s) of an object for contextual debugging purposes.
+        Specialization to peek a rigid object. See _peek_object.
+        Compute a camera placement to view an object. Show/save an observation. Return the filepath.
+
+        :param obj: The ManagedRigidObject to peek.
+        :param cam_local_pos: Optionally provide a camera location in location local coordinates. Otherwise offset along local -Z axis from the object.
+        :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
+        :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
+        :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param show: If True, open and display the image immediately.
         """
 
         return self._peek_object(
@@ -170,8 +200,16 @@ def peek_articulated_object(
         show: bool = False,
     ) -> str:
         """
-        Specialization to peek an articulated object.
-        See _peek_object.
+        Helper function to generate image(s) of an object for contextual debugging purposes.
+        Specialization to peek an articulated object. See _peek_object.
+        Compute a camera placement to view an object. Show/save an observation. Return the filepath.
+
+        :param obj: The ManagedArticulatedObject to peek.
+        :param cam_local_pos: Optionally provide a camera location in location local coordinates. Otherwise offset along local -Z axis from the object.
+        :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
+        :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
+        :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param show: If True, open and display the image immediately.
         """
         from habitat.sims.habitat_simulator.sim_utilities import (
             get_ao_global_bb,
@@ -203,10 +241,16 @@ def _peek_object(
         show: bool = False,
     ) -> str:
         """
-        Compute a camera placement to view an object and show/save an observation.
-        Return the filepath.
-        If peek_all_axis, then create a merged 3x2 matrix of images looking at the object from all angles.
-        debug_lines: optionally provide a list of debug line render tuples, each with a sequence of points and a color. These will be displayed in all peek images.
+        Internal helper function to generate image(s) of an object for contextual debugging purposes.
+        Compute a camera placement to view an object. Show/save an observation. Return the filepath.
+
+        :param obj: The ManagedRigidObject or ManagedArticulatedObject to peek.
+        :param obj_bb: The object's bounding box (provided by consumer functions.)
+        :param cam_local_pos: Optionally provide a camera location in location local coordinates. Otherwise offset along local -Z axis from the object.
+        :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
+        :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
+        :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param show: If True, open and display the image immediately.
         """
         obj_abs_transform = obj.root_scene_node.absolute_transformation()
         look_at = obj_abs_transform.translation
@@ -283,8 +327,14 @@ def make_debug_video(
         obs_cache: Optional[List[Any]] = None,
     ) -> None:
         """
-        Produce a video from a set of debug observations.
+        Produce and save a video from a set of debug observations.
+
+        :param output_path: Optional directory path for saving the video. Otherwise use self.output_path.
+        :param prefix: Optional prefix for output filename. Filename format: "<output_path><prefix><timestamp>"
+        :param fps: Framerate of the video. Defaults to 4FPS expecting disjoint still frames.
+        :param obs_cache: Optioanlly provide an external observation cache datastructure in place of self._debug_obs.
         """
+
         if output_path is None:
             output_path = self.output_path
 

From 2ed69ef2142e24dd4b8658329bc7165ba21e42d9 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 30 Jan 2023 13:57:30 -0800
Subject: [PATCH 05/27] update config to test against habitat-sim branch with
 updated test assets

---
 .circleci/config.yml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.circleci/config.yml b/.circleci/config.yml
index 08bda76222..521bc30827 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -191,6 +191,8 @@ jobs:
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat;
               cd habitat-sim
+              git fetch --all
+              git checkout receptacle-test-assets
               pip install -r requirements.txt --progress-bar off
               pip install pillow
               python -u setup.py install --headless --with-cuda --bullet

From 492461d92c8e3ac69c086ff88ec65bf81899dfef Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 31 Jan 2023 09:49:47 -0800
Subject: [PATCH 06/27] bugfix with global transform + cleanup

---
 .../habitat/datasets/rearrange/samplers/receptacle.py     | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index bd67a94d12..41e6427f41 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -230,7 +230,7 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
             return l2w4
 
         # base class implements getting transform from attached objects
-        return super().get_global_transform
+        return super().get_global_transform(sim)
 
     def add_receptacle_visualization(
         self, sim: habitat_sim.Simulator
@@ -498,10 +498,8 @@ def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
         while line_index < len(lines):
             if lines[line_index].startswith("element vertex"):
                 num_verts = int(lines[line_index][14:])
-                print(f"num_verts = {num_verts}")
             elif lines[line_index].startswith("element face"):
                 num_faces = int(lines[line_index][12:])
-                print(f"num_faces = {num_faces}")
             elif lines[line_index] == "end_header":
                 # done parsing header
                 line_index += 1
@@ -649,9 +647,11 @@ def parse_receptacles_from_user_config(
 
 def find_receptacles(
     sim: habitat_sim.Simulator,
-) -> List[Union[Receptacle, AABBReceptacle]]:
+) -> List[Union[Receptacle, AABBReceptacle, TriangleMeshReceptacle]]:
     """
     Scrape and return a list of all Receptacles defined in the metadata belonging to the scene's currently instanced objects.
+
+    :param sim: Simulator must be provided. 
     """
 
     obj_mgr = sim.get_rigid_object_manager()

From 61de77b4c1d9c2784c9520a5a82367e0e252ce07 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 31 Jan 2023 09:53:44 -0800
Subject: [PATCH 07/27] mesh receptacle debug_draw with global transforms

---
 habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index 41e6427f41..fcd60697e9 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -414,6 +414,7 @@ def debug_draw(self, sim:habitat_sim.Simulator, color:Optional[mn.Color4]=None)-
         if color is None:
             color = mn.Color4.magenta()
         dblr = sim.get_debug_line_render()
+        dblr.push_transform(self.get_global_transform(sim))
         assert len(self.mesh_data[1]) % 3 == 0, "must be triangles"
         for face in range(int(len(self.mesh_data[1]) / 3)):
             verts = self.get_face_verts(f_ix=face)
@@ -421,6 +422,7 @@ def debug_draw(self, sim:habitat_sim.Simulator, color:Optional[mn.Color4]=None)-
                 dblr.draw_transformed_line(
                     verts[edge], verts[(edge + 1) % 3], color
                 )
+        dblr.pop_transform()
 
 
 def get_all_scenedataset_receptacles(sim:habitat_sim.Simulator) -> Dict[str, Dict[str, List[str]]]:

From 93446164e4bfea8e35022cce5d62f99d3eabe8e2 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 31 Jan 2023 10:43:52 -0800
Subject: [PATCH 08/27] add test WIP

---
 test/test_rearrange_task.py | 69 +++++++++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)

diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index ed2fed6306..2b457e9028 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -12,6 +12,7 @@
 import time
 from glob import glob
 
+import magnum as mn
 import pytest
 import torch
 import yaml
@@ -19,9 +20,11 @@
 
 import habitat
 import habitat.datasets.rearrange.run_episode_generator as rr_gen
+import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle
 import habitat.tasks.rearrange.rearrange_sim
 import habitat.tasks.rearrange.rearrange_task
 import habitat.utils.env_utils
+import habitat_sim
 from habitat.config.default import _HABITAT_CFG_DIR, get_config
 from habitat.core.embodied_task import Episode
 from habitat.core.environments import get_env_class
@@ -238,3 +241,69 @@ def test_tp_srl(test_cfg_path, mode):
     # Deinit processes group
     if torch.distributed.is_initialized():
         torch.distributed.destroy_process_group()
+
+
+@pytest.mark.skipif(
+    not osp.exists("data/test_assets/"),
+    reason="This test requires habitat-sim test assets.",
+)
+def test_receptacle_parsing():
+    ##########################
+    # Test Mesh Receptacles
+    ##########################
+    # 1. Load the parameterized scene
+    sim_settings = habitat_sim.utils.settings.default_sim_settings.copy()
+    # sim_settings["scene"] = "data/test_assets/scenes/simple_room.stage_config.json"
+    sim_settings[
+        "scene"
+    ] = "data/test_assets/scenes/simple_room.stage_config.json"
+    sim_settings["sensor_height"] = 0
+    sim_settings["enable_physics"] = True
+    cfg = habitat_sim.utils.settings.make_cfg(sim_settings)
+    with habitat_sim.Simulator(cfg) as sim:
+
+        # load test assets
+        sim.metadata_mediator.object_template_manager.load_configs(
+            "data/test_assets/objects/chair.object_config.json"
+        )
+        # TODO: add an AO w/ receptacles also
+
+        # test quick receptacle listing:
+        list_receptacles = hab_receptacle.get_all_scenedataset_receptacles(sim)
+        print(f"list_receptacles = {list_receptacles}")
+        # receptacles from stage configs:
+        assert "receptacle_aabb_simpleroom_test" in list_receptacles["stage"]['data/test_assets/scenes/simple_room.stage_config.json']
+        assert "receptacle_mesh_simpleroom_test" in list_receptacles["stage"]['data/test_assets/scenes/simple_room.stage_config.json']
+        # receptacles from rigid object configs:
+        assert "receptacle_aabb_chair_test" in list_receptacles["rigid"]['data/test_assets/objects/chair.object_config.json']
+        assert "receptacle_mesh_chair_test" in list_receptacles["rigid"]['data/test_assets/objects/chair.object_config.json']
+        # TODO: receptacles from articulated object configs:
+        # assert "" in list_receptacles["articulated"]
+
+        #add the chair to the scene
+        chair_template_handle = sim.metadata_mediator.object_template_manager.get_template_handles("chair")[0]
+        chair_obj = sim.get_rigid_object_manager().add_object_by_template_handle(chair_template_handle)
+
+        # parse the metadata into Receptacle objects
+        test_receptacles = hab_receptacle.find_receptacles(sim)
+
+        #test the Receptacle instances
+        num_test_samples = 10
+        for receptacle in test_receptacles:
+            if receptacle.name == "receptacle_aabb_chair_test":
+                assert type(receptacle) is hab_receptacle.AABBReceptacle
+            elif receptacle.name == "receptacle_mesh_chair_test":
+                assert type(receptacle) is hab_receptacle.TriangleMeshReceptacle
+            elif receptacle.name == "receptacle_aabb_simpleroom_test":
+                assert type(receptacle) is hab_receptacle.AABBReceptacle
+            elif receptacle.name == "receptacle_mesh_simpleroom_test":
+                assert type(receptacle) is hab_receptacle.TriangleMeshReceptacle
+            else:
+                raise AssertionError(
+                    f"Unknown Receptacle '{receptacle.name}' detected. Update unit test golden values if this is expected."
+                )
+
+        #   if type(receptacle) is hab_receptacle.AABBReceptacle:
+        #        for _six in range(num_test_samples):
+        #            sample_point = receptacle.sample_uniform_global(sim,sample_region_scale=1.0)
+                    

From 1bc1aad9e9a818af26e0315c916124fef695a7b4 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 31 Jan 2023 16:28:13 -0800
Subject: [PATCH 09/27] add some tests for sampling accuracy

---
 test/test_rearrange_task.py | 122 ++++++++++++++++++++++++++++++++++--
 1 file changed, 117 insertions(+), 5 deletions(-)

diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 2b457e9028..1afd9f5d0b 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -13,6 +13,7 @@
 from glob import glob
 
 import magnum as mn
+import numpy as np
 import pytest
 import torch
 import yaml
@@ -253,7 +254,6 @@ def test_receptacle_parsing():
     ##########################
     # 1. Load the parameterized scene
     sim_settings = habitat_sim.utils.settings.default_sim_settings.copy()
-    # sim_settings["scene"] = "data/test_assets/scenes/simple_room.stage_config.json"
     sim_settings[
         "scene"
     ] = "data/test_assets/scenes/simple_room.stage_config.json"
@@ -283,6 +283,75 @@ def test_receptacle_parsing():
         #add the chair to the scene
         chair_template_handle = sim.metadata_mediator.object_template_manager.get_template_handles("chair")[0]
         chair_obj = sim.get_rigid_object_manager().add_object_by_template_handle(chair_template_handle)
+        
+        def randomize_obj_state():
+            chair_obj.translation = np.random.random(3)
+            chair_obj.rotation = habitat_sim.utils.common.random_quaternion()
+            #TODO: also randomize AO state here
+
+        def point_in_triangle_test(p,v0,v1,v2):
+            """
+            Return True if the point is in the triangle.
+            Algorithm: https://math.stackexchange.com/questions/51326/determining-if-an-arbitrary-point-lies-inside-a-triangle-defined-by-three-points
+            """
+            #1. move the triangle such that point is the origin
+            a = v0-p
+            b = v1-p
+            c = v2-p
+
+            #check that the origin is planar
+            tri_norm = mn.math.cross(c-a, b-a)
+            if abs(mn.math.dot(a, tri_norm)) > 1e-7:
+                #print("ORIGIN is non-planar")
+                return False
+
+            #2. create 3 triangles with origin + pairs of vertices and compute the normals
+            u = mn.math.cross(b,c)
+            v = mn.math.cross(c,a)
+            w = mn.math.cross(a,b)
+
+            #3. check that all new triangle normals are aligned
+            if mn.math.dot(u,v) < 0.0:
+                return False
+            if mn.math.dot(u,w) < 0.0:
+                return False
+            if mn.math.dot(v,w) < 0.0:
+                return False
+            return True
+
+        #contrived triangle test
+        test_tri = (
+            mn.Vector3(0.0,0.0,1.0), 
+            mn.Vector3(0.0,1.0,0.0), 
+            mn.Vector3(0.0,0.0,0.0))
+        test_pairs = [
+            #corners
+            (mn.Vector3(0.0,0.0,1.0), True),
+            (mn.Vector3(0.0,0.99,0.0), True),
+            (mn.Vector3(0,0,0), True),
+            #inside planar
+            (mn.Vector3(0,0.49,0.49), True),
+            (mn.Vector3(0.0,0.2,0.2), True),
+            (mn.Vector3(0.0,0.2,0.4), True),
+            (mn.Vector3(0.0,0.15,0.3), True),
+            #outside but planar
+            (mn.Vector3(0,0,1.01), False),
+            (mn.Vector3(0,0,-0.01), False),
+            (mn.Vector3(0,0.51,0.51), False),
+            (mn.Vector3(0,-0.01,0.51), False),
+            (mn.Vector3(0,-0.01,-0.01), False),
+            #inside non-planar
+            (mn.Vector3(0.01,0,0), False),
+            (mn.Vector3(0.2,-0.01,0.51), False),
+            (mn.Vector3(-0.2,-0.01,-0.01), False),
+            (mn.Vector3(0.1,0.2,0.2), False),
+            (mn.Vector3(-0.01,0.2,0.2), False),
+            #test epsilon padding around normal
+            (mn.Vector3(1e-6,0,0), False),
+            (mn.Vector3(1e-30,0,0), True),
+        ]
+        for test_pair in test_pairs:
+            assert point_in_triangle_test(test_pair[0], test_tri[0], test_tri[1], test_tri[2]) == test_pair[1]
 
         # parse the metadata into Receptacle objects
         test_receptacles = hab_receptacle.find_receptacles(sim)
@@ -290,6 +359,7 @@ def test_receptacle_parsing():
         #test the Receptacle instances
         num_test_samples = 10
         for receptacle in test_receptacles:
+            #check for contents and correct type parsing
             if receptacle.name == "receptacle_aabb_chair_test":
                 assert type(receptacle) is hab_receptacle.AABBReceptacle
             elif receptacle.name == "receptacle_mesh_chair_test":
@@ -299,11 +369,53 @@ def test_receptacle_parsing():
             elif receptacle.name == "receptacle_mesh_simpleroom_test":
                 assert type(receptacle) is hab_receptacle.TriangleMeshReceptacle
             else:
+                #TODO: add AO receptacles
                 raise AssertionError(
                     f"Unknown Receptacle '{receptacle.name}' detected. Update unit test golden values if this is expected."
                 )
 
-        #   if type(receptacle) is hab_receptacle.AABBReceptacle:
-        #        for _six in range(num_test_samples):
-        #            sample_point = receptacle.sample_uniform_global(sim,sample_region_scale=1.0)
-                    
+            for _six in range(num_test_samples):
+                randomize_obj_state()
+                #check that parenting and global transforms are as expected:
+                parent_object = None
+                expected_global_transform = mn.Matrix4.identity_init()
+                global_transform = receptacle.get_global_transform(sim)
+                if receptacle.parent_object_handle is not None:
+                    parent_object = None
+                    if receptacle.parent_link is not None:
+                        #articulated object
+                        assert receptacle.is_parent_object_articulated
+                        parent_object = sim.get_articulated_object_manager().get_object_by_handle(receptacle.parent_object_handle)
+                        expected_global_transform = parent_object.get_link_scene_node(receptacle.parent_link).absolute_transformation()
+                    else:
+                        #rigid object
+                        assert not receptacle.is_parent_object_articulated
+                        parent_object = sim.get_rigid_object_manager().get_object_by_handle(receptacle.parent_object_handle)
+                        # NOTE: we use absolute transformation from the 2nd visual node (scaling node) and root of all render assets to correctly account for any COM shifting, re-orienting, or scaling which has been applied.
+                        expected_global_transform = parent_object.visual_scene_nodes[1].absolute_transformation()
+                    assert parent_object is not None
+                    assert np.allclose(global_transform, expected_global_transform, atol=1e-06)
+                else:
+                    #this is a stage Receptacle (global transform)
+                    if type(receptacle) is not hab_receptacle.AABBReceptacle:
+                        assert np.allclose(global_transform, expected_global_transform, atol=1e-06)
+                    else:
+                        #NOTE: global AABB Receptacles have special handling here which is not explicitly tested. See AABBReceptacle.get_global_transform()
+                        expected_global_transform = global_transform
+                        pass
+
+                for _six2 in range(num_test_samples):
+                    sample_point = receptacle.sample_uniform_global(sim,sample_region_scale=1.0)
+                    expected_local_sample_point = expected_global_transform.inverted().transform_point(sample_point)
+                    if type(receptacle) is hab_receptacle.AABBReceptacle:
+                        #check that the world->local sample point is contained in the local AABB
+                        assert receptacle.bounds.contains(expected_local_sample_point)
+                    elif type(receptacle) is hab_receptacle.TriangleMeshReceptacle:
+                        #check that the local point is within a mesh triangle
+                        in_mesh = False
+                        for f_ix in range(int(len(receptacle.mesh_data[1]) / 3)):
+                            verts = receptacle.get_face_verts(f_ix)
+                            if point_in_triangle_test(expected_local_sample_point, verts[0], verts[1], verts[2]):
+                                in_mesh = True
+                                break
+                        assert in_mesh, "The point must belong to a triangle of the local mesh to be valid."
\ No newline at end of file

From ecb498d5977220758f232f9a61377820b3e36555 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 31 Jan 2023 16:30:07 -0800
Subject: [PATCH 10/27] remove erroneous comment

---
 test/test_rearrange_task.py | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 1afd9f5d0b..8629faa805 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -249,9 +249,6 @@ def test_tp_srl(test_cfg_path, mode):
     reason="This test requires habitat-sim test assets.",
 )
 def test_receptacle_parsing():
-    ##########################
-    # Test Mesh Receptacles
-    ##########################
     # 1. Load the parameterized scene
     sim_settings = habitat_sim.utils.settings.default_sim_settings.copy()
     sim_settings[

From 8ca69f403536ce07febf49ae54086171ab03c6d5 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Wed, 1 Feb 2023 09:52:01 -0800
Subject: [PATCH 11/27] pre-commit changes

---
 .../datasets/rearrange/rearrange_generator.py |   2 +-
 .../datasets/rearrange/samplers/receptacle.py |  36 ++-
 test/test_rearrange_task.py                   | 235 ++++++++++++------
 3 files changed, 179 insertions(+), 94 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
index a58553bc6e..7e5366ebe3 100644
--- a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
@@ -930,7 +930,7 @@ def settle_sim(
 
         logger.info("----------------------------------------")
 
-        #generate debug images of all final object placements
+        # generate debug images of all final object placements
         if self._render_debug_obs and success:
             for obj in self.ep_sampled_objects:
                 self.vdb.peek_rigid_object(obj, peek_all_axis=True)
diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index fcd60697e9..b676c5adf3 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -108,12 +108,14 @@ def add_receptacle_visualization(
         return []
 
     @abstractmethod
-    def debug_draw(self, sim: habitat_sim.Simulator, color:Optional[mn.Color4]=None) -> None:
+    def debug_draw(
+        self, sim: habitat_sim.Simulator, color: Optional[mn.Color4] = None
+    ) -> None:
         """
         Render the Receptacle with DebugLineRender utility at the current frame.
         Must be called after each frame is rendered, before querying the image data.
 
-        :param sim: Simulator must be provided. 
+        :param sim: Simulator must be provided.
         :param color: Optionally provide wireframe color, otherwise magenta.
         """
 
@@ -138,12 +140,14 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
 
         return mn.Matrix4([[targ_T[j][i] for j in range(4)] for i in range(4)])
 
-    def debug_draw(self, sim: habitat_sim.Simulator, color:Optional[mn.Color4]=None) -> None:
+    def debug_draw(
+        self, sim: habitat_sim.Simulator, color: Optional[mn.Color4] = None
+    ) -> None:
         """
         Render the Receptacle with DebugLineRender utility at the current frame.
         Must be called after each frame is rendered, before querying the image data.
 
-        :param sim: Simulator must be provided. 
+        :param sim: Simulator must be provided.
         :param color: Optionally provide wireframe color, otherwise magenta.
         """
         # TODO:
@@ -269,12 +273,14 @@ def add_receptacle_visualization(
             )
         return [box_obj]
 
-    def debug_draw(self, sim: habitat_sim.Simulator, color:Optional[mn.Color4]=None)->None:
+    def debug_draw(
+        self, sim: habitat_sim.Simulator, color: Optional[mn.Color4] = None
+    ) -> None:
         """
         Render the AABBReceptacle with DebugLineRender utility at the current frame.
         Must be called after each frame is rendered, before querying the image data.
 
-        :param sim: Simulator must be provided. 
+        :param sim: Simulator must be provided.
         :param color: Optionally provide wireframe color, otherwise magenta.
         """
         # draw the box
@@ -317,7 +323,7 @@ def __init__(
         )  # normalized float weights for each triangle for sampling
         assert len(mesh_data[1]) % 3 == 0, "must be triangles"
 
-        #pre-compute the normalized cumulative area of all triangle faces for later sampling
+        # pre-compute the normalized cumulative area of all triangle faces for later sampling
         self.total_area = 0
         for f_ix in range(int(len(mesh_data[1]) / 3)):
             v = self.get_face_verts(f_ix)
@@ -359,7 +365,7 @@ def sample_area_weighted_triangle(self) -> int:
         Returns a random triangle index sampled with area weighting.
         """
 
-        def find_ge(a:List[Any], x)->Any:
+        def find_ge(a: List[Any], x) -> Any:
             "Find leftmost item greater than or equal to x"
             from bisect import bisect_left
 
@@ -401,13 +407,15 @@ def sample_uniform_local(
 
         return rand_point
 
-    def debug_draw(self, sim:habitat_sim.Simulator, color:Optional[mn.Color4]=None)->None:
+    def debug_draw(
+        self, sim: habitat_sim.Simulator, color: Optional[mn.Color4] = None
+    ) -> None:
         """
         Render the Receptacle with DebugLineRender utility at the current frame.
         Draws the Receptacle mesh.
         Must be called after each frame is rendered, before querying the image data.
 
-        :param sim: Simulator must be provided. 
+        :param sim: Simulator must be provided.
         :param color: Optionally provide wireframe color, otherwise magenta.
         """
         # draw all mesh triangles
@@ -425,14 +433,16 @@ def debug_draw(self, sim:habitat_sim.Simulator, color:Optional[mn.Color4]=None)-
         dblr.pop_transform()
 
 
-def get_all_scenedataset_receptacles(sim:habitat_sim.Simulator) -> Dict[str, Dict[str, List[str]]]:
+def get_all_scenedataset_receptacles(
+    sim: habitat_sim.Simulator,
+) -> Dict[str, Dict[str, List[str]]]:
     """
     Scrapes the active SceneDataset from a Simulator for all receptacle names defined in rigid/articulated object and stage templates for investigation and preview purposes.
     Note this will not include scene-specific overrides defined in scene_config.json files. Only receptacles defined in object_config.json, ao_config.json, and stage_config.json files or added programmatically to associated Attributes objects will be found.
 
     Returns a dict with keys {"stage", "rigid", "articulated"} mapping object template handles to lists of receptacle names.
 
-    :param sim: Simulator must be provided. 
+    :param sim: Simulator must be provided.
     """
     # cache the rigid and articulated receptacles seperately
     receptacles: Dict[str, Dict[str, List[str]]] = {
@@ -653,7 +663,7 @@ def find_receptacles(
     """
     Scrape and return a list of all Receptacles defined in the metadata belonging to the scene's currently instanced objects.
 
-    :param sim: Simulator must be provided. 
+    :param sim: Simulator must be provided.
     """
 
     obj_mgr = sim.get_rigid_object_manager()
diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 8629faa805..73f98343a7 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -269,150 +269,225 @@ def test_receptacle_parsing():
         list_receptacles = hab_receptacle.get_all_scenedataset_receptacles(sim)
         print(f"list_receptacles = {list_receptacles}")
         # receptacles from stage configs:
-        assert "receptacle_aabb_simpleroom_test" in list_receptacles["stage"]['data/test_assets/scenes/simple_room.stage_config.json']
-        assert "receptacle_mesh_simpleroom_test" in list_receptacles["stage"]['data/test_assets/scenes/simple_room.stage_config.json']
+        assert (
+            "receptacle_aabb_simpleroom_test"
+            in list_receptacles["stage"][
+                "data/test_assets/scenes/simple_room.stage_config.json"
+            ]
+        )
+        assert (
+            "receptacle_mesh_simpleroom_test"
+            in list_receptacles["stage"][
+                "data/test_assets/scenes/simple_room.stage_config.json"
+            ]
+        )
         # receptacles from rigid object configs:
-        assert "receptacle_aabb_chair_test" in list_receptacles["rigid"]['data/test_assets/objects/chair.object_config.json']
-        assert "receptacle_mesh_chair_test" in list_receptacles["rigid"]['data/test_assets/objects/chair.object_config.json']
+        assert (
+            "receptacle_aabb_chair_test"
+            in list_receptacles["rigid"][
+                "data/test_assets/objects/chair.object_config.json"
+            ]
+        )
+        assert (
+            "receptacle_mesh_chair_test"
+            in list_receptacles["rigid"][
+                "data/test_assets/objects/chair.object_config.json"
+            ]
+        )
         # TODO: receptacles from articulated object configs:
         # assert "" in list_receptacles["articulated"]
 
-        #add the chair to the scene
-        chair_template_handle = sim.metadata_mediator.object_template_manager.get_template_handles("chair")[0]
-        chair_obj = sim.get_rigid_object_manager().add_object_by_template_handle(chair_template_handle)
-        
+        # add the chair to the scene
+        chair_template_handle = (
+            sim.metadata_mediator.object_template_manager.get_template_handles(
+                "chair"
+            )[0]
+        )
+        chair_obj = (
+            sim.get_rigid_object_manager().add_object_by_template_handle(
+                chair_template_handle
+            )
+        )
+
         def randomize_obj_state():
             chair_obj.translation = np.random.random(3)
             chair_obj.rotation = habitat_sim.utils.common.random_quaternion()
-            #TODO: also randomize AO state here
+            # TODO: also randomize AO state here
 
-        def point_in_triangle_test(p,v0,v1,v2):
+        def point_in_triangle_test(p, v0, v1, v2):
             """
             Return True if the point is in the triangle.
             Algorithm: https://math.stackexchange.com/questions/51326/determining-if-an-arbitrary-point-lies-inside-a-triangle-defined-by-three-points
             """
-            #1. move the triangle such that point is the origin
-            a = v0-p
-            b = v1-p
-            c = v2-p
+            # 1. move the triangle such that point is the origin
+            a = v0 - p
+            b = v1 - p
+            c = v2 - p
 
-            #check that the origin is planar
-            tri_norm = mn.math.cross(c-a, b-a)
+            # check that the origin is planar
+            tri_norm = mn.math.cross(c - a, b - a)
             if abs(mn.math.dot(a, tri_norm)) > 1e-7:
-                #print("ORIGIN is non-planar")
+                # print("ORIGIN is non-planar")
                 return False
 
-            #2. create 3 triangles with origin + pairs of vertices and compute the normals
-            u = mn.math.cross(b,c)
-            v = mn.math.cross(c,a)
-            w = mn.math.cross(a,b)
+            # 2. create 3 triangles with origin + pairs of vertices and compute the normals
+            u = mn.math.cross(b, c)
+            v = mn.math.cross(c, a)
+            w = mn.math.cross(a, b)
 
-            #3. check that all new triangle normals are aligned
-            if mn.math.dot(u,v) < 0.0:
+            # 3. check that all new triangle normals are aligned
+            if mn.math.dot(u, v) < 0.0:
                 return False
-            if mn.math.dot(u,w) < 0.0:
+            if mn.math.dot(u, w) < 0.0:
                 return False
-            if mn.math.dot(v,w) < 0.0:
+            if mn.math.dot(v, w) < 0.0:
                 return False
             return True
 
-        #contrived triangle test
+        # contrived triangle test
         test_tri = (
-            mn.Vector3(0.0,0.0,1.0), 
-            mn.Vector3(0.0,1.0,0.0), 
-            mn.Vector3(0.0,0.0,0.0))
+            mn.Vector3(0.0, 0.0, 1.0),
+            mn.Vector3(0.0, 1.0, 0.0),
+            mn.Vector3(0.0, 0.0, 0.0),
+        )
         test_pairs = [
-            #corners
-            (mn.Vector3(0.0,0.0,1.0), True),
-            (mn.Vector3(0.0,0.99,0.0), True),
-            (mn.Vector3(0,0,0), True),
-            #inside planar
-            (mn.Vector3(0,0.49,0.49), True),
-            (mn.Vector3(0.0,0.2,0.2), True),
-            (mn.Vector3(0.0,0.2,0.4), True),
-            (mn.Vector3(0.0,0.15,0.3), True),
-            #outside but planar
-            (mn.Vector3(0,0,1.01), False),
-            (mn.Vector3(0,0,-0.01), False),
-            (mn.Vector3(0,0.51,0.51), False),
-            (mn.Vector3(0,-0.01,0.51), False),
-            (mn.Vector3(0,-0.01,-0.01), False),
-            #inside non-planar
-            (mn.Vector3(0.01,0,0), False),
-            (mn.Vector3(0.2,-0.01,0.51), False),
-            (mn.Vector3(-0.2,-0.01,-0.01), False),
-            (mn.Vector3(0.1,0.2,0.2), False),
-            (mn.Vector3(-0.01,0.2,0.2), False),
-            #test epsilon padding around normal
-            (mn.Vector3(1e-6,0,0), False),
-            (mn.Vector3(1e-30,0,0), True),
+            # corners
+            (mn.Vector3(0.0, 0.0, 1.0), True),
+            (mn.Vector3(0.0, 0.99, 0.0), True),
+            (mn.Vector3(0, 0, 0), True),
+            # inside planar
+            (mn.Vector3(0, 0.49, 0.49), True),
+            (mn.Vector3(0.0, 0.2, 0.2), True),
+            (mn.Vector3(0.0, 0.2, 0.4), True),
+            (mn.Vector3(0.0, 0.15, 0.3), True),
+            # outside but planar
+            (mn.Vector3(0, 0, 1.01), False),
+            (mn.Vector3(0, 0, -0.01), False),
+            (mn.Vector3(0, 0.51, 0.51), False),
+            (mn.Vector3(0, -0.01, 0.51), False),
+            (mn.Vector3(0, -0.01, -0.01), False),
+            # inside non-planar
+            (mn.Vector3(0.01, 0, 0), False),
+            (mn.Vector3(0.2, -0.01, 0.51), False),
+            (mn.Vector3(-0.2, -0.01, -0.01), False),
+            (mn.Vector3(0.1, 0.2, 0.2), False),
+            (mn.Vector3(-0.01, 0.2, 0.2), False),
+            # test epsilon padding around normal
+            (mn.Vector3(1e-6, 0, 0), False),
+            (mn.Vector3(1e-30, 0, 0), True),
         ]
         for test_pair in test_pairs:
-            assert point_in_triangle_test(test_pair[0], test_tri[0], test_tri[1], test_tri[2]) == test_pair[1]
+            assert (
+                point_in_triangle_test(
+                    test_pair[0], test_tri[0], test_tri[1], test_tri[2]
+                )
+                == test_pair[1]
+            )
 
         # parse the metadata into Receptacle objects
         test_receptacles = hab_receptacle.find_receptacles(sim)
 
-        #test the Receptacle instances
+        # test the Receptacle instances
         num_test_samples = 10
         for receptacle in test_receptacles:
-            #check for contents and correct type parsing
+            # check for contents and correct type parsing
             if receptacle.name == "receptacle_aabb_chair_test":
                 assert type(receptacle) is hab_receptacle.AABBReceptacle
             elif receptacle.name == "receptacle_mesh_chair_test":
-                assert type(receptacle) is hab_receptacle.TriangleMeshReceptacle
+                assert (
+                    type(receptacle) is hab_receptacle.TriangleMeshReceptacle
+                )
             elif receptacle.name == "receptacle_aabb_simpleroom_test":
                 assert type(receptacle) is hab_receptacle.AABBReceptacle
             elif receptacle.name == "receptacle_mesh_simpleroom_test":
-                assert type(receptacle) is hab_receptacle.TriangleMeshReceptacle
+                assert (
+                    type(receptacle) is hab_receptacle.TriangleMeshReceptacle
+                )
             else:
-                #TODO: add AO receptacles
+                # TODO: add AO receptacles
                 raise AssertionError(
                     f"Unknown Receptacle '{receptacle.name}' detected. Update unit test golden values if this is expected."
                 )
 
             for _six in range(num_test_samples):
                 randomize_obj_state()
-                #check that parenting and global transforms are as expected:
+                # check that parenting and global transforms are as expected:
                 parent_object = None
                 expected_global_transform = mn.Matrix4.identity_init()
                 global_transform = receptacle.get_global_transform(sim)
                 if receptacle.parent_object_handle is not None:
                     parent_object = None
                     if receptacle.parent_link is not None:
-                        #articulated object
+                        # articulated object
                         assert receptacle.is_parent_object_articulated
-                        parent_object = sim.get_articulated_object_manager().get_object_by_handle(receptacle.parent_object_handle)
-                        expected_global_transform = parent_object.get_link_scene_node(receptacle.parent_link).absolute_transformation()
+                        parent_object = sim.get_articulated_object_manager().get_object_by_handle(
+                            receptacle.parent_object_handle
+                        )
+                        expected_global_transform = (
+                            parent_object.get_link_scene_node(
+                                receptacle.parent_link
+                            ).absolute_transformation()
+                        )
                     else:
-                        #rigid object
+                        # rigid object
                         assert not receptacle.is_parent_object_articulated
-                        parent_object = sim.get_rigid_object_manager().get_object_by_handle(receptacle.parent_object_handle)
+                        parent_object = sim.get_rigid_object_manager().get_object_by_handle(
+                            receptacle.parent_object_handle
+                        )
                         # NOTE: we use absolute transformation from the 2nd visual node (scaling node) and root of all render assets to correctly account for any COM shifting, re-orienting, or scaling which has been applied.
-                        expected_global_transform = parent_object.visual_scene_nodes[1].absolute_transformation()
+                        expected_global_transform = (
+                            parent_object.visual_scene_nodes[
+                                1
+                            ].absolute_transformation()
+                        )
                     assert parent_object is not None
-                    assert np.allclose(global_transform, expected_global_transform, atol=1e-06)
+                    assert np.allclose(
+                        global_transform, expected_global_transform, atol=1e-06
+                    )
                 else:
-                    #this is a stage Receptacle (global transform)
+                    # this is a stage Receptacle (global transform)
                     if type(receptacle) is not hab_receptacle.AABBReceptacle:
-                        assert np.allclose(global_transform, expected_global_transform, atol=1e-06)
+                        assert np.allclose(
+                            global_transform,
+                            expected_global_transform,
+                            atol=1e-06,
+                        )
                     else:
-                        #NOTE: global AABB Receptacles have special handling here which is not explicitly tested. See AABBReceptacle.get_global_transform()
+                        # NOTE: global AABB Receptacles have special handling here which is not explicitly tested. See AABBReceptacle.get_global_transform()
                         expected_global_transform = global_transform
-                        pass
 
                 for _six2 in range(num_test_samples):
-                    sample_point = receptacle.sample_uniform_global(sim,sample_region_scale=1.0)
-                    expected_local_sample_point = expected_global_transform.inverted().transform_point(sample_point)
+                    sample_point = receptacle.sample_uniform_global(
+                        sim, sample_region_scale=1.0
+                    )
+                    expected_local_sample_point = (
+                        expected_global_transform.inverted().transform_point(
+                            sample_point
+                        )
+                    )
                     if type(receptacle) is hab_receptacle.AABBReceptacle:
-                        #check that the world->local sample point is contained in the local AABB
-                        assert receptacle.bounds.contains(expected_local_sample_point)
-                    elif type(receptacle) is hab_receptacle.TriangleMeshReceptacle:
-                        #check that the local point is within a mesh triangle
+                        # check that the world->local sample point is contained in the local AABB
+                        assert receptacle.bounds.contains(
+                            expected_local_sample_point
+                        )
+                    elif (
+                        type(receptacle)
+                        is hab_receptacle.TriangleMeshReceptacle
+                    ):
+                        # check that the local point is within a mesh triangle
                         in_mesh = False
-                        for f_ix in range(int(len(receptacle.mesh_data[1]) / 3)):
+                        for f_ix in range(
+                            int(len(receptacle.mesh_data[1]) / 3)
+                        ):
                             verts = receptacle.get_face_verts(f_ix)
-                            if point_in_triangle_test(expected_local_sample_point, verts[0], verts[1], verts[2]):
+                            if point_in_triangle_test(
+                                expected_local_sample_point,
+                                verts[0],
+                                verts[1],
+                                verts[2],
+                            ):
                                 in_mesh = True
                                 break
-                        assert in_mesh, "The point must belong to a triangle of the local mesh to be valid."
\ No newline at end of file
+                        assert (
+                            in_mesh
+                        ), "The point must belong to a triangle of the local mesh to be valid."

From 768645a1570d154e39436aee9ed79bfe99d00baa Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Wed, 1 Feb 2023 09:55:15 -0800
Subject: [PATCH 12/27] numpy typing fix

---
 habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index b676c5adf3..aed7a8c294 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -343,13 +343,13 @@ def __init__(
                 ] += self.area_weighted_accumulator[f_ix - 1]
         # print(self.area_weighted_accumulator)
 
-    def get_face_verts(self, f_ix: int) -> List[np.array]:
+    def get_face_verts(self, f_ix: int) -> List[np.ndarray]:
         """
         Get all three vertices of a mesh triangle given it's face index as a list of numpy arrays.
 
         :param f_ix: The index of the mesh triangle.
         """
-        verts: List[np.array] = []
+        verts: List[np.ndarray] = []
         for ix in range(3):
             verts.append(
                 np.array(

From c9d5eeadc14ac4f1a196fbda6536a3d59f3b0834 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Wed, 1 Feb 2023 11:54:45 -0800
Subject: [PATCH 13/27] move triangle sampling and testing logic to
 geometry_utils and move/add tests

---
 .../datasets/rearrange/samplers/receptacle.py |  9 +--
 habitat-lab/habitat/utils/geometry_utils.py   | 51 +++++++++++++
 test/test_geom_utils.py                       | 73 +++++++++++++++++++
 test/test_rearrange_task.py                   | 71 +-----------------
 4 files changed, 127 insertions(+), 77 deletions(-)
 create mode 100644 test/test_geom_utils.py

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index aed7a8c294..83742275b1 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -17,6 +17,7 @@
 import habitat_sim
 from habitat.core.logging import logger
 from habitat.sims.habitat_simulator.sim_utilities import add_wire_box
+from habitat.utils.geometry_utils import random_triangle_point
 
 
 class Receptacle(ABC):
@@ -396,14 +397,8 @@ def sample_uniform_local(
         tri_index = self.sample_area_weighted_triangle()
 
         # then sample a random point in the triangle
-        # https://math.stackexchange.com/questions/538458/how-to-sample-points-on-a-triangle-surface-in-3d
-        coef1 = random.random()
-        coef2 = random.random()
-        if coef1 + coef2 >= 1:
-            coef1 = 1 - coef1
-            coef2 = 1 - coef2
         v = self.get_face_verts(f_ix=tri_index)
-        rand_point = v[0] + coef1 * (v[1] - v[0]) + coef2 * (v[2] - v[0])
+        rand_point = random_triangle_point(v[0], v[1], v[2])
 
         return rand_point
 
diff --git a/habitat-lab/habitat/utils/geometry_utils.py b/habitat-lab/habitat/utils/geometry_utils.py
index 0fd70ba423..00da19f4f0 100644
--- a/habitat-lab/habitat/utils/geometry_utils.py
+++ b/habitat-lab/habitat/utils/geometry_utils.py
@@ -4,6 +4,7 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
+import random
 from typing import List, Tuple, Union
 
 import numpy as np
@@ -115,3 +116,53 @@ def agent_state_target2ref(
     )
 
     return (rotation_in_ref_coordinate, position_in_ref_coordinate)
+
+
+def random_triangle_point(
+    v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
+) -> np.ndarray:
+    """
+    Sample a random point from a triangle given its vertices.
+    """
+
+    # reference: https://mathworld.wolfram.com/TrianglePointPicking.html
+    coef1 = random.random()
+    coef2 = random.random()
+    if coef1 + coef2 >= 1:
+        # transform "outside" points back inside
+        coef1 = 1 - coef1
+        coef2 = 1 - coef2
+    return v0 + coef1 * (v1 - v0) + coef2 * (v2 - v0)
+
+
+def point_in_triangle_test(
+    p: np.ndarray, v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
+) -> bool:
+    """
+    Return True if the point, p, is in the triangle defined by vertices v0,v1,v2.
+    Algorithm: https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/point_in_triangle.html
+    """
+    # 1. move the triangle such that point is the origin
+    a = v0 - p
+    b = v1 - p
+    c = v2 - p
+
+    # 2. check that the origin is planar
+    tri_norm = np.cross(c - a, b - a)
+    # NOTE: small epsilon error allowed here empirically
+    if abs(np.dot(a, tri_norm)) > 1e-7:
+        return False
+
+    # 3. create 3 triangles with origin + pairs of vertices and compute the normals
+    u = np.cross(b, c)
+    v = np.cross(c, a)
+    w = np.cross(a, b)
+
+    # 4. check that all new triangle normals are aligned
+    if np.dot(u, v) < 0.0:
+        return False
+    if np.dot(u, w) < 0.0:
+        return False
+    if np.dot(v, w) < 0.0:
+        return False
+    return True
diff --git a/test/test_geom_utils.py b/test/test_geom_utils.py
new file mode 100644
index 0000000000..45b1f39c66
--- /dev/null
+++ b/test/test_geom_utils.py
@@ -0,0 +1,73 @@
+#!/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 numpy as np
+
+from habitat.utils.geometry_utils import (
+    point_in_triangle_test,
+    random_triangle_point,
+)
+
+
+def test_point_in_triangle_test():
+    # contrived triangle test
+    test_tri = (
+        np.array([0.0, 0.0, 1.0]),
+        np.array([0.0, 1.0, 0.0]),
+        np.array([0.0, 0.0, 0.0]),
+    )
+    test_pairs = [
+        # corners
+        (np.array([0.0, 0.0, 1.0]), True),
+        (np.array([0.0, 0.99, 0.0]), True),
+        (np.array([0, 0, 0]), True),
+        # inside planar
+        (np.array([0, 0.49, 0.49]), True),
+        (np.array([0.0, 0.2, 0.2]), True),
+        (np.array([0.0, 0.2, 0.4]), True),
+        (np.array([0.0, 0.15, 0.3]), True),
+        # outside but planar
+        (np.array([0, 0, 1.01]), False),
+        (np.array([0, 0, -0.01]), False),
+        (np.array([0, 0.51, 0.51]), False),
+        (np.array([0, -0.01, 0.51]), False),
+        (np.array([0, -0.01, -0.01]), False),
+        # inside non-planar
+        (np.array([0.01, 0, 0]), False),
+        (np.array([0.2, -0.01, 0.51]), False),
+        (np.array([-0.2, -0.01, -0.01]), False),
+        (np.array([0.1, 0.2, 0.2]), False),
+        (np.array([-0.01, 0.2, 0.2]), False),
+        # test epsilon padding around normal
+        (np.array([1e-6, 0.1, 0.1]), False),
+        (np.array([1e-7, 0.1, 0.1]), True),
+    ]
+    for test_pair in test_pairs:
+        assert (
+            point_in_triangle_test(
+                test_pair[0], test_tri[0], test_tri[1], test_tri[2]
+            )
+            == test_pair[1]
+        )
+
+
+def test_random_triangle_point():
+    # sample random points from random triangles, all should return True
+    num_tris = 5
+    num_samples = 10000
+    for _tri in range(num_tris):
+        v = [np.random.random(3) * 2 - np.ones(3) for _ in range(3)]
+        sample_centroid = np.zeros(3)
+        for _samp in range(num_samples):
+            tri_point = random_triangle_point(v[0], v[1], v[2])
+            assert point_in_triangle_test(tri_point, v[0], v[1], v[2])
+            sample_centroid += tri_point
+        # check uniformity of distribution by comparing sample centroid and triangle centroid
+        sample_centroid /= num_samples
+        true_centroid = (v[0] + v[1] + v[2]) / 3.0
+        # print(np.linalg.norm(sample_centroid-true_centroid))
+        # NOTE: need to be loose here because sample size is low
+        assert np.allclose(sample_centroid, true_centroid, atol=0.01)
diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 73f98343a7..2ab71ac33a 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -32,6 +32,7 @@
 from habitat.core.logging import logger
 from habitat.datasets.rearrange.rearrange_dataset import RearrangeDatasetV0
 from habitat.tasks.rearrange.multi_task.composite_task import CompositeTask
+from habitat.utils.geometry_utils import point_in_triangle_test
 from habitat_baselines.config.default import get_config as baselines_get_config
 from habitat_baselines.rl.ddppo.ddp_utils import find_free_port
 from habitat_baselines.run import run_exp
@@ -314,76 +315,6 @@ def randomize_obj_state():
             chair_obj.rotation = habitat_sim.utils.common.random_quaternion()
             # TODO: also randomize AO state here
 
-        def point_in_triangle_test(p, v0, v1, v2):
-            """
-            Return True if the point is in the triangle.
-            Algorithm: https://math.stackexchange.com/questions/51326/determining-if-an-arbitrary-point-lies-inside-a-triangle-defined-by-three-points
-            """
-            # 1. move the triangle such that point is the origin
-            a = v0 - p
-            b = v1 - p
-            c = v2 - p
-
-            # check that the origin is planar
-            tri_norm = mn.math.cross(c - a, b - a)
-            if abs(mn.math.dot(a, tri_norm)) > 1e-7:
-                # print("ORIGIN is non-planar")
-                return False
-
-            # 2. create 3 triangles with origin + pairs of vertices and compute the normals
-            u = mn.math.cross(b, c)
-            v = mn.math.cross(c, a)
-            w = mn.math.cross(a, b)
-
-            # 3. check that all new triangle normals are aligned
-            if mn.math.dot(u, v) < 0.0:
-                return False
-            if mn.math.dot(u, w) < 0.0:
-                return False
-            if mn.math.dot(v, w) < 0.0:
-                return False
-            return True
-
-        # contrived triangle test
-        test_tri = (
-            mn.Vector3(0.0, 0.0, 1.0),
-            mn.Vector3(0.0, 1.0, 0.0),
-            mn.Vector3(0.0, 0.0, 0.0),
-        )
-        test_pairs = [
-            # corners
-            (mn.Vector3(0.0, 0.0, 1.0), True),
-            (mn.Vector3(0.0, 0.99, 0.0), True),
-            (mn.Vector3(0, 0, 0), True),
-            # inside planar
-            (mn.Vector3(0, 0.49, 0.49), True),
-            (mn.Vector3(0.0, 0.2, 0.2), True),
-            (mn.Vector3(0.0, 0.2, 0.4), True),
-            (mn.Vector3(0.0, 0.15, 0.3), True),
-            # outside but planar
-            (mn.Vector3(0, 0, 1.01), False),
-            (mn.Vector3(0, 0, -0.01), False),
-            (mn.Vector3(0, 0.51, 0.51), False),
-            (mn.Vector3(0, -0.01, 0.51), False),
-            (mn.Vector3(0, -0.01, -0.01), False),
-            # inside non-planar
-            (mn.Vector3(0.01, 0, 0), False),
-            (mn.Vector3(0.2, -0.01, 0.51), False),
-            (mn.Vector3(-0.2, -0.01, -0.01), False),
-            (mn.Vector3(0.1, 0.2, 0.2), False),
-            (mn.Vector3(-0.01, 0.2, 0.2), False),
-            # test epsilon padding around normal
-            (mn.Vector3(1e-6, 0, 0), False),
-            (mn.Vector3(1e-30, 0, 0), True),
-        ]
-        for test_pair in test_pairs:
-            assert (
-                point_in_triangle_test(
-                    test_pair[0], test_tri[0], test_tri[1], test_tri[2]
-                )
-                == test_pair[1]
-            )
-
         # parse the metadata into Receptacle objects
         test_receptacles = hab_receptacle.find_receptacles(sim)
 

From 28ee901aea3f31dbf18907c6184397b139fd5f2e Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Wed, 1 Feb 2023 12:01:06 -0800
Subject: [PATCH 14/27] formatting from black 23.1

---
 test/test_rearrange_task.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 2ab71ac33a..2d6df2fb79 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -259,7 +259,6 @@ def test_receptacle_parsing():
     sim_settings["enable_physics"] = True
     cfg = habitat_sim.utils.settings.make_cfg(sim_settings)
     with habitat_sim.Simulator(cfg) as sim:
-
         # load test assets
         sim.metadata_mediator.object_template_manager.load_configs(
             "data/test_assets/objects/chair.object_config.json"

From 59fecd1be2e8a646354b32973f934aeb8043aa0c Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Fri, 10 Feb 2023 10:40:52 -0800
Subject: [PATCH 15/27] use magnum trade importer

---
 .../datasets/rearrange/samplers/receptacle.py | 61 +++++++------------
 1 file changed, 23 insertions(+), 38 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index 83742275b1..2644c3ac82 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -342,7 +342,6 @@ def __init__(
                 self.area_weighted_accumulator[
                     f_ix
                 ] += self.area_weighted_accumulator[f_ix - 1]
-        # print(self.area_weighted_accumulator)
 
     def get_face_verts(self, f_ix: int) -> List[np.ndarray]:
         """
@@ -452,7 +451,7 @@ def get_all_scenedataset_receptacles(
         stage_template = stm.get_template_by_handle(template_handle)
         for item in stage_template.get_user_config().get_subconfig_keys():
             if item.startswith("receptacle_"):
-                print(
+                logger.info(
                     f"template file_directory = {stage_template.file_directory}"
                 )
                 if template_handle not in receptacles["stage"]:
@@ -465,7 +464,7 @@ def get_all_scenedataset_receptacles(
         obj_template = rotm.get_template_by_handle(template_handle)
         for item in obj_template.get_user_config().get_subconfig_keys():
             if item.startswith("receptacle_"):
-                print(
+                logger.info(
                     f"template file_directory = {obj_template.file_directory}"
                 )
                 if template_handle not in receptacles["rigid"]:
@@ -488,44 +487,30 @@ def get_all_scenedataset_receptacles(
 
 def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
     """
-    Returns a Tuple of (verts,indices) from a ply mesh.
-    TODO: This could be replaced by a standard importer, but I didn't want to add additional dependencies for such as small feature.
+    Returns a Tuple of (verts,indices) from a ply mesh using magnum trade importer.
 
     :param ply_file: The input PLY mesh file. NOTE: must contain only triangles.
     """
-    mesh_data: Tuple[List[mn.Vector3], List[int]] = ([], [])
-    with open(ply_file) as f:
-        lines = [line.rstrip() for line in f]
-        assert lines[0] == "ply", f"Must be PLY format. '{ply_file}'"
-        assert "format ascii" in lines[1], f"Must be ascii PLY. '{ply_file}'"
-        # parse the header
-        line_index = 2
-        num_verts = 0
-        num_faces = 0
-        while line_index < len(lines):
-            if lines[line_index].startswith("element vertex"):
-                num_verts = int(lines[line_index][14:])
-            elif lines[line_index].startswith("element face"):
-                num_faces = int(lines[line_index][12:])
-            elif lines[line_index] == "end_header":
-                # done parsing header
-                line_index += 1
-                break
-            line_index += 1
-        assert (
-            len(lines) - line_index == num_verts + num_faces
-        ), f"Lines after header ({len(lines) - line_index}) should agree with forward declared content. {num_verts} verts and {num_faces} faces expected. '{ply_file}'"
-        # parse the verts
-        for vert_line in range(line_index, num_verts + line_index):
-            coords = [float(x) for x in lines[vert_line].split(" ")]
-            mesh_data[0].append(mn.Vector3(coords))
-        line_index += num_verts
-        for face_line in range(line_index, num_faces + line_index):
-            assert (
-                int(lines[face_line][0]) == 3
-            ), f"Faces must be triangles. '{ply_file}'"
-            indices = [int(x) for x in lines[face_line].split(" ")[1:]]
-            mesh_data[1].extend(indices)
+    manager = mn.trade.ImporterManager()
+    # TODO: replace AssimpImporter once a better importer can handle binary PLY or we use another format
+    importer = manager.load_and_instantiate("AssimpImporter")
+    importer.open_file(ply_file)
+
+    # NOTE: We don't support mesh merging or multi-mesh parsing currently
+    if importer.mesh_count > 1:
+        raise NotImplementedError("TODO: multi-mesh receptacle support.")
+
+    mesh_ix = 0
+    mesh = importer.mesh(mesh_ix)
+    assert (
+        mesh.primitive == mn.MeshPrimitive.TRIANGLES
+    ), "Must be a triangle mesh."
+
+    # zero-copy reference to importer datastructures
+    mesh_data: Tuple[List[mn.Vector3], List[int]] = (
+        mesh.attribute(mn.trade.MeshAttribute.POSITION),
+        mesh.indices,
+    )
 
     return mesh_data
 

From d1f49e45cdc1cdfc44740900f00c46173abec7a3 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Fri, 10 Feb 2023 13:26:08 -0800
Subject: [PATCH 16/27] use AnySceneImporter

---
 habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index 2644c3ac82..47abbce0e6 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -492,8 +492,7 @@ def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
     :param ply_file: The input PLY mesh file. NOTE: must contain only triangles.
     """
     manager = mn.trade.ImporterManager()
-    # TODO: replace AssimpImporter once a better importer can handle binary PLY or we use another format
-    importer = manager.load_and_instantiate("AssimpImporter")
+    importer = manager.load_and_instantiate("AnySceneImporter")
     importer.open_file(ply_file)
 
     # NOTE: We don't support mesh merging or multi-mesh parsing currently

From 9a0c7ad01194027c79d414ee7ae88df2ecba0a6f Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 13 Feb 2023 11:17:56 -0800
Subject: [PATCH 17/27] try fixing the plugin import issue

---
 test/test_rearrange_task.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 2d6df2fb79..37b589c929 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -4,14 +4,19 @@
 # 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 gc
 import itertools
 import json
 import os
 import os.path as osp
+import sys
 import time
 from glob import glob
 
+flags = sys.getdlopenflags()
+sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL)
+
 import magnum as mn
 import numpy as np
 import pytest

From b7a0a922f884aa7345f654c266aad7cc16bf2a88 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Fri, 17 Feb 2023 12:22:16 -0800
Subject: [PATCH 18/27] pre-commit

---
 test/test_rearrange_task.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index 7159da1caf..b242ba395d 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -187,6 +187,7 @@ def test_rearrange_episode_generator(
         f"successful_ep = {len(dataset.episodes)} generated in {time.time()-start_time} seconds."
     )
 
+
 @pytest.mark.skipif(
     not osp.exists("data/test_assets/"),
     reason="This test requires habitat-sim test assets.",

From f6b9a82d717270d307e3072a2f6a4242a558ffb6 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 7 Mar 2023 17:54:20 -0800
Subject: [PATCH 19/27] bugfix - use default_sensor_uid instead of hardcoded
 "rgb"

---
 .../habitat/sims/habitat_simulator/debug_visualizer.py    | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
index 1e19718118..160dada426 100644
--- a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
+++ b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
@@ -125,7 +125,9 @@ def save_observation(
         check_make_dir(output_path)
         from habitat_sim.utils import viz_utils as vut
 
-        image = vut.observation_to_image(obs_cache[0]["rgb"], "color")
+        image = vut.observation_to_image(
+            obs_cache[0][self.default_sensor_uuid], "color"
+        )
         from datetime import datetime
 
         # filename format "prefixmonth_day_year_hourminutesecondmicrosecond.png"
@@ -300,7 +302,9 @@ def _peek_object(
         from habitat_sim.utils import viz_utils as vut
 
         for ix, obs in enumerate(axis_obs):
-            image = vut.observation_to_image(obs["rgb"], "color")
+            image = vut.observation_to_image(
+                obs[self.default_sensor_uuid], "color"
+            )
             if stitched_image is None:
                 stitched_image = Image.new(
                     image.mode, (image.size[0] * 3, image.size[1] * 2)

From b3789392014ce0d08c4c27b1c7240afe00bf7f63 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Fri, 10 Mar 2023 08:14:26 -0800
Subject: [PATCH 20/27] adjust debug peek target to bb center to better capture
 objects not centered at COM

---
 habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py  | 2 +-
 habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index 47abbce0e6..15ea9585c9 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -520,7 +520,7 @@ def parse_receptacles_from_user_config(
     parent_template_directory: str = "",
     valid_link_names: Optional[List[str]] = None,
     ao_uniform_scaling: float = 1.0,
-) -> List[Union[Receptacle, AABBReceptacle]]:
+) -> List[Union[Receptacle, AABBReceptacle, TriangleMeshReceptacle]]:
     """
     Parse receptacle metadata from the provided user subconfig object.
 
diff --git a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
index 160dada426..60f2fe2f02 100644
--- a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
+++ b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
@@ -255,7 +255,7 @@ def _peek_object(
         :param show: If True, open and display the image immediately.
         """
         obj_abs_transform = obj.root_scene_node.absolute_transformation()
-        look_at = obj_abs_transform.translation
+        look_at = obj_abs_transform.transform_point(obj_bb.center())
         bb_size = obj_bb.size()
         # TODO: query fov and aspect from the camera spec
         fov = 90

From c0c4cb01a5899f68ad8f8389b9f9123ab3c75f58 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Fri, 10 Mar 2023 16:00:12 -0800
Subject: [PATCH 21/27] add debug circles to the peek API

---
 .../habitat_simulator/debug_visualizer.py     | 40 ++++++++++++++++++-
 1 file changed, 39 insertions(+), 1 deletion(-)

diff --git a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
index 60f2fe2f02..bc6f84292c 100644
--- a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
+++ b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
@@ -143,7 +143,7 @@ def render_debug_lines(
         debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
     ) -> None:
         """
-        Draw a set of debug lines with accomanying colors.
+        Draw a set of debug lines with accompanying colors.
 
         :param debug_lines: A set of debug line strips with accompanying colors. Each list entry contains a list of points and a color.
         """
@@ -160,6 +160,28 @@ def render_debug_lines(
                         color,
                     )
 
+    def render_debug_circles(
+        self,
+        debug_circles: Optional[
+            List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]]
+        ] = None,
+    ) -> None:
+        """
+        Draw a set of debug circles with accompanying colors.
+
+        :param debug_circles: A list of debug line render circle Tuples, each with (center, radius, normal, color).
+        """
+        # support None input to make useage easier elsewhere
+        if debug_circles is not None:
+            for center, radius, normal, color in debug_circles:
+                self.debug_line_render.draw_circle(
+                    translation=center,
+                    radius=radius,
+                    color=color,
+                    num_segments=12,
+                    normal=normal,
+                )
+
     def peek_rigid_object(
         self,
         obj: habitat_sim.physics.ManagedRigidObject,
@@ -167,6 +189,9 @@ def peek_rigid_object(
         peek_all_axis: bool = False,
         additional_savefile_prefix="",
         debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        debug_circles: Optional[
+            List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]]
+        ] = None,
         show: bool = False,
     ) -> str:
         """
@@ -179,6 +204,7 @@ def peek_rigid_object(
         :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
         :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
         :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param debug_circles: Optionally provide a list of debug line render circle Tuples, each with (center, radius, normal, color). These will be displayed in all peek images.
         :param show: If True, open and display the image immediately.
         """
 
@@ -189,6 +215,7 @@ def peek_rigid_object(
             peek_all_axis,
             additional_savefile_prefix,
             debug_lines,
+            debug_circles,
             show,
         )
 
@@ -199,6 +226,9 @@ def peek_articulated_object(
         peek_all_axis: bool = False,
         additional_savefile_prefix="",
         debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        debug_circles: Optional[
+            List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]]
+        ] = None,
         show: bool = False,
     ) -> str:
         """
@@ -211,6 +241,7 @@ def peek_articulated_object(
         :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
         :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
         :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param debug_circles: Optionally provide a list of debug line render circle Tuples, each with (center, radius, normal, color). These will be displayed in all peek images.
         :param show: If True, open and display the image immediately.
         """
         from habitat.sims.habitat_simulator.sim_utilities import (
@@ -226,6 +257,7 @@ def peek_articulated_object(
             peek_all_axis,
             additional_savefile_prefix,
             debug_lines,
+            debug_circles,
             show,
         )
 
@@ -240,6 +272,9 @@ def _peek_object(
         peek_all_axis: bool = False,
         additional_savefile_prefix="",
         debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        debug_circles: Optional[
+            List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]]
+        ] = None,
         show: bool = False,
     ) -> str:
         """
@@ -252,6 +287,7 @@ def _peek_object(
         :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
         :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
         :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param debug_circles: Optionally provide a list of debug line render circle Tuples, each with (center, radius, normal, color). These will be displayed in all peek images.
         :param show: If True, open and display the image immediately.
         """
         obj_abs_transform = obj.root_scene_node.absolute_transformation()
@@ -276,6 +312,7 @@ def _peek_object(
                 + look_at
             )
             self.render_debug_lines(debug_lines)
+            self.render_debug_circles(debug_circles)
             return self.save_observation(
                 prefix=additional_savefile_prefix + "peek_" + obj.handle,
                 look_at=look_at,
@@ -294,6 +331,7 @@ def _peek_object(
                 + look_at
             )
             self.render_debug_lines(debug_lines)
+            self.render_debug_circles(debug_circles)
             self.get_observation(look_at, look_from, axis_obs)
         # stitch images together
         stitched_image = None

From 51a4d14df3528d827d1dc55c8b7da5be775742ac Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Sat, 11 Mar 2023 10:59:28 -0800
Subject: [PATCH 22/27] address review comments

---
 .../datasets/rearrange/rearrange_generator.py | 46 +++++++-----
 .../datasets/rearrange/samplers/receptacle.py | 72 ++++++++++++-------
 habitat-lab/habitat/utils/geometry_utils.py   |  2 +-
 test/test_geom_utils.py                       |  6 +-
 test/test_rearrange_task.py                   |  6 +-
 5 files changed, 82 insertions(+), 50 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
index 7e5366ebe3..fa18cca1d2 100644
--- a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
@@ -400,7 +400,11 @@ def visualize_scene_receptacles(self) -> None:
         for receptacle in receptacles:
             logger.info("receptacle processing")
             receptacle.debug_draw(self.sim)
-            self.vdb.look_at(receptacle.sample_uniform_global(self.sim, 1.0))
+            # Receptacle does not have a position cached relative to the object it is attached to, so sample a position from it instead
+            sampled_look_target = receptacle.sample_uniform_global(
+                self.sim, 1.0
+            )
+            self.vdb.look_at(sampled_look_target)
             self.vdb.get_observation()
 
     def generate_episodes(
@@ -533,7 +537,7 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]:
             self.vdb.make_debug_video(prefix="receptacles_")
 
         # sample object placements
-        self.object_to_containing_receptacle = {}
+        self.object_to_containing_receptacle: Dict[str, Receptacle] = {}
         for sampler_name, obj_sampler in self._obj_samplers.items():
             object_sample_data = obj_sampler.sample(
                 self.sim,
@@ -562,13 +566,15 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]:
             )
             # debug visualization showing each newly added object
             if self._render_debug_obs:
-                logger.info(
+                logger.debug(
                     f"Generating debug images for {len(new_objects)} objects..."
                 )
                 for new_object in new_objects:
                     self.vdb.look_at(new_object.translation)
                     self.vdb.get_observation()
-                logger.info("   ... done")
+                logger.debug(
+                    f"... done generating the debug images for {len(new_objects)} objects."
+                )
 
         # simulate the world for a few seconds to validate the placements
         if not self.settle_sim():
@@ -821,10 +827,13 @@ def settle_sim(
             if self._render_debug_obs:
                 self.vdb.get_observation(obs_cache=settle_db_obs)
 
-        logger.info(f"   ...done in {time.time()-settle_start_time} seconds.")
+        logger.info(
+            f"   ...done with placement stability analysis in {time.time()-settle_start_time} seconds."
+        )
         # check stability of placements
-        logger.info("Computing placement stability report:")
-        logger.info("----------------------------------------")
+        logger.info(
+            "Computing placement stability report:\n----------------------------------------"
+        )
         max_settle_displacement = 0
         error_eps = 0.1
         unstable_placements: List[str] = []  # list of unstable object handles
@@ -870,17 +879,19 @@ def settle_sim(
         logger.info("  Detailed sampling stats:")
 
         # compute number of unstable objects for each receptacle
-        # receptacle: [num_objects, num_unstable_objects]
-        rec_num_obj_vs_unstable: Dict[Receptacle, List[int]] = {}
+        rec_num_obj_vs_unstable: Dict[Receptacle, Dict[str, int]] = {}
         for obj_name, rec in self.object_to_containing_receptacle.items():
             if rec not in rec_num_obj_vs_unstable:
-                rec_num_obj_vs_unstable[rec] = [0, 0]
-            rec_num_obj_vs_unstable[rec][0] += 1
+                rec_num_obj_vs_unstable[rec] = {
+                    "num_objects": 0,
+                    "num_unstable_objects": 0,
+                }
+            rec_num_obj_vs_unstable[rec]["num_objects"] += 1
             if obj_name in unstable_placements:
-                rec_num_obj_vs_unstable[rec][1] += 1
-        for rec, details in rec_num_obj_vs_unstable.items():
+                rec_num_obj_vs_unstable[rec]["num_unstable_objects"] += 1
+        for rec, obj_in_rec in rec_num_obj_vs_unstable.items():
             logger.info(
-                f"      receptacle '{rec.name}': ({details[1]}/{details[0]}) (unstable/total) objects."
+                f"      receptacle '{rec.name}': ({obj_in_rec['num_unstable_objects']}/{obj_in_rec['num_objects']}) (unstable/total) objects."
             )
 
         success = len(unstable_placements) == 0
@@ -899,10 +910,9 @@ def settle_sim(
                     if obj_name in obj_names
                 ]
                 # check that we have freedom to reject some objects
-                if (
-                    len(objects) - len(unstable_subset)
-                    >= sampler.num_objects[0]
-                ):
+                num_required_objects = sampler.num_objects[0]
+                num_stable_objects = len(objects) - len(unstable_subset)
+                if num_stable_objects >= num_required_objects:
                     # remove the unstable objects from datastructures
                     self.episode_data["sampled_objects"][sampler_name] = [
                         obj
diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
index 15ea9585c9..338968f330 100644
--- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
+++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py
@@ -7,9 +7,10 @@
 import os
 import random
 from abc import ABC, abstractmethod
+from collections import namedtuple
 from copy import deepcopy
 from dataclasses import dataclass
-from typing import Any, Dict, List, Optional, Tuple, Union
+from typing import Any, Dict, List, Optional, Union
 
 import magnum as mn
 import numpy as np
@@ -74,20 +75,24 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4:
         """
         Isolates boilerplate necessary to extract receptacle global transform of the Receptacle at the current state.
         """
+        # handle global parent
         if self.parent_object_handle is None:
             # global identify by default
             return mn.Matrix4.identity_init()
-        elif not self.is_parent_object_articulated:
+
+        # handle RigidObject parent
+        if not self.is_parent_object_articulated:
             obj_mgr = sim.get_rigid_object_manager()
             obj = obj_mgr.get_object_by_handle(self.parent_object_handle)
             # NOTE: we use absolute transformation from the 2nd visual node (scaling node) and root of all render assets to correctly account for any COM shifting, re-orienting, or scaling which has been applied.
             return obj.visual_scene_nodes[1].absolute_transformation()
-        else:
-            ao_mgr = sim.get_articulated_object_manager()
-            obj = ao_mgr.get_object_by_handle(self.parent_object_handle)
-            return obj.get_link_scene_node(
-                self.parent_link
-            ).absolute_transformation()
+
+        # handle ArticulatedObject parent
+        ao_mgr = sim.get_articulated_object_manager()
+        obj = ao_mgr.get_object_by_handle(self.parent_object_handle)
+        return obj.get_link_scene_node(
+            self.parent_link
+        ).absolute_transformation()
 
     def sample_uniform_global(
         self, sim: habitat_sim.Simulator, sample_region_scale: float
@@ -119,6 +124,7 @@ def debug_draw(
         :param sim: Simulator must be provided.
         :param color: Optionally provide wireframe color, otherwise magenta.
         """
+        raise NotImplementedError
 
 
 class OnTopOfReceptacle(Receptacle):
@@ -294,6 +300,22 @@ def debug_draw(
         # TODO: test this
 
 
+# TriangleMeshData "vertices":List[mn.Vector3] "indices":List[int]
+TriangleMeshData = namedtuple(
+    "TriangleMeshData",
+    "vertices indices",
+)
+
+
+def assert_triangles(indices: List[int]) -> None:
+    """
+    Assert that an index array is divisible by 3 as a heuristic for triangle-only faces.
+    """
+    assert (
+        len(indices) % 3 == 0
+    ), "TriangleMeshReceptacles must be exclusively composed of triangles. The provided mesh_data is not."
+
+
 class TriangleMeshReceptacle(Receptacle):
     """
     Defines a Receptacle surface as a triangle mesh.
@@ -303,7 +325,7 @@ class TriangleMeshReceptacle(Receptacle):
     def __init__(
         self,
         name: str,
-        mesh_data: Tuple[List[Any], List[Any]],  # vertices, indices
+        mesh_data: TriangleMeshData,  # vertices, indices
         parent_object_handle: str = None,
         parent_link: Optional[int] = None,
         up: Optional[mn.Vector3] = None,
@@ -322,11 +344,11 @@ def __init__(
         self.area_weighted_accumulator = (
             []
         )  # normalized float weights for each triangle for sampling
-        assert len(mesh_data[1]) % 3 == 0, "must be triangles"
+        assert_triangles(mesh_data.indices)
 
         # pre-compute the normalized cumulative area of all triangle faces for later sampling
         self.total_area = 0
-        for f_ix in range(int(len(mesh_data[1]) / 3)):
+        for f_ix in range(int(len(mesh_data.indices) / 3)):
             v = self.get_face_verts(f_ix)
             w1 = v[1] - v[0]
             w2 = v[2] - v[1]
@@ -353,7 +375,9 @@ def get_face_verts(self, f_ix: int) -> List[np.ndarray]:
         for ix in range(3):
             verts.append(
                 np.array(
-                    self.mesh_data[0][self.mesh_data[1][int(f_ix * 3 + ix)]]
+                    self.mesh_data.vertices[
+                        self.mesh_data.indices[int(f_ix * 3 + ix)]
+                    ]
                 )
             )
         return verts
@@ -372,7 +396,9 @@ def find_ge(a: List[Any], x) -> Any:
             i = bisect_left(a, x)
             if i != len(a):
                 return i
-            raise ValueError
+            raise ValueError(
+                f"Value '{x}' is greater than all items in the list. Maximum value should be <1."
+            )
 
         # first area weighted sampling of a triangle
         sample_val = random.random()
@@ -417,8 +443,8 @@ def debug_draw(
             color = mn.Color4.magenta()
         dblr = sim.get_debug_line_render()
         dblr.push_transform(self.get_global_transform(sim))
-        assert len(self.mesh_data[1]) % 3 == 0, "must be triangles"
-        for face in range(int(len(self.mesh_data[1]) / 3)):
+        assert_triangles(self.mesh_data.indices)
+        for face in range(int(len(self.mesh_data.indices) / 3)):
             verts = self.get_face_verts(f_ix=face)
             for edge in range(3):
                 dblr.draw_transformed_line(
@@ -451,9 +477,6 @@ def get_all_scenedataset_receptacles(
         stage_template = stm.get_template_by_handle(template_handle)
         for item in stage_template.get_user_config().get_subconfig_keys():
             if item.startswith("receptacle_"):
-                logger.info(
-                    f"template file_directory = {stage_template.file_directory}"
-                )
                 if template_handle not in receptacles["stage"]:
                     receptacles["stage"][template_handle] = []
                 receptacles["stage"][template_handle].append(item)
@@ -464,9 +487,6 @@ def get_all_scenedataset_receptacles(
         obj_template = rotm.get_template_by_handle(template_handle)
         for item in obj_template.get_user_config().get_subconfig_keys():
             if item.startswith("receptacle_"):
-                logger.info(
-                    f"template file_directory = {obj_template.file_directory}"
-                )
                 if template_handle not in receptacles["rigid"]:
                     receptacles["rigid"][template_handle] = []
                 receptacles["rigid"][template_handle].append(item)
@@ -485,7 +505,7 @@ def get_all_scenedataset_receptacles(
     return receptacles
 
 
-def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
+def import_tri_mesh_ply(ply_file: str) -> TriangleMeshData:
     """
     Returns a Tuple of (verts,indices) from a ply mesh using magnum trade importer.
 
@@ -495,9 +515,11 @@ def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
     importer = manager.load_and_instantiate("AnySceneImporter")
     importer.open_file(ply_file)
 
-    # NOTE: We don't support mesh merging or multi-mesh parsing currently
+    # TODO: We don't support mesh merging or multi-mesh parsing currently
     if importer.mesh_count > 1:
-        raise NotImplementedError("TODO: multi-mesh receptacle support.")
+        raise NotImplementedError(
+            "Importing multi-mesh receptacles (mesh merging or multi-mesh parsing) is not supported."
+        )
 
     mesh_ix = 0
     mesh = importer.mesh(mesh_ix)
@@ -506,7 +528,7 @@ def import_tri_mesh_ply(ply_file: str) -> Tuple[List[mn.Vector3], List[int]]:
     ), "Must be a triangle mesh."
 
     # zero-copy reference to importer datastructures
-    mesh_data: Tuple[List[mn.Vector3], List[int]] = (
+    mesh_data = TriangleMeshData(
         mesh.attribute(mn.trade.MeshAttribute.POSITION),
         mesh.indices,
     )
diff --git a/habitat-lab/habitat/utils/geometry_utils.py b/habitat-lab/habitat/utils/geometry_utils.py
index 00da19f4f0..a8dc5d903e 100644
--- a/habitat-lab/habitat/utils/geometry_utils.py
+++ b/habitat-lab/habitat/utils/geometry_utils.py
@@ -135,7 +135,7 @@ def random_triangle_point(
     return v0 + coef1 * (v1 - v0) + coef2 * (v2 - v0)
 
 
-def point_in_triangle_test(
+def is_point_in_triangle(
     p: np.ndarray, v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
 ) -> bool:
     """
diff --git a/test/test_geom_utils.py b/test/test_geom_utils.py
index 45b1f39c66..d8b2e9352e 100644
--- a/test/test_geom_utils.py
+++ b/test/test_geom_utils.py
@@ -7,7 +7,7 @@
 import numpy as np
 
 from habitat.utils.geometry_utils import (
-    point_in_triangle_test,
+    is_point_in_triangle,
     random_triangle_point,
 )
 
@@ -47,7 +47,7 @@ def test_point_in_triangle_test():
     ]
     for test_pair in test_pairs:
         assert (
-            point_in_triangle_test(
+            is_point_in_triangle(
                 test_pair[0], test_tri[0], test_tri[1], test_tri[2]
             )
             == test_pair[1]
@@ -63,7 +63,7 @@ def test_random_triangle_point():
         sample_centroid = np.zeros(3)
         for _samp in range(num_samples):
             tri_point = random_triangle_point(v[0], v[1], v[2])
-            assert point_in_triangle_test(tri_point, v[0], v[1], v[2])
+            assert is_point_in_triangle(tri_point, v[0], v[1], v[2])
             sample_centroid += tri_point
         # check uniformity of distribution by comparing sample centroid and triangle centroid
         sample_centroid /= num_samples
diff --git a/test/test_rearrange_task.py b/test/test_rearrange_task.py
index b242ba395d..5e30ede00d 100644
--- a/test/test_rearrange_task.py
+++ b/test/test_rearrange_task.py
@@ -33,7 +33,7 @@
 from habitat.core.logging import logger
 from habitat.datasets.rearrange.rearrange_dataset import RearrangeDatasetV0
 from habitat.tasks.rearrange.multi_task.composite_task import CompositeTask
-from habitat.utils.geometry_utils import point_in_triangle_test
+from habitat.utils.geometry_utils import is_point_in_triangle
 from habitat_baselines.config.default import get_config as baselines_get_config
 
 CFG_TEST = "benchmark/rearrange/pick.yaml"
@@ -350,10 +350,10 @@ def randomize_obj_state():
                         # check that the local point is within a mesh triangle
                         in_mesh = False
                         for f_ix in range(
-                            int(len(receptacle.mesh_data[1]) / 3)
+                            int(len(receptacle.mesh_data.indices) / 3)
                         ):
                             verts = receptacle.get_face_verts(f_ix)
-                            if point_in_triangle_test(
+                            if is_point_in_triangle(
                                 expected_local_sample_point,
                                 verts[0],
                                 verts[1],

From 13c8770344ea2753bf608903c141cd213d8bc48d Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Sat, 11 Mar 2023 11:09:54 -0800
Subject: [PATCH 23/27] refactor stability logging per review comments

---
 .../datasets/rearrange/rearrange_generator.py | 30 +++++++++++--------
 1 file changed, 17 insertions(+), 13 deletions(-)

diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
index fa18cca1d2..01eeef7734 100644
--- a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py
@@ -875,8 +875,10 @@ def settle_sim(
                 prefix="settle_", fps=30, obs_cache=settle_db_obs
             )
 
-        # detailed receptacle stability report
-        logger.info("  Detailed sampling stats:")
+        # collect detailed receptacle stability report log
+        detailed_receptacle_stability_report = (
+            "  Detailed receptacle stability analysis:"
+        )
 
         # compute number of unstable objects for each receptacle
         rec_num_obj_vs_unstable: Dict[Receptacle, Dict[str, int]] = {}
@@ -890,15 +892,15 @@ def settle_sim(
             if obj_name in unstable_placements:
                 rec_num_obj_vs_unstable[rec]["num_unstable_objects"] += 1
         for rec, obj_in_rec in rec_num_obj_vs_unstable.items():
-            logger.info(
-                f"      receptacle '{rec.name}': ({obj_in_rec['num_unstable_objects']}/{obj_in_rec['num_objects']}) (unstable/total) objects."
-            )
+            detailed_receptacle_stability_report += f"\n      receptacle '{rec.name}': ({obj_in_rec['num_unstable_objects']}/{obj_in_rec['num_objects']}) (unstable/total) objects."
 
         success = len(unstable_placements) == 0
 
         # optionally salvage the episode by removing unstable objects
         if self.cfg.correct_unstable_results and not success:
-            logger.info("  attempting to correct unstable placements...")
+            detailed_receptacle_stability_report += (
+                "\n  attempting to correct unstable placements..."
+            )
             for sampler_name, objects in self.episode_data[
                 "sampled_objects"
             ].items():
@@ -927,18 +929,20 @@ def settle_sim(
                         if obj.handle not in unstable_subset
                     ]
                 else:
-                    logger.info(
-                        f"  ... could not remove all unstable placements without violating minimum object sampler requirements for {sampler_name}"
+                    detailed_receptacle_stability_report += f"\n  ... could not remove all unstable placements without violating minimum object sampler requirements for {sampler_name}"
+                    detailed_receptacle_stability_report += (
+                        "\n----------------------------------------"
                     )
-                    logger.info("----------------------------------------")
+                    logger.info(detailed_receptacle_stability_report)
                     return False
-            logger.info(
-                f"  ... corrected unstable placements successfully. Final object count = {len(self.ep_sampled_objects)}"
-            )
+            detailed_receptacle_stability_report += f"\n  ... corrected unstable placements successfully. Final object count = {len(self.ep_sampled_objects)}"
             # we removed all unstable placements
             success = True
 
-        logger.info("----------------------------------------")
+        detailed_receptacle_stability_report += (
+            "\n----------------------------------------"
+        )
+        logger.info(detailed_receptacle_stability_report)
 
         # generate debug images of all final object placements
         if self._render_debug_obs and success:

From 1695371ddcfcb070296d479977ea6f8aed4a3aeb Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Mon, 13 Mar 2023 11:52:06 -0700
Subject: [PATCH 24/27] add peek_scene variant for easliy getting images of a
 full scene or stage.

---
 .../habitat_simulator/debug_visualizer.py     | 89 +++++++++++++++++--
 1 file changed, 83 insertions(+), 6 deletions(-)

diff --git a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
index bc6f84292c..1c665a8ad4 100644
--- a/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
+++ b/habitat-lab/habitat/sims/habitat_simulator/debug_visualizer.py
@@ -291,8 +291,85 @@ def _peek_object(
         :param show: If True, open and display the image immediately.
         """
         obj_abs_transform = obj.root_scene_node.absolute_transformation()
-        look_at = obj_abs_transform.transform_point(obj_bb.center())
-        bb_size = obj_bb.size()
+        return self._peek_bb(
+            bb_name=obj.handle,
+            bb=obj_bb,
+            world_transform=obj_abs_transform,
+            cam_local_pos=cam_local_pos,
+            peek_all_axis=peek_all_axis,
+            additional_savefile_prefix=additional_savefile_prefix,
+            debug_lines=debug_lines,
+            debug_circles=debug_circles,
+            show=show,
+        )
+
+    def peek_scene(
+        self,
+        cam_local_pos: Optional[mn.Vector3] = None,
+        peek_all_axis: bool = False,
+        additional_savefile_prefix="",
+        debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        debug_circles: Optional[
+            List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]]
+        ] = None,
+        show: bool = False,
+    ) -> str:
+        """
+        Helper function to generate image(s) of the scene for contextual debugging purposes.
+        Specialization to peek a scene. See _peek_bb.
+        Compute a camera placement to view the scene. Show/save an observation. Return the filepath.
+
+        :param cam_local_pos: Optionally provide a camera location in location local coordinates. Otherwise offset along local -Z axis from the object.
+        :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
+        :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
+        :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param debug_circles: Optionally provide a list of debug line render circle Tuples, each with (center, radius, normal, color). These will be displayed in all peek images.
+        :param show: If True, open and display the image immediately.
+        """
+        return self._peek_bb(
+            bb_name=self.sim.curr_scene_name,
+            bb=self.sim.get_active_scene_graph().get_root_node().cumulative_bb,
+            world_transform=mn.Matrix4.identity_init(),
+            cam_local_pos=cam_local_pos,
+            peek_all_axis=peek_all_axis,
+            additional_savefile_prefix=additional_savefile_prefix,
+            debug_lines=debug_lines,
+            debug_circles=debug_circles,
+            show=show,
+        )
+
+    def _peek_bb(
+        self,
+        bb_name: str,
+        bb: mn.Range3D,
+        world_transform: Optional[mn.Matrix4] = None,
+        cam_local_pos: Optional[mn.Vector3] = None,
+        peek_all_axis: bool = False,
+        additional_savefile_prefix="",
+        debug_lines: Optional[List[Tuple[List[mn.Vector3], mn.Color4]]] = None,
+        debug_circles: Optional[
+            List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]]
+        ] = None,
+        show: bool = False,
+    ) -> str:
+        """
+        Internal helper function to generate image(s) of any bb for contextual debugging purposes.
+        Compute a camera placement to view the bb. Show/save an observation. Return the filepath.
+
+        :param bb_name: The name of the entity we're peeking for filepath naming.
+        :param bb: The entity's bounding box (provided by consumer functions.)
+        :param world_transform: The entity's world transform provided by consumer functions, default identity.
+        :param cam_local_pos: Optionally provide a camera location in location local coordinates. Otherwise offset along local -Z axis from the object.
+        :param peek_all_axis: Optionally create a merged 3x2 matrix of images looking at the object from all angles.
+        :param additional_savefile_prefix: Optionally provide an additional prefix for the save filename to differentiate the images.
+        :param debug_lines: Optionally provide a list of debug line render tuples, each with a list of points and a color. These will be displayed in all peek images.
+        :param debug_circles: Optionally provide a list of debug line render circle Tuples, each with (center, radius, normal, color). These will be displayed in all peek images.
+        :param show: If True, open and display the image immediately.
+        """
+        if world_transform is None:
+            world_transform = mn.Matrix4.identity_init()
+        look_at = world_transform.transform_point(bb.center())
+        bb_size = bb.size()
         # TODO: query fov and aspect from the camera spec
         fov = 90
         aspect = 0.75
@@ -307,14 +384,14 @@ def _peek_object(
             cam_local_pos = mn.Vector3(0, 0, -1)
         if not peek_all_axis:
             look_from = (
-                obj_abs_transform.transform_vector(cam_local_pos).normalized()
+                world_transform.transform_vector(cam_local_pos).normalized()
                 * distance
                 + look_at
             )
             self.render_debug_lines(debug_lines)
             self.render_debug_circles(debug_circles)
             return self.save_observation(
-                prefix=additional_savefile_prefix + "peek_" + obj.handle,
+                prefix=additional_savefile_prefix + "peek_" + bb_name,
                 look_at=look_at,
                 look_from=look_from,
                 show=show,
@@ -326,7 +403,7 @@ def _peek_object(
             axis_vec = mn.Vector3()
             axis_vec[axis % 3] = 1 if axis // 3 == 0 else -1
             look_from = (
-                obj_abs_transform.transform_vector(axis_vec).normalized()
+                world_transform.transform_vector(axis_vec).normalized()
                 * distance
                 + look_at
             )
@@ -356,7 +433,7 @@ def _peek_object(
             stitched_image.show()
         save_path = os.path.join(
             self.output_path,
-            additional_savefile_prefix + "peek_6x_" + obj.handle + ".png",
+            additional_savefile_prefix + "peek_6x_" + bb_name + ".png",
         )
         stitched_image.save(save_path)
         return save_path

From 226a8292d16ffa398015c0c1c3e43bd9df33e025 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 14 Mar 2023 15:49:07 -0700
Subject: [PATCH 25/27] revert testing on sim branch back to main

---
 .circleci/config.yml | 52 ++++++++++++++++----------------------------
 1 file changed, 19 insertions(+), 33 deletions(-)

diff --git a/.circleci/config.yml b/.circleci/config.yml
index ebaae7800e..067148127b 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -1,9 +1,7 @@
-
-
 version: 2.1
 gpu: &gpu
   machine:
-    image: ubuntu-1604-cuda-10.1:201909-23
+    image: ubuntu-2004-cuda-11.4:202110-01
   resource_class: gpu.nvidia.medium
   environment:
     FPS_THRESHOLD: 900
@@ -101,16 +99,18 @@ jobs:
                   curl \
                   vim \
                   ca-certificates \
-                  libbullet-dev \
                   libjpeg-dev \
                   libglm-dev \
                   libegl1-mesa-dev \
+                  ninja-build \
                   xorg-dev \
                   freeglut3-dev \
                   pkg-config \
                   wget \
                   zip \
+                  lcov\
                   libhdf5-dev \
+                  libomp-dev \
                   unzip || true
               sudo apt install --allow-change-held-packages \
                   texlive-base \
@@ -121,13 +121,7 @@ jobs:
           name: Check CUDA
           no_output_timeout: 20m
           background: true
-          command: |
-              # wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu1604/x86_64/cuda-repo-ubuntu1604_8.0.44-1_amd64.deb
-              # sudo dpkg -i cuda-repo-ubuntu1604_8.0.44-1_amd64.deb
-              # sudo apt-get update || true
-              # sudo apt-get --yes --force-yes install cuda
-              # touch ./cuda_installed
-              nvidia-smi
+          command: nvidia-smi
       # Restore Conda cache
       - restore_cache:
           keys:
@@ -145,8 +139,7 @@ jobs:
                 export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
                 conda create -y -n habitat python=3.7
                 . activate habitat
-                pip install -U pip
-                conda install -q -y -c conda-forge ninja ccache numpy
+                conda install -q -y -c conda-forge ninja ccache
                 pip install pytest-sugar>=0.9.6 mock cython pygame flaky pytest pytest-mock pytest-cov psutil
               fi
       - run:
@@ -158,14 +151,16 @@ jobs:
               . activate habitat;
               if [ ! -f ~/miniconda/pytorch_installed ]
               then
+                # For whatever reason we have to install pytorch first. If it isn't
+                # it installs the 1.4 cpuonly version. Which is no good.
                 echo "Installing pytorch"
-                conda install -y pytorch==1.4.0 torchvision cudatoolkit=10.0 -c pytorch
+                conda install -y pytorch==1.12.1=py3.7_cuda11.3_cudnn8.3.2_0 torchvision==0.13.1=py37_cu113 cudatoolkit=11.3 -c pytorch -c nvidia
               fi
               touch ~/miniconda/pytorch_installed
               python -c 'import torch; print("Has cuda ? ",torch.cuda.is_available()); print("torch version : ",torch.__version__);'
       - restore_cache:
           keys:
-            - habitat-sim-{{ checksum "./hsim_sha" }}
+            - v1-habitat-sim-{{ checksum "./hsim_sha" }}
       - restore_cache:
           keys:
             - ccache-{{ arch }}-main
@@ -187,14 +182,11 @@ jobs:
               then
                 git clone https://github.com/facebookresearch/habitat-sim.git --recursive
               fi
-              # while [ ! -f ./cuda_installed ]; do sleep 2; done # wait for CUDA
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat;
               cd habitat-sim
-              git fetch --all
-              git checkout receptacle-test-assets
               pip install -r requirements.txt --progress-bar off
-              pip install pillow
+              pip install imageio imageio-ffmpeg
               python -u setup.py install --headless --with-cuda --bullet
       - run:
           name: Ccache stats
@@ -222,7 +214,7 @@ jobs:
               fi
               if [ ! -f ./habitat-sim/data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf ]
               then
-                python -m habitat_sim.utils.datasets_download --uids hab_spot_arm --data-path habitat-sim/data/
+                python -m habitat_sim.utils.datasets_download --uids hab_spot_arm --data-path habitat-sim/data/ --replace
               fi
               if [ ! -f ./habitat-sim/data/robots/hab_stretch/urdf/hab_stretch.urdf ]
               then
@@ -241,12 +233,11 @@ jobs:
       - run:
           name: Run sim benchmark
           command: |
-              # while [ ! -f ./cuda_installed ]; do sleep 2; done # wait for CUDA
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat; cd habitat-sim
               python examples/example.py --scene data/scene_datasets/habitat-test-scenes/van-gogh-room.glb --silent --test_fps_regression $FPS_THRESHOLD
       - save_cache:
-          key: habitat-sim-{{ checksum "./hsim_sha" }}
+          key: v1-habitat-sim-{{ checksum "./hsim_sha" }}
           background: true
           paths:
             - ./habitat-sim
@@ -256,15 +247,15 @@ jobs:
           paths:
             - /home/circleci/.ccache
       - run:
-          name: Install api
+          name: Install api with baselines
           no_output_timeout: 20m
           command: |
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat; cd habitat-lab
-              while [ ! -f ~/miniconda/pytorch_installed ]; do sleep 2; done # wait for Pytorch
               ln -s ../habitat-sim/data data
-              pip install -r habitat-lab/requirements.txt --progress-bar off
-              touch ~/miniconda/pip_deps_installed
+              while [ ! -f ~/miniconda/pytorch_installed ]; do sleep 2; done # wait for Pytorch
+              pip install -e habitat-lab
+              pip install -e habitat-baselines
       - save_cache:
           key: conda-{{ checksum "habitat-lab/.circleci/config.yml" }}
           background: true
@@ -274,11 +265,8 @@ jobs:
           name: Run api tests
           no_output_timeout: 120m
           command: |
-
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat; cd habitat-lab
-              pip install -e habitat-lab
-              pip install -e habitat-baselines
               export PYTHONPATH=.:$PYTHONPATH
               export MULTI_PROC_OFFSET=0 && export MAGNUM_LOG=quiet && export HABITAT_SIM_LOG=quiet
               python -m pytest --cov-report=xml --cov-report term  --cov=./
@@ -289,8 +277,6 @@ jobs:
           command: |
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat; cd habitat-lab
-              pip install -e habitat-lab
-              pip install -e habitat-baselines
               export PYTHONPATH=.:$PYTHONPATH
               export MULTI_PROC_OFFSET=0 && export MAGNUM_LOG=quiet && export HABITAT_SIM_LOG=quiet
               # This is a flag that enables test_test_baseline_training to work
@@ -307,13 +293,13 @@ jobs:
               cp data/hab2_bench_assets/bench_scene.json.gz data/ep_datasets/
               bash scripts/hab2_bench/bench_runner.sh
               python scripts/hab2_bench/plot_bench.py
+              # Assert the SPS number are up to standard
+              python scripts/hab2_bench/assert_bench.py
       - run:
           name: Build api documentation
           command: |
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat; cd habitat-lab
-              pip install -e habitat-lab
-              pip install -e habitat-baselines
               # Download sim inventory for crosslinking (no need to build
               # the whole sim docs for that)
               # TODO: take it from github.com/facebookmicrosites/habitat-website

From 8d34540b6ef4721048956baf0c783fdb580b9b62 Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Tue, 14 Mar 2023 15:54:51 -0700
Subject: [PATCH 26/27] Add a note and todo about stability culling flag

---
 habitat-lab/habitat/datasets/rearrange/run_episode_generator.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py b/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py
index 1a62703404..3b8c637169 100644
--- a/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py
+++ b/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py
@@ -51,6 +51,7 @@ class RearrangeEpisodeGeneratorConfig:
         default_factory=lambda: ["data/objects/ycb/"]
     )
     # optionally correct unstable states by removing extra unstable objects (within minimum samples limitations)
+    # TODO: This option is off by default for backwards compatibility and because it does not yet work with target sampling.
     correct_unstable_results: bool = False
     # ----- resource set definitions ------
     # Define the sets of scenes, objects, and receptacles which can be sampled from.

From 1c9fb5d95b6ba639c015bd18ac4a186e3f04d9b7 Mon Sep 17 00:00:00 2001
From: Alexander Clegg <alexanderwclegg@gmail.com>
Date: Tue, 14 Mar 2023 15:57:15 -0700
Subject: [PATCH 27/27] Remove unused config file

---
 .../configs/all_receptacles_test.yaml         | 54 -------------------
 1 file changed, 54 deletions(-)
 delete mode 100644 habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml

diff --git a/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml b/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
deleted file mode 100644
index fd2406cdca..0000000000
--- a/habitat-lab/habitat/datasets/rearrange/configs/all_receptacles_test.yaml
+++ /dev/null
@@ -1,54 +0,0 @@
----
-## This config is meant to test receptacle configurations and clutter generation for new datasets.
-## All receptacles used with a small batch of known YCB objects.
-
-# Define your own dataset path,
-dataset_path: "data/replica_cad/replicaCAD.scene_dataset_config.json"
-additional_object_paths:
-  - "data/objects/ycb/configs/"
-correct_unstable_results: True
-scene_sets:
-  -
-    name: "all_scenes"
-    included_substrings:
-      - ""
-    excluded_substrings: ["NONE"]
-
-object_sets:
-  -
-    name: "simple_objects"
-    included_substrings:
-      - "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"
-    excluded_substrings: []
-receptacle_sets:
-  -
-    name: "all_receptacles"
-    included_object_substrings:
-      - ""
-    excluded_object_substrings: []
-    included_receptacle_substrings:
-      - ""
-    excluded_receptacle_substrings: []
-
-scene_sampler:
-  type: "subset"
-  params:
-    scene_sets: ["all_scenes"]
-
-object_samplers:
-  -
-    name: "simple_objects_sample"
-    type: "uniform"
-    params:
-      object_sets: ["simple_objects"]
-      receptacle_sets: ["all_receptacles"]
-      num_samples: [10, 200]
-      orientation_sampling: "up"