From e99cce4eff5fa8f135132ffb8c33839b789ee1b3 Mon Sep 17 00:00:00 2001 From: Alexander Clegg Date: Wed, 15 Mar 2023 09:36:18 -0700 Subject: [PATCH] TriangleMeshReceptacles and RearrangeGenerator Improvements (#1108) * updates to rearrange episode generation to support mesh receptacles and improved debugging * docstrings and typing for DebugVisualizer util * mesh receptacle debug_draw with global transforms * add some tests for sampling accuracy * bugfix - use default_sensor_uid instead of hardcoded "rgb" in DebugVisualizer * adjust debug peek target to bb center to better capture objects not centered at COM * add debug circles to the peek API * add peek_scene variant for easliy getting images of a full scene or stage. --- .../datasets/rearrange/rearrange_generator.py | 153 ++++++-- .../rearrange/run_episode_generator.py | 3 + .../rearrange/samplers/object_sampler.py | 13 + .../datasets/rearrange/samplers/receptacle.py | 360 ++++++++++++++++-- .../habitat_simulator/debug_visualizer.py | 314 ++++++++++++--- habitat-lab/habitat/utils/geometry_utils.py | 51 +++ test/test_geom_utils.py | 73 ++++ test/test_rearrange_task.py | 188 +++++++++ 8 files changed, 1046 insertions(+), 109 deletions(-) create mode 100644 test/test_geom_utils.py diff --git a/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py b/habitat-lab/habitat/datasets/rearrange/rearrange_generator.py index 853a61ffa0..01eeef7734 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,19 @@ 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) + # 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( self, num_episodes: int = 1, verbose: bool = False @@ -545,7 +537,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: Dict[str, Receptacle] = {} for sampler_name, obj_sampler in self._obj_samplers.items(): object_sample_data = obj_sampler.sample( self.sim, @@ -558,7 +550,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 +566,15 @@ def generate_single_episode(self) -> Optional[RearrangeEpisode]: ) # debug visualization showing each newly added object if self._render_debug_obs: + 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.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(): @@ -613,7 +611,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 +692,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 +785,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 +801,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 +827,16 @@ def settle_sim( if self._render_debug_obs: self.vdb.get_observation(obs_cache=settle_db_obs) + 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( + "Computing placement stability report:\n----------------------------------------" + ) 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 +847,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 +875,79 @@ def settle_sim( prefix="settle_", fps=30, obs_cache=settle_db_obs ) + # 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]] = {} + 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] = { + "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]["num_unstable_objects"] += 1 + for rec, obj_in_rec in rec_num_obj_vs_unstable.items(): + 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: + detailed_receptacle_stability_report += ( + "\n 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 + 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 + 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: + 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(detailed_receptacle_stability_report) + return False + 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 + + 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: + 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..3b8c637169 100644 --- a/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py +++ b/habitat-lab/habitat/datasets/rearrange/run_episode_generator.py @@ -50,6 +50,9 @@ 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) + # 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. # 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..338968f330 100644 --- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py +++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py @@ -4,16 +4,21 @@ # 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 collections import namedtuple from copy import deepcopy from dataclasses import dataclass -from typing import Dict, List, Optional, Union +from typing import Any, Dict, List, Optional, 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 +from habitat.utils.geometry_utils import random_triangle_point class Receptacle(ABC): @@ -66,11 +71,28 @@ 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. """ + # handle global parent + if self.parent_object_handle is None: + # global identify by default + return mn.Matrix4.identity_init() + + # 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() + + # 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 @@ -91,6 +113,19 @@ def add_receptacle_visualization( """ return [] + @abstractmethod + 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 color: Optionally provide wireframe color, otherwise magenta. + """ + raise NotImplementedError + class OnTopOfReceptacle(Receptacle): def __init__(self, name: str, places: List[str]): @@ -112,6 +147,18 @@ 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: + """ + 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 color: Optionally provide wireframe color, otherwise magenta. + """ + # TODO: + class AABBReceptacle(Receptacle): """ @@ -161,6 +208,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 +240,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(sim) def add_receptacle_visualization( self, sim: habitat_sim.Simulator @@ -241,13 +280,189 @@ def add_receptacle_visualization( ) return [box_obj] + 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 color: Optionally provide wireframe color, otherwise magenta. + """ + # 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 + + +# 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. + TODO: configurable maximum height. + """ + + def __init__( + self, + name: str, + mesh_data: TriangleMeshData, # vertices, indices + parent_object_handle: str = None, + parent_link: Optional[int] = None, + 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 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 + self.area_weighted_accumulator = ( + [] + ) # normalized float weights for each triangle for sampling + 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.indices) / 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] + + 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.ndarray] = [] + for ix in range(3): + verts.append( + np.array( + self.mesh_data.vertices[ + self.mesh_data.indices[int(f_ix * 3 + ix)] + ] + ) + ) + return verts + + 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: List[Any], x) -> Any: + "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( + 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() + 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 + v = self.get_face_verts(f_ix=tri_index) + rand_point = random_triangle_point(v[0], v[1], v[2]) + + return rand_point + + 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 color: Optionally provide wireframe color, otherwise magenta. + """ + # draw all mesh triangles + if color is None: + color = mn.Color4.magenta() + dblr = sim.get_debug_line_render() + dblr.push_transform(self.get_global_transform(sim)) + 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( + verts[edge], verts[(edge + 1) % 3], color + ) + dblr.pop_transform() -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]]] = { @@ -290,12 +505,44 @@ def get_all_scenedataset_receptacles(sim) -> Dict[str, Dict[str, List[str]]]: return receptacles +def import_tri_mesh_ply(ply_file: str) -> TriangleMeshData: + """ + 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. + """ + manager = mn.trade.ImporterManager() + importer = manager.load_and_instantiate("AnySceneImporter") + importer.open_file(ply_file) + + # TODO: We don't support mesh merging or multi-mesh parsing currently + if importer.mesh_count > 1: + raise NotImplementedError( + "Importing multi-mesh receptacles (mesh merging or multi-mesh parsing) is not supported." + ) + + 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 = TriangleMeshData( + mesh.attribute(mn.trade.MeshAttribute.POSITION), + mesh.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]]: +) -> List[Union[Receptacle, AABBReceptacle, TriangleMeshReceptacle]]: """ Parse receptacle metadata from the provided user subconfig object. @@ -307,11 +554,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,60 +617,98 @@ 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 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() 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..1c665a8ad4 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 @@ -29,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 @@ -45,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 = ( @@ -75,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) @@ -93,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: "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) @@ -104,26 +125,87 @@ 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" 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, + ) -> None: + """ + 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. + """ + # 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 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, 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: """ - 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 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_object( @@ -131,6 +213,10 @@ def peek_rigid_object( obj.root_scene_node.cumulative_bb, cam_local_pos, peek_all_axis, + additional_savefile_prefix, + debug_lines, + debug_circles, + show, ) def peek_articulated_object( @@ -138,10 +224,25 @@ 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, + debug_circles: Optional[ + List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]] + ] = None, + 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 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 ( get_ao_global_bb, @@ -149,7 +250,16 @@ 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, + debug_circles, + show, + ) def _peek_object( self, @@ -160,15 +270,106 @@ 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, + debug_circles: Optional[ + List[Tuple[mn.Vector3, float, mn.Vector3, mn.Color4]] + ] = None, + show: bool = False, ) -> str: """ - Compute a camera placement to view an ArticulatedObject 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. + 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 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() - look_at = obj_abs_transform.translation - 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 @@ -183,46 +384,59 @@ 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="peek_" + obj.handle, + prefix=additional_savefile_prefix + "peek_" + bb_name, 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 - - 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), + + # 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 = ( + world_transform.transform_vector(axis_vec).normalized() + * distance + + 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 PIL import Image + + from habitat_sim.utils import viz_utils as vut + + for ix, obs in enumerate(axis_obs): + 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) ) - 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_" + bb_name + ".png", + ) + stitched_image.save(save_path) + return save_path def make_debug_video( self, @@ -232,8 +446,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: "" + :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 @@ -249,7 +469,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 diff --git a/habitat-lab/habitat/utils/geometry_utils.py b/habitat-lab/habitat/utils/geometry_utils.py index 0fd70ba423..a8dc5d903e 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 is_point_in_triangle( + 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..d8b2e9352e --- /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 ( + is_point_in_triangle, + 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 ( + is_point_in_triangle( + 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 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 + 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 0a8af4f067..7b370ff81d 100644 --- a/test/test_rearrange_task.py +++ b/test/test_rearrange_task.py @@ -4,26 +4,36 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. +import ctypes import json import 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 import yaml from omegaconf import DictConfig, OmegaConf 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 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 is_point_in_triangle from habitat_baselines.config.default import get_config as baselines_get_config CFG_TEST = "benchmark/rearrange/pick.yaml" @@ -186,3 +196,181 @@ def test_rearrange_episode_generator( logger.info( 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.", +) +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["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 + ) + ) + + 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 + + # 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: + # 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 + ) + 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: + # 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: + 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 + + 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.indices) / 3) + ): + verts = receptacle.get_face_verts(f_ix) + if is_point_in_triangle( + 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."